diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions index 5e1035de20c64..4119e69ed72e1 100644 --- a/.cppcheck_suppressions +++ b/.cppcheck_suppressions @@ -1,4 +1,6 @@ *:*/test/* +*:*/examples/* +*:*/benchmarks/* checkersReport missingInclude diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 7403da173eb13..4cff38cfeb145 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,7 @@ ### Automatically generated from package.xml ### common/autoware_adapi_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_component_interface_specs/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp +common/autoware_component_interface_specs_universe/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/autoware_component_interface_tools/** isamu.takagi@tier4.jp common/autoware_component_interface_utils/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/autoware_fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -10,17 +10,13 @@ common/autoware_glog_component/** takamasa.horibe@tier4.jp common/autoware_goal_distance_calculator/** taiki.tanaka@tier4.jp common/autoware_grid_map_utils/** maxime.clement@tier4.jp common/autoware_interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp -common/autoware_kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp common/autoware_motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/autoware_object_recognition_utils/** shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yoshi.ri@tier4.jp -common/autoware_osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/autoware_path_distance_calculator/** isamu.takagi@tier4.jp -common/autoware_point_types/** david.wong@tier4.jp max.schmeller@tier4.jp common/autoware_polar_grid/** yukihiro.saito@tier4.jp common/autoware_pyplot/** mamoru.sobue@tier4.jp yukinari.hisaki.2@tier4.jp -common/autoware_qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/autoware_signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp +common/autoware_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp yukinari.hisaki.2@tier4.jp zulfaqar.azmi@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp @@ -31,6 +27,7 @@ common/autoware_vehicle_info_utils/** mamoru.sobue@tier4.jp shumpei.wakabayashi@ common/tier4_api_utils/** isamu.takagi@tier4.jp control/autoware_autonomous_emergency_braking/** alqudah.mohammad@tier4.jp daniel.sanchez@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/autoware_collision_detector/** go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp tomohito.ando@tier4.jp +control/autoware_control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp junya.sasaki@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_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 @@ -39,20 +36,19 @@ control/autoware_mpc_lateral_controller/** kyoichi.sugahara@tier4.jp takamasa.ho control/autoware_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/autoware_operation_mode_transition_manager/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/autoware_pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp +control/autoware_predicted_path_checker/** berkay@leodrive.ai junya.sasaki@tier4.jp control/autoware_pure_pursuit/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_shift_decider/** 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_trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/autoware_trajectory_follower_node/** 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/predicted_path_checker/** berkay@leodrive.ai evaluator/autoware_control_evaluator/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp temkei.kem@tier4.jp +evaluator/autoware_kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp junya.sasaki@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +evaluator/autoware_localization_evaluator/** anh.nguyen.2@tier4.jp dominik.jargot@robotec.ai junya.sasaki@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +evaluator/autoware_perception_online_evaluator/** fumiya.watanabe@tier4.jp junya.sasaki@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp evaluator/autoware_planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp temkei.kem@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 -evaluator/localization_evaluator/** anh.nguyen.2@tier4.jp dominik.jargot@robotec.ai koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp -evaluator/perception_online_evaluator/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp -evaluator/scenario_simulator_v2_adapter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp +evaluator/autoware_scenario_simulator_v2_adapter/** junya.sasaki@tier4.jp kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp 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 masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp @@ -70,7 +66,6 @@ localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/* localization/autoware_landmark_based_localizer/autoware_landmark_manager/** 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/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/** shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/autoware_localization_error_monitor/** 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/autoware_localization_util/** 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/autoware_ndt_scan_matcher/** 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/autoware_pose2twist/** 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/autoware_pose_covariance_modifier/** melike@leodrive.ai @@ -134,20 +129,20 @@ planning/autoware_costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@ 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_mission_planner_universe/** 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_obstacle_cruise_planner/** berkay@leodrive.ai kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/autoware_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/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_factor_interface/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp +planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp mamoru.sobue@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_route_handler/** alqudah.mohammad@tier4.jp 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_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/autoware_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -155,7 +150,7 @@ planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/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/behavior_path_planner/autoware_behavior_path_lane_change_module/** alqudah.mohammad@tier4.jp fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp maxime.clement@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_planner/autoware_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/autoware_behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@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/autoware_behavior_path_planner_common/** alqudah.mohammad@tier4.jp 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_planner/autoware_behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp planning/behavior_path_planner/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 @@ -170,6 +165,7 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_m planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/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/behavior_velocity_planner/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/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/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_planner/autoware_behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yukinari.hisaki.2@tier4.jp zhe.shen@tier4.jp @@ -180,8 +176,8 @@ planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/** planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** alqudah.mohammad@tier4.jp mamoru.sobue@tier4.jp 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/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp -planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/** alqudah.mohammad@tier4.jp maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/** alqudah.mohammad@tier4.jp maxime.clement@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 @@ -200,46 +196,49 @@ sensing/autoware_radar_tracks_noise_filter/** satoshi.tanaka@tier4.jp shunsuke.m sensing/autoware_vehicle_velocity_converter/** ryu.yamamoto@tier4.jp sensing/livox/autoware_livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp simulator/autoware_carla_interface/** maxime.clement@tier4.jp mradityagio@gmail.com -simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp -simulator/fault_injection/** keisuke.shima@tier4.jp -simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp -simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +simulator/autoware_dummy_perception_publisher/** junya.sasaki@tier4.jp yukihiro.saito@tier4.jp +simulator/autoware_fault_injection/** junya.sasaki@tier4.jp keisuke.shima@tier4.jp +simulator/autoware_learning_based_vehicle_model/** junya.sasaki@tier4.jp maxime.clement@tier4.jp nagy.tomas@tier4.jp +simulator/autoware_simple_planning_simulator/** junya.sasaki@tier4.jp kotaro.yoshimoto@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +simulator/autoware_vehicle_door_simulator/** isamu.takagi@tier4.jp junya.sasaki@tier4.jp simulator/tier4_dummy_object_rviz_plugin/** yukihiro.saito@tier4.jp -simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp +system/autoware_bluetooth_monitor/** fumihito.ito@tier4.jp junya.sasaki@tier4.jp system/autoware_component_monitor/** baris@leodrive.ai memin@leodrive.ai yavuz@leodrive.ai system/autoware_default_adapi/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/autoware_default_adapi_helpers/autoware_adapi_adaptors/** isamu.takagi@tier4.jp junya.sasaki@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/autoware_default_adapi_helpers/autoware_adapi_visualizers/** isamu.takagi@tier4.jp junya.sasaki@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/** isamu.takagi@tier4.jp junya.sasaki@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/autoware_diagnostic_graph_utils/** isamu.takagi@tier4.jp junya.sasaki@tier4.jp +system/autoware_dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp +system/autoware_dummy_infrastructure/** junya.sasaki@tier4.jp ryohsuke.mitsudome@tier4.jp +system/autoware_duplicated_node_checker/** junya.sasaki@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp +system/autoware_hazard_status_converter/** isamu.takagi@tier4.jp junya.sasaki@tier4.jp +system/autoware_mrm_comfortable_stop_operator/** junya.sasaki@tier4.jp makoto.kurihara@tier4.jp tomohito.ando@tier4.jp +system/autoware_mrm_emergency_stop_operator/** junya.sasaki@tier4.jp makoto.kurihara@tier4.jp tomohito.ando@tier4.jp +system/autoware_mrm_handler/** junya.sasaki@tier4.jp makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/autoware_processing_time_checker/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp -system/bluetooth_monitor/** fumihito.ito@tier4.jp +system/autoware_system_monitor/** fumihito.ito@tier4.jp junya.sasaki@tier4.jp tetsuhiro.kawaguchi@tier4.jp +system/autoware_topic_relay_controller/** makoto.kurihara@tier4.jp tetsuhiro.kawaguchi@tier4.jp +system/autoware_velodyne_monitor/** fumihito.ito@tier4.jp junya.sasaki@tier4.jp system/component_state_monitor/** isamu.takagi@tier4.jp -system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/diagnostic_graph_aggregator/** isamu.takagi@tier4.jp -system/diagnostic_graph_utils/** isamu.takagi@tier4.jp -system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp -system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp -system/duplicated_node_checker/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp -system/hazard_status_converter/** isamu.takagi@tier4.jp -system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp -system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp -system/mrm_handler/** makoto.kurihara@tier4.jp ryuta.kambe@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/system_diagnostic_monitor/** isamu.takagi@tier4.jp -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/autoware_accel_brake_map_calibrator/** eiki.nagata.2@tier4.jp 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/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp sho.iwasawa.2@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp +visualization/autoware_bag_time_manager_rviz_plugin/** junya.sasaki@tier4.jp taiki.tanaka@tier4.jp visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai +visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/** satoshi.ota@tier4.jp takayuki.murooka@tier4.jp visualization/autoware_perception_rviz_plugin/** opensource@apex.ai shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp -visualization/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp visualization/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp visualization/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp visualization/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp visualization/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp yamato.ando@tier4.jp +visualization/tier4_planning_factor_rviz_plugin/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp visualization/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp visualization/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp khalil@leodrive.ai visualization/tier4_system_rviz_plugin/** koji.minoda@tier4.jp diff --git a/.github/actions/build-and-test-differential/action.yaml b/.github/actions/build-and-test-differential/action.yaml deleted file mode 100644 index 89893e9f55fb5..0000000000000 --- a/.github/actions/build-and-test-differential/action.yaml +++ /dev/null @@ -1,111 +0,0 @@ -name: build-and-test-differential -description: "" - -inputs: - rosdistro: - description: "" - required: true - container: - description: "" - required: true - container-suffix: - description: "" - required: true - runner: - description: "" - required: true - build-depends-repos: - description: "" - required: true - build-pre-command: - description: "" - required: true - codecov-token: - description: "" - required: true - -runs: - using: composite - steps: - - name: Show disk space before the tasks - run: df -h - shell: bash - - - name: Show machine specs - run: lscpu && free -h - shell: bash - - - 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: Create ccache directory - run: | - mkdir -p ${CCACHE_DIR} - du -sh ${CCACHE_DIR} && ccache -s - shell: bash - - - name: Attempt to restore ccache - uses: actions/cache/restore@v4 - with: - path: | - /root/.ccache - key: ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-${{ github.event.pull_request.base.sha }} - restore-keys: | - ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}- - - - name: Show ccache stats before build and reset stats - run: | - du -sh ${CCACHE_DIR} && ccache -s - ccache --zero-stats - shell: bash - - - name: Export CUDA state as a variable for adding to cache key - run: | - build_type_cuda_state=nocuda - if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then - build_type_cuda_state=cuda - fi - echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" - echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" - shell: bash - - - name: Build - if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-build@v1 - with: - rosdistro: ${{ inputs.rosdistro }} - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - build-depends-repos: ${{ inputs.build-depends-repos }} - cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} - build-pre-command: ${{ inputs.build-pre-command }} - - - name: Show ccache stats after build - run: du -sh ${CCACHE_DIR} && ccache -s - shell: bash - - - name: Test - id: test - if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} - uses: autowarefoundation/autoware-github-actions/colcon-test@v1 - with: - rosdistro: ${{ inputs.rosdistro }} - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - build-depends-repos: ${{ inputs.build-depends-repos }} - - - name: Upload coverage to CodeCov - if: ${{ steps.test.outputs.coverage-report-files != '' }} - uses: codecov/codecov-action@v4 - with: - files: ${{ steps.test.outputs.coverage-report-files }} - fail_ci_if_error: false - verbose: true - flags: differential - token: ${{ inputs.codecov-token }} - - - name: Show disk space after the tasks - run: df -h - shell: bash diff --git a/.github/dependabot.yaml b/.github/dependabot.yaml index 8fd9b7f4ae0a5..8e2d7193aed5d 100644 --- a/.github/dependabot.yaml +++ b/.github/dependabot.yaml @@ -6,8 +6,9 @@ version: 2 updates: - package-ecosystem: github-actions directory: / + # https://docs.github.com/en/code-security/dependabot/dependabot-version-updates/configuration-options-for-the-dependabot.yml-file#scheduleinterval schedule: - interval: daily + interval: monthly open-pull-requests-limit: 1 labels: - tag:bot diff --git a/.github/labeler.yaml b/.github/labeler.yaml index 115f75197f41a..47f8737ebbf39 100644 --- a/.github/labeler.yaml +++ b/.github/labeler.yaml @@ -39,6 +39,3 @@ - tools/**/* "component:vehicle": - vehicle/**/* -"tag:require-cuda-build-and-test": - - perception/**/* - - sensing/**/* diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index f62904b03c6e4..c25f6b24c5e47 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -1,57 +1,40 @@ name: build-and-test-differential on: - pull_request: - types: - - opened - - synchronize - - reopened - - labeled - -concurrency: - group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} - cancel-in-progress: true + workflow_call: + inputs: + container: + required: true + type: string + runner: + default: ubuntu-24.04 + required: false + type: string + rosdistro: + default: humble + required: false + type: string + run-condition: + default: true + required: false + type: boolean + container-suffix: + required: false + default: "" + type: string + secrets: + codecov-token: + required: true env: CC: /usr/lib/ccache/gcc CXX: /usr/lib/ccache/g++ jobs: - make-sure-label-is-present: - uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 - with: - label: tag:run-build-and-test-differential - - make-sure-require-cuda-label-is-present: - uses: autowarefoundation/autoware-github-actions/.github/workflows/make-sure-label-is-present.yaml@v1 - with: - label: tag:require-cuda-build-and-test - - prepare-build-and-test-differential: - runs-on: ubuntu-latest - needs: [make-sure-label-is-present, make-sure-require-cuda-label-is-present] - outputs: - cuda_build: ${{ steps.check-if-cuda-build-is-required.outputs.cuda_build }} - steps: - - name: Check if cuda-build is required - id: check-if-cuda-build-is-required - run: | - if ${{ needs.make-sure-require-cuda-label-is-present.outputs.result == 'true' }}; then - echo "cuda-build is required" - echo "cuda_build=true" >> $GITHUB_OUTPUT - else - echo "cuda-build is not required" - echo "cuda_build=false" >> $GITHUB_OUTPUT - fi - shell: bash - - name: Fail if the tag:run-build-and-test-differential is missing - if: ${{ needs.make-sure-label-is-present.outputs.result != 'true' }} - run: exit 1 - build-and-test-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:universe-devel - needs: prepare-build-and-test-differential + if: ${{ inputs.run-condition }} + runs-on: ${{ inputs.runner }} + container: ${{ inputs.container }}${{ inputs.container-suffix }} steps: - name: Set PR fetch depth run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" @@ -63,61 +46,13 @@ jobs: ref: ${{ github.event.pull_request.head.sha }} fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - name: Run build-and-test-differential action - uses: ./.github/actions/build-and-test-differential - with: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - container-suffix: "" - runner: ubuntu-latest - build-depends-repos: build_depends.repos - build-pre-command: "" - codecov-token: ${{ secrets.CODECOV_TOKEN }} - - build-and-test-differential-cuda: - runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda - needs: prepare-build-and-test-differential - if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'true' }} - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" - shell: bash - - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 - with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - - name: Run build-and-test-differential action - uses: ./.github/actions/build-and-test-differential - with: - rosdistro: humble - container: ghcr.io/autowarefoundation/autoware:universe-devel - container-suffix: -cuda - runner: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - build-depends-repos: build_depends.repos - build-pre-command: taskset --cpu-list 0-5 - codecov-token: ${{ secrets.CODECOV_TOKEN }} - - clang-tidy-differential: - needs: [build-and-test-differential, prepare-build-and-test-differential] - if: ${{ needs.prepare-build-and-test-differential.outputs.cuda_build == 'false' }} - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:universe-devel - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" - - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 - with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - name: Show disk space before the tasks run: df -h + shell: bash + + - name: Show machine specs + run: lscpu && free -h + shell: bash - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 @@ -126,66 +61,69 @@ jobs: id: get-modified-packages uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - name: Get changed files (existing files only) - id: get-changed-files + - name: Create ccache directory run: | - echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + mkdir -p ${CCACHE_DIR} + du -sh ${CCACHE_DIR} && ccache -s shell: bash - - name: Run clang-tidy - if: ${{ steps.get-changed-files.outputs.changed-files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + - name: Attempt to restore ccache + uses: actions/cache/restore@v4 with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci - clang-tidy-ignore-path: .clang-tidy-ignore - build-depends-repos: build_depends.repos - cache-key-element: cuda + path: | + /root/.ccache + key: ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}-${{ github.event.pull_request.base.sha }} + restore-keys: | + ccache-main-${{ runner.arch }}-${{ inputs.rosdistro }}- - - name: Show disk space after the tasks - run: df -h + - name: Show ccache stats before build and reset stats + run: | + du -sh ${CCACHE_DIR} && ccache -s + ccache --zero-stats + shell: bash - clang-tidy-differential-cuda: - needs: build-and-test-differential-cuda - runs-on: codebuild-autoware-us-east-1-${{ github.run_id }}-${{ github.run_attempt }}-ubuntu-7.0-large - container: ghcr.io/autowarefoundation/autoware:universe-devel-cuda - steps: - - name: Set PR fetch depth - run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + - name: Export CUDA state as a variable for adding to cache key + run: | + build_type_cuda_state=nocuda + if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then + build_type_cuda_state=cuda + fi + echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" + echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" + shell: bash - - name: Checkout PR branch and all PR commits - uses: actions/checkout@v4 + - name: Build + if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} + uses: autowarefoundation/autoware-github-actions/colcon-build@v1 with: - ref: ${{ github.event.pull_request.head.sha }} - fetch-depth: ${{ env.PR_FETCH_DEPTH }} - - - 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 + rosdistro: ${{ inputs.rosdistro }} + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + build-depends-repos: build_depends.repos + cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} - - name: Get changed files (existing files only) - id: get-changed-files - run: | - echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + - name: Show ccache stats after build + run: du -sh ${CCACHE_DIR} && ccache -s shell: bash - - name: Run clang-tidy - if: ${{ steps.get-changed-files.outputs.changed-files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + - name: Test + id: test + if: ${{ steps.get-modified-packages.outputs.modified-packages != '' }} + uses: autowarefoundation/autoware-github-actions/colcon-test@v1 with: - rosdistro: humble + rosdistro: ${{ inputs.rosdistro }} target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci - clang-tidy-ignore-path: .clang-tidy-ignore build-depends-repos: build_depends.repos - cache-key-element: cuda + + - name: Upload coverage to CodeCov + if: ${{ steps.test.outputs.coverage-report-files != '' }} + uses: codecov/codecov-action@v4 + with: + files: ${{ steps.test.outputs.coverage-report-files }} + fail_ci_if_error: false + verbose: true + flags: differential${{ inputs.container-suffix }} + token: ${{ secrets.codecov-token }} - name: Show disk space after the tasks run: df -h + shell: bash diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 6b3886f124a44..60567d18b6beb 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -4,11 +4,11 @@ on: push: branches: - main - workflow_dispatch: concurrency: - group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} - cancel-in-progress: true + # Ensures sequential execution of this workflow + group: ${{ github.workflow }} + cancel-in-progress: false env: CC: /usr/lib/ccache/gcc diff --git a/.github/workflows/build-test-tidy-pr.yaml b/.github/workflows/build-test-tidy-pr.yaml new file mode 100644 index 0000000000000..c0442694b5c77 --- /dev/null +++ b/.github/workflows/build-test-tidy-pr.yaml @@ -0,0 +1,83 @@ +name: build-test-tidy-pr + +on: + pull_request: + types: + - opened + - synchronize + - reopened + - labeled + +concurrency: + group: ${{ github.workflow }}-${{ github.event.pull_request.number || github.run_id }} + cancel-in-progress: true + +jobs: + require-label: + uses: autowarefoundation/autoware-github-actions/.github/workflows/require-label.yaml@v1 + with: + label: run:build-and-test-differential + + check-if-cuda-job-is-needed: + needs: require-label + runs-on: ubuntu-latest + outputs: + cuda_job_is_needed: ${{ steps.check.outputs.any_changed }} + steps: + - uses: actions/checkout@v4 + + - name: Check if relevant files changed + id: check + uses: tj-actions/changed-files@v45 + with: + files: | + perception/** + sensing/** + + - name: Output result + run: | + echo "CUDA job needed: ${{ steps.check.outputs.any_changed }}" + shell: bash + + build-and-test-differential: + if: ${{ always() }} + needs: + - require-label + uses: ./.github/workflows/build-and-test-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + run-condition: ${{ needs.require-label.outputs.result == 'true' }} + secrets: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + build-and-test-differential-cuda: + if: ${{ always() }} + needs: check-if-cuda-job-is-needed + uses: ./.github/workflows/build-and-test-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' }} + secrets: + codecov-token: ${{ secrets.CODECOV_TOKEN }} + + clang-tidy-differential: + if: ${{ always() }} # always run to provide report for status check + needs: + - check-if-cuda-job-is-needed + - build-and-test-differential + uses: ./.github/workflows/clang-tidy-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'false' && needs.build-and-test-differential.result == 'success' }} + + clang-tidy-differential-cuda: + if: ${{ always() }} # always run to provide report for status check + needs: + - check-if-cuda-job-is-needed + - build-and-test-differential-cuda + uses: ./.github/workflows/clang-tidy-differential.yaml + with: + container: ghcr.io/autowarefoundation/autoware:universe-devel + container-suffix: -cuda + run-condition: ${{ needs.check-if-cuda-job-is-needed.outputs.cuda_job_is_needed == 'true' && needs.build-and-test-differential-cuda.result == 'success' }} diff --git a/.github/workflows/bump-version-pr.yaml b/.github/workflows/bump-version-pr.yaml new file mode 100644 index 0000000000000..5f3a931e90494 --- /dev/null +++ b/.github/workflows/bump-version-pr.yaml @@ -0,0 +1,80 @@ +name: bump-version-pr + +on: + workflow_dispatch: + inputs: + version_part: + description: Which part of the version number to bump? + required: true + default: minor + type: choice + options: + - major + - minor + - patch + +jobs: + bump-version-pr: + runs-on: ubuntu-22.04 + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + ref: humble + fetch-depth: 0 + + - name: Generate token + id: generate-token + uses: actions/create-github-app-token@v1 + with: + app-id: ${{ secrets.APP_ID }} + private-key: ${{ secrets.PRIVATE_KEY }} + + - name: Set git config + uses: autowarefoundation/autoware-github-actions/set-git-config@v1 + with: + token: ${{ steps.generate-token.outputs.token }} + + - name: Setup Python 3.x + uses: actions/setup-python@v5 + with: + python-version: 3.x + + - name: Install dependencies + run: pip3 install -U catkin_tools + shell: bash + + - name: Bump version from humble branch + id: bump-version-from-humble-branch + run: | + git checkout -b tmp/bot/bump_version_base + git fetch origin main + git merge origin/main + catkin_generate_changelog -y + git add * + git commit -m "update CHANGELOG.rst" + catkin_prepare_release -y --bump ${{ inputs.version_part }} --no-push + version=$(git describe --tags) + echo "version=${version}" >> $GITHUB_OUTPUT + shell: bash + + - name: Create target branch + run: | + git checkout origin/main + git checkout -b chore/bot/bump_version + git merge tmp/bot/bump_version_base + git push origin chore/bot/bump_version --force + shell: bash + + - name: Create PR + id: create-pr + run: > + gh + pr + create + --base=main + --body="Bump version to ${{ steps.bump-version-from-humble-branch.outputs.version }}" + --title="chore: bump version to ${{ steps.bump-version-from-humble-branch.outputs.version }}" + --head=chore/bot/bump_version + env: + GH_TOKEN: ${{ steps.generate-token.outputs.token }} diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..41d722c8e07b0 --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,80 @@ +name: clang-tidy-differential + +on: + workflow_call: + inputs: + container: + required: true + type: string + container-suffix: + required: false + default: "" + type: string + runner: + default: ubuntu-24.04 + required: false + type: string + run-condition: + default: true + required: false + type: boolean + +jobs: + clang-tidy-differential: + if: ${{ inputs.run-condition }} + runs-on: ${{ inputs.runner }} + container: ${{ inputs.container }}${{ inputs.container-suffix }} + steps: + - name: Set PR fetch depth + run: echo "PR_FETCH_DEPTH=$(( ${{ github.event.pull_request.commits }} + 1 ))" >> "${GITHUB_ENV}" + + - name: Checkout PR branch and all PR commits + uses: actions/checkout@v4 + with: + ref: ${{ github.event.pull_request.head.sha }} + fetch-depth: ${{ env.PR_FETCH_DEPTH }} + + - name: Show machine specs + run: lscpu && free -h + shell: bash + + - name: Show disk space before the tasks + run: df -h + shell: bash + + - 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 changed files (existing files only) + id: get-changed-files + run: | + echo "changed-files=$(git diff --name-only "origin/${{ github.base_ref }}"...HEAD | grep -E '\.(cpp|hpp)$' | while read -r file; do [ -e "$file" ] && echo -n "$file "; done)" >> $GITHUB_OUTPUT + shell: bash + + - name: Export CUDA state as a variable for adding to cache key + run: | + build_type_cuda_state=nocuda + if [[ "${{ inputs.container-suffix }}" == "-cuda" ]]; then + build_type_cuda_state=cuda + fi + echo "BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" >> "${GITHUB_ENV}" + echo "::notice::BUILD_TYPE_CUDA_STATE=$build_type_cuda_state" + shell: bash + + - name: Run clang-tidy + if: ${{ steps.get-changed-files.outputs.changed-files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy-ci + clang-tidy-ignore-path: .clang-tidy-ignore + build-depends-repos: build_depends.repos + cache-key-element: ${{ env.BUILD_TYPE_CUDA_STATE }} + + - name: Show disk space after the tasks + run: df -h diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index d39e97e540794..47009a25e69fd 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -26,7 +26,7 @@ jobs: prevent-no-label-execution: uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 with: - label: tag:deploy-docs + label: run:deploy-docs deploy-docs: needs: prevent-no-label-execution diff --git a/.github/workflows/github-release.yaml b/.github/workflows/github-release.yaml index bbe2ac512d70d..ac4f6f05ed9ec 100644 --- a/.github/workflows/github-release.yaml +++ b/.github/workflows/github-release.yaml @@ -6,14 +6,12 @@ name: github-release on: push: - branches: - - beta/v* tags: - - v* + - "[0-9]+.[0-9]+.[0-9]+" workflow_dispatch: inputs: - beta-branch-or-tag-name: - description: The name of the beta branch or tag to release + tag-name: + description: The name of the tag to release type: string required: true @@ -25,36 +23,24 @@ jobs: id: set-tag-name run: | if [ "${{ github.event_name }}" = "workflow_dispatch" ]; then - REF_NAME="${{ github.event.inputs.beta-branch-or-tag-name }}" + REF_NAME="${{ github.event.inputs.tag-name }}" else REF_NAME="${{ github.ref_name }}" fi - echo "ref-name=$REF_NAME" >> $GITHUB_OUTPUT - echo "tag-name=${REF_NAME#beta/}" >> $GITHUB_OUTPUT + echo "tag-name=$REF_NAME" >> $GITHUB_OUTPUT - name: Check out repository uses: actions/checkout@v4 with: fetch-depth: 0 - ref: ${{ steps.set-tag-name.outputs.ref-name }} - - - name: Set target name for beta branches - id: set-target-name - run: | - if [[ "${{ steps.set-tag-name.outputs.ref-name }}" =~ "beta/" ]]; then - echo "target-name=${{ steps.set-tag-name.outputs.ref-name }}" >> $GITHUB_OUTPUT - fi - - - name: Create a local tag for beta branches - run: | - if [ "${{ steps.set-target-name.outputs.target-name }}" != "" ]; then - git tag "${{ steps.set-tag-name.outputs.tag-name }}" - fi + ref: ${{ steps.set-tag-name.outputs.tag-name }} - name: Run generate-changelog id: generate-changelog uses: autowarefoundation/autoware-github-actions/generate-changelog@v1 + with: + git-cliff-args: --tag-pattern "^(\d+)\.(\d+)\.(\d+)$" --latest - name: Select verb id: select-verb @@ -74,7 +60,6 @@ jobs: run: | gh release ${{ steps.select-verb.outputs.verb }} "${{ steps.set-tag-name.outputs.tag-name }}" \ --draft \ - --target "${{ steps.set-target-name.outputs.target-name }}" \ --title "Release ${{ steps.set-tag-name.outputs.tag-name }}" \ --notes "$NOTES" env: diff --git a/.github/workflows/pre-commit-autoupdate.yaml b/.github/workflows/pre-commit-autoupdate.yaml index 489e32a1de166..60c17d9dabf72 100644 --- a/.github/workflows/pre-commit-autoupdate.yaml +++ b/.github/workflows/pre-commit-autoupdate.yaml @@ -6,7 +6,7 @@ name: pre-commit-autoupdate on: schedule: - - cron: 0 0 * * * + - cron: 0 0 1 1,4,7,10 * # quarterly workflow_dispatch: jobs: diff --git a/.github/workflows/pre-commit-optional-autoupdate.yaml b/.github/workflows/pre-commit-optional-autoupdate.yaml index be79ad481d16e..6639e5ca6c61a 100644 --- a/.github/workflows/pre-commit-optional-autoupdate.yaml +++ b/.github/workflows/pre-commit-optional-autoupdate.yaml @@ -6,7 +6,7 @@ name: pre-commit-optional-autoupdate on: schedule: - - cron: 0 0 * * * + - cron: 0 0 1 1,4,7,10 * # quarterly workflow_dispatch: jobs: diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index ff325af5e8774..f0b5bdba2d24d 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -2,6 +2,13 @@ # https://github.com/autowarefoundation/sync-file-templates # To make changes, update the source repository and follow the guidelines in its README. +# https://pre-commit.ci/#configuration +ci: + autofix_commit_msg: "style(pre-commit-optional): autofix" + # we already have our own daily update mechanism, we set this to quarterly + autoupdate_schedule: quarterly + autoupdate_commit_msg: "ci(pre-commit-optional): quarterly autoupdate" + repos: - repo: https://github.com/tcort/markdown-link-check rev: v3.12.2 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6e7c64fd982fc..48a97c13ef958 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -2,8 +2,12 @@ # https://github.com/autowarefoundation/sync-file-templates # To make changes, update the source repository and follow the guidelines in its README. +# https://pre-commit.ci/#configuration ci: autofix_commit_msg: "style(pre-commit): autofix" + # we already have our own daily update mechanism, we set this to quarterly + autoupdate_schedule: quarterly + autoupdate_commit_msg: "ci(pre-commit): quarterly autoupdate" repos: - repo: https://github.com/pre-commit/pre-commit-hooks @@ -70,7 +74,7 @@ repos: args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v19.1.4 + rev: v19.1.5 hooks: - id: clang-format types_or: [c++, c, cuda] diff --git a/build_depends.repos b/build_depends.repos index 4db947b7c26a8..2313a9be487a6 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -32,7 +32,7 @@ repositories: core/autoware_internal_msgs: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git - version: 1.1.0 + version: 1.3.0 # universe universe/external/tier4_autoware_msgs: type: git diff --git a/codecov.yaml b/codecov.yaml index 89b40a063b0ed..0a033171d0250 100644 --- a/codecov.yaml +++ b/codecov.yaml @@ -195,16 +195,16 @@ component_management: - planning/autoware_external_velocity_limit_selector/** - planning/autoware_freespace_planner/** - planning/autoware_freespace_planning_algorithms/** - - planning/autoware_mission_planner/** + - planning/autoware_mission_planner_universe/** # - planning/autoware_objects_of_interest_marker_interface/** - planning/autoware_obstacle_cruise_planner/** # - planning/autoware_obstacle_stop_planner/** - planning/autoware_path_optimizer/** - planning/autoware_path_smoother/** - planning/autoware_planning_test_manager/** - - planning/autoware_planning_topic_converter/** + # - planning/autoware_planning_topic_converter/** - planning/autoware_planning_validator/** - - planning/autoware_remaining_distance_time_calculator/** + # - planning/autoware_remaining_distance_time_calculator/** - planning/autoware_route_handler/** - planning/autoware_rtc_interface/** - planning/autoware_scenario_selector/** @@ -244,7 +244,7 @@ component_management: - planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/** - planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/** - planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** - - planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** - - planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** + - planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/** + - planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/** #### sampling_based_planner - planning/sampling_based_planner/autoware_bezier_sampler/** diff --git a/common/.pages b/common/.pages index 3c5a77c4d68cf..f49b7e1f6ba3d 100644 --- a/common/.pages +++ b/common/.pages @@ -46,6 +46,6 @@ nav: - 'Path Distance Calculator': common/autoware_path_distance_calculator/Readme - 'Others': - 'autoware_adapi_specs': common/autoware_adapi_specs - - 'autoware_component_interface_specs': common/autoware_component_interface_specs + - 'autoware_component_interface_specs_universe': common/autoware_component_interface_specs_universe - 'autoware_component_interface_tools': common/autoware_component_interface_tools - 'autoware_component_interface_utils': common/autoware_component_interface_utils diff --git a/common/autoware_adapi_specs/CHANGELOG.rst b/common/autoware_adapi_specs/CHANGELOG.rst index 7693074353d9f..e359542e5d0f1 100644 --- a/common/autoware_adapi_specs/CHANGELOG.rst +++ b/common/autoware_adapi_specs/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package autoware_adapi_specs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_default_adapi): release adapi v1.6.0 (`#9704 `_) + * feat: reject clearing route during autonomous mode + * feat: modify check and relay door service + * fix door condition + * fix error and add option + * update v1.6.0 + --------- +* Contributors: Fumiya Watanabe, Takagi, Isamu + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp b/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp index a7568d54b5e1a..dc907b6610cde 100644 --- a/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp +++ b/common/autoware_adapi_specs/include/autoware/adapi_specs/vehicle.hpp @@ -41,7 +41,7 @@ struct VehicleStatus using Message = autoware_adapi_v1_msgs::msg::VehicleStatus; static constexpr char name[] = "/api/vehicle/status"; static constexpr size_t depth = 1; - static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; }; diff --git a/common/autoware_adapi_specs/package.xml b/common/autoware_adapi_specs/package.xml index 5424d5bad52d5..6f02fafe11269 100644 --- a/common/autoware_adapi_specs/package.xml +++ b/common/autoware_adapi_specs/package.xml @@ -2,7 +2,7 @@ autoware_adapi_specs - 0.40.0 + 0.41.0 The autoware_adapi_specs package Takagi, Isamu Ryohsuke Mitsudome diff --git a/common/autoware_auto_common/CHANGELOG.rst b/common/autoware_auto_common/CHANGELOG.rst index 63f0fe2ac83e3..ec8415f295fbe 100644 --- a/common/autoware_auto_common/CHANGELOG.rst +++ b/common/autoware_auto_common/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_auto_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/common/autoware_auto_common/package.xml b/common/autoware_auto_common/package.xml index ccdcd4bab8608..f3bdd38dbc37b 100644 --- a/common/autoware_auto_common/package.xml +++ b/common/autoware_auto_common/package.xml @@ -2,7 +2,7 @@ autoware_auto_common - 0.40.0 + 0.41.0 Miscellaneous helper functions Apex.AI, Inc. Tomoya Kimura diff --git a/common/autoware_component_interface_specs/CHANGELOG.rst b/common/autoware_component_interface_specs_universe/CHANGELOG.rst similarity index 89% rename from common/autoware_component_interface_specs/CHANGELOG.rst rename to common/autoware_component_interface_specs_universe/CHANGELOG.rst index 43590966602d2..50b1f1a414fec 100644 --- a/common/autoware_component_interface_specs/CHANGELOG.rst +++ b/common/autoware_component_interface_specs_universe/CHANGELOG.rst @@ -1,7 +1,13 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_component_interface_specs +Changelog for package autoware_component_interface_specs_universe ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_component_interface_specs/CMakeLists.txt b/common/autoware_component_interface_specs_universe/CMakeLists.txt similarity index 88% rename from common/autoware_component_interface_specs/CMakeLists.txt rename to common/autoware_component_interface_specs_universe/CMakeLists.txt index a4e93cc8f04f9..b07b84edbdb80 100644 --- a/common/autoware_component_interface_specs/CMakeLists.txt +++ b/common/autoware_component_interface_specs_universe/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_component_interface_specs) +project(autoware_component_interface_specs_universe) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/common/autoware_component_interface_specs/README.md b/common/autoware_component_interface_specs_universe/README.md similarity index 55% rename from common/autoware_component_interface_specs/README.md rename to common/autoware_component_interface_specs_universe/README.md index 9d9298a95e329..3e372df1d83c7 100644 --- a/common/autoware_component_interface_specs/README.md +++ b/common/autoware_component_interface_specs_universe/README.md @@ -1,3 +1,3 @@ -# autoware_component_interface_specs +# autoware_component_interface_specs_universe This package is a specification of component interfaces. diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/control.hpp b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/control.hpp similarity index 86% rename from common/autoware_component_interface_specs/include/autoware/component_interface_specs/control.hpp rename to common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/control.hpp index 03c93d75d674b..8c289a29c4792 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/control.hpp +++ b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ -#define AUTOWARE__COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ +#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__CONTROL_HPP_ +#define AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__CONTROL_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace autoware::component_interface_specs::control +namespace autoware::component_interface_specs_universe::control { struct SetPause @@ -65,6 +65,6 @@ struct IsStopped static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; -} // namespace autoware::component_interface_specs::control +} // namespace autoware::component_interface_specs_universe::control -#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS__CONTROL_HPP_ +#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__CONTROL_HPP_ diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/localization.hpp b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/localization.hpp similarity index 85% rename from common/autoware_component_interface_specs/include/autoware/component_interface_specs/localization.hpp rename to common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/localization.hpp index b2e41a179506a..8e9680fef4210 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/localization.hpp +++ b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/localization.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_ -#define AUTOWARE__COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_ +#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__LOCALIZATION_HPP_ +#define AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__LOCALIZATION_HPP_ #include @@ -22,7 +22,7 @@ #include #include -namespace autoware::component_interface_specs::localization +namespace autoware::component_interface_specs_universe::localization { struct Initialize @@ -58,6 +58,6 @@ struct Acceleration static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; }; -} // namespace autoware::component_interface_specs::localization +} // namespace autoware::component_interface_specs_universe::localization -#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS__LOCALIZATION_HPP_ +#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__LOCALIZATION_HPP_ diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/map.hpp b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/map.hpp similarity index 76% rename from common/autoware_component_interface_specs/include/autoware/component_interface_specs/map.hpp rename to common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/map.hpp index aeb09e44c665f..ced71d0eb47db 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/map.hpp +++ b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/map.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS__MAP_HPP_ -#define AUTOWARE__COMPONENT_INTERFACE_SPECS__MAP_HPP_ +#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__MAP_HPP_ +#define AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__MAP_HPP_ #include #include -namespace autoware::component_interface_specs::map +namespace autoware::component_interface_specs_universe::map { struct MapProjectorInfo @@ -31,6 +31,6 @@ struct MapProjectorInfo static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; -} // namespace autoware::component_interface_specs::map +} // namespace autoware::component_interface_specs_universe::map -#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS__MAP_HPP_ +#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__MAP_HPP_ diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/perception.hpp b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/perception.hpp similarity index 74% rename from common/autoware_component_interface_specs/include/autoware/component_interface_specs/perception.hpp rename to common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/perception.hpp index 4f450e6a3ee1c..42adf7d1d54ae 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/perception.hpp +++ b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/perception.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ -#define AUTOWARE__COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ +#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__PERCEPTION_HPP_ +#define AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__PERCEPTION_HPP_ #include #include -namespace autoware::component_interface_specs::perception +namespace autoware::component_interface_specs_universe::perception { struct ObjectRecognition @@ -31,6 +31,6 @@ struct ObjectRecognition static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; }; -} // namespace autoware::component_interface_specs::perception +} // namespace autoware::component_interface_specs_universe::perception -#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS__PERCEPTION_HPP_ +#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__PERCEPTION_HPP_ diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/planning.hpp b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/planning.hpp similarity index 88% rename from common/autoware_component_interface_specs/include/autoware/component_interface_specs/planning.hpp rename to common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/planning.hpp index 247844123881b..7e79a47c70676 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/planning.hpp +++ b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/planning.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS__PLANNING_HPP_ -#define AUTOWARE__COMPONENT_INTERFACE_SPECS__PLANNING_HPP_ +#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__PLANNING_HPP_ +#define AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__PLANNING_HPP_ #include @@ -24,7 +24,7 @@ #include #include -namespace autoware::component_interface_specs::planning +namespace autoware::component_interface_specs_universe::planning { struct SetLaneletRoute @@ -73,6 +73,6 @@ struct Trajectory static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; }; -} // namespace autoware::component_interface_specs::planning +} // namespace autoware::component_interface_specs_universe::planning -#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS__PLANNING_HPP_ +#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__PLANNING_HPP_ diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/system.hpp b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/system.hpp similarity index 85% rename from common/autoware_component_interface_specs/include/autoware/component_interface_specs/system.hpp rename to common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/system.hpp index 6ae3447c0786d..11e69b0cd26a1 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/system.hpp +++ b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/system.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ -#define AUTOWARE__COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ +#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__SYSTEM_HPP_ +#define AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__SYSTEM_HPP_ #include @@ -22,7 +22,7 @@ #include #include -namespace autoware::component_interface_specs::system +namespace autoware::component_interface_specs_universe::system { struct MrmState @@ -55,6 +55,6 @@ struct OperationModeState static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; -} // namespace autoware::component_interface_specs::system +} // namespace autoware::component_interface_specs_universe::system -#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS__SYSTEM_HPP_ +#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__SYSTEM_HPP_ diff --git a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/vehicle.hpp b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/vehicle.hpp similarity index 91% rename from common/autoware_component_interface_specs/include/autoware/component_interface_specs/vehicle.hpp rename to common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/vehicle.hpp index 6c071299faae8..8db960f83ca15 100644 --- a/common/autoware_component_interface_specs/include/autoware/component_interface_specs/vehicle.hpp +++ b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/vehicle.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_ -#define AUTOWARE__COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_ +#ifndef AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__VEHICLE_HPP_ +#define AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__VEHICLE_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace autoware::component_interface_specs::vehicle +namespace autoware::component_interface_specs_universe::vehicle { struct SteeringStatus @@ -95,6 +95,6 @@ struct DoorStatus static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; }; -} // namespace autoware::component_interface_specs::vehicle +} // namespace autoware::component_interface_specs_universe::vehicle -#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS__VEHICLE_HPP_ +#endif // AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__VEHICLE_HPP_ diff --git a/common/autoware_component_interface_specs/package.xml b/common/autoware_component_interface_specs_universe/package.xml similarity index 87% rename from common/autoware_component_interface_specs/package.xml rename to common/autoware_component_interface_specs_universe/package.xml index 4bbe62e7e9701..456eca42701a0 100644 --- a/common/autoware_component_interface_specs/package.xml +++ b/common/autoware_component_interface_specs_universe/package.xml @@ -1,9 +1,9 @@ - autoware_component_interface_specs - 0.40.0 - The autoware_component_interface_specs package + autoware_component_interface_specs_universe + 0.41.0 + The autoware_component_interface_specs_universe package Takagi, Isamu Yukihiro Saito Apache License 2.0 diff --git a/common/autoware_component_interface_specs/test/gtest_main.cpp b/common/autoware_component_interface_specs_universe/test/gtest_main.cpp similarity index 100% rename from common/autoware_component_interface_specs/test/gtest_main.cpp rename to common/autoware_component_interface_specs_universe/test/gtest_main.cpp diff --git a/common/autoware_component_interface_specs/test/test_control.cpp b/common/autoware_component_interface_specs_universe/test/test_control.cpp similarity index 82% rename from common/autoware_component_interface_specs/test/test_control.cpp rename to common/autoware_component_interface_specs_universe/test/test_control.cpp index db6e7817e7a9d..5095a1b1be4bf 100644 --- a/common/autoware_component_interface_specs/test/test_control.cpp +++ b/common/autoware_component_interface_specs_universe/test/test_control.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/component_interface_specs/control.hpp" +#include "autoware/component_interface_specs_universe/control.hpp" #include "gtest/gtest.h" TEST(control, interface) { { - using autoware::component_interface_specs::control::IsPaused; + using autoware::component_interface_specs_universe::control::IsPaused; IsPaused is_paused; size_t depth = 1; EXPECT_EQ(is_paused.depth, depth); @@ -27,7 +27,7 @@ TEST(control, interface) } { - using autoware::component_interface_specs::control::IsStartRequested; + using autoware::component_interface_specs_universe::control::IsStartRequested; IsStartRequested is_start_requested; size_t depth = 1; EXPECT_EQ(is_start_requested.depth, depth); @@ -36,7 +36,7 @@ TEST(control, interface) } { - using autoware::component_interface_specs::control::IsStopped; + using autoware::component_interface_specs_universe::control::IsStopped; IsStopped is_stopped; size_t depth = 1; EXPECT_EQ(is_stopped.depth, depth); diff --git a/common/autoware_component_interface_specs/test/test_localization.cpp b/common/autoware_component_interface_specs_universe/test/test_localization.cpp similarity index 81% rename from common/autoware_component_interface_specs/test/test_localization.cpp rename to common/autoware_component_interface_specs_universe/test/test_localization.cpp index 18ec5f15acaf8..43bad964907e2 100644 --- a/common/autoware_component_interface_specs/test/test_localization.cpp +++ b/common/autoware_component_interface_specs_universe/test/test_localization.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/component_interface_specs/localization.hpp" +#include "autoware/component_interface_specs_universe/localization.hpp" #include "gtest/gtest.h" TEST(localization, interface) { { - using autoware::component_interface_specs::localization::InitializationState; + using autoware::component_interface_specs_universe::localization::InitializationState; InitializationState initialization_state; size_t depth = 1; EXPECT_EQ(initialization_state.depth, depth); @@ -27,7 +27,7 @@ TEST(localization, interface) } { - using autoware::component_interface_specs::localization::KinematicState; + using autoware::component_interface_specs_universe::localization::KinematicState; KinematicState kinematic_state; size_t depth = 1; EXPECT_EQ(kinematic_state.depth, depth); @@ -36,7 +36,7 @@ TEST(localization, interface) } { - using autoware::component_interface_specs::localization::Acceleration; + using autoware::component_interface_specs_universe::localization::Acceleration; Acceleration acceleration; size_t depth = 1; EXPECT_EQ(acceleration.depth, depth); diff --git a/common/autoware_component_interface_specs/test/test_map.cpp b/common/autoware_component_interface_specs_universe/test/test_map.cpp similarity index 86% rename from common/autoware_component_interface_specs/test/test_map.cpp rename to common/autoware_component_interface_specs_universe/test/test_map.cpp index aafb33e73c9dc..ed7932eb5f3a3 100644 --- a/common/autoware_component_interface_specs/test/test_map.cpp +++ b/common/autoware_component_interface_specs_universe/test/test_map.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/component_interface_specs/map.hpp" +#include "autoware/component_interface_specs_universe/map.hpp" #include "gtest/gtest.h" TEST(map, interface) { { - using autoware::component_interface_specs::map::MapProjectorInfo; + using autoware::component_interface_specs_universe::map::MapProjectorInfo; MapProjectorInfo map_projector; size_t depth = 1; EXPECT_EQ(map_projector.depth, depth); diff --git a/common/autoware_component_interface_specs/test/test_perception.cpp b/common/autoware_component_interface_specs_universe/test/test_perception.cpp similarity index 85% rename from common/autoware_component_interface_specs/test/test_perception.cpp rename to common/autoware_component_interface_specs_universe/test/test_perception.cpp index 8ad6d9dfceb6f..ddf84ab7f1196 100644 --- a/common/autoware_component_interface_specs/test/test_perception.cpp +++ b/common/autoware_component_interface_specs_universe/test/test_perception.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/component_interface_specs/perception.hpp" +#include "autoware/component_interface_specs_universe/perception.hpp" #include "gtest/gtest.h" TEST(perception, interface) { { - using autoware::component_interface_specs::perception::ObjectRecognition; + using autoware::component_interface_specs_universe::perception::ObjectRecognition; ObjectRecognition object_recognition; size_t depth = 1; EXPECT_EQ(object_recognition.depth, depth); diff --git a/common/autoware_component_interface_specs/test/test_planning.cpp b/common/autoware_component_interface_specs_universe/test/test_planning.cpp similarity index 81% rename from common/autoware_component_interface_specs/test/test_planning.cpp rename to common/autoware_component_interface_specs_universe/test/test_planning.cpp index 8278bf86f3861..3eff1653d298c 100644 --- a/common/autoware_component_interface_specs/test/test_planning.cpp +++ b/common/autoware_component_interface_specs_universe/test/test_planning.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/component_interface_specs/planning.hpp" +#include "autoware/component_interface_specs_universe/planning.hpp" #include "gtest/gtest.h" TEST(planning, interface) { { - using autoware::component_interface_specs::planning::RouteState; + using autoware::component_interface_specs_universe::planning::RouteState; RouteState state; size_t depth = 1; EXPECT_EQ(state.depth, depth); @@ -27,7 +27,7 @@ TEST(planning, interface) } { - using autoware::component_interface_specs::planning::LaneletRoute; + using autoware::component_interface_specs_universe::planning::LaneletRoute; LaneletRoute route; size_t depth = 1; EXPECT_EQ(route.depth, depth); @@ -36,7 +36,7 @@ TEST(planning, interface) } { - using autoware::component_interface_specs::planning::Trajectory; + using autoware::component_interface_specs_universe::planning::Trajectory; Trajectory trajectory; size_t depth = 1; EXPECT_EQ(trajectory.depth, depth); diff --git a/common/autoware_component_interface_specs/test/test_system.cpp b/common/autoware_component_interface_specs_universe/test/test_system.cpp similarity index 83% rename from common/autoware_component_interface_specs/test/test_system.cpp rename to common/autoware_component_interface_specs_universe/test/test_system.cpp index 3faff08734905..af2befc7b481d 100644 --- a/common/autoware_component_interface_specs/test/test_system.cpp +++ b/common/autoware_component_interface_specs_universe/test/test_system.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/component_interface_specs/system.hpp" +#include "autoware/component_interface_specs_universe/system.hpp" #include "gtest/gtest.h" TEST(system, interface) { { - using autoware::component_interface_specs::system::MrmState; + using autoware::component_interface_specs_universe::system::MrmState; MrmState state; size_t depth = 1; EXPECT_EQ(state.depth, depth); @@ -27,7 +27,7 @@ TEST(system, interface) } { - using autoware::component_interface_specs::system::OperationModeState; + using autoware::component_interface_specs_universe::system::OperationModeState; OperationModeState state; size_t depth = 1; EXPECT_EQ(state.depth, depth); diff --git a/common/autoware_component_interface_specs/test/test_vehicle.cpp b/common/autoware_component_interface_specs_universe/test/test_vehicle.cpp similarity index 79% rename from common/autoware_component_interface_specs/test/test_vehicle.cpp rename to common/autoware_component_interface_specs_universe/test/test_vehicle.cpp index e9ee65e2fa210..53eaede43ffd8 100644 --- a/common/autoware_component_interface_specs/test/test_vehicle.cpp +++ b/common/autoware_component_interface_specs_universe/test/test_vehicle.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/component_interface_specs/vehicle.hpp" +#include "autoware/component_interface_specs_universe/vehicle.hpp" #include "gtest/gtest.h" TEST(vehicle, interface) { { - using autoware::component_interface_specs::vehicle::SteeringStatus; + using autoware::component_interface_specs_universe::vehicle::SteeringStatus; SteeringStatus status; size_t depth = 1; EXPECT_EQ(status.depth, depth); @@ -27,7 +27,7 @@ TEST(vehicle, interface) } { - using autoware::component_interface_specs::vehicle::GearStatus; + using autoware::component_interface_specs_universe::vehicle::GearStatus; GearStatus status; size_t depth = 1; EXPECT_EQ(status.depth, depth); @@ -36,7 +36,7 @@ TEST(vehicle, interface) } { - using autoware::component_interface_specs::vehicle::TurnIndicatorStatus; + using autoware::component_interface_specs_universe::vehicle::TurnIndicatorStatus; TurnIndicatorStatus status; size_t depth = 1; EXPECT_EQ(status.depth, depth); @@ -45,7 +45,7 @@ TEST(vehicle, interface) } { - using autoware::component_interface_specs::vehicle::HazardLightStatus; + using autoware::component_interface_specs_universe::vehicle::HazardLightStatus; HazardLightStatus status; size_t depth = 1; EXPECT_EQ(status.depth, depth); @@ -54,7 +54,7 @@ TEST(vehicle, interface) } { - using autoware::component_interface_specs::vehicle::EnergyStatus; + using autoware::component_interface_specs_universe::vehicle::EnergyStatus; EnergyStatus status; size_t depth = 1; EXPECT_EQ(status.depth, depth); diff --git a/common/autoware_component_interface_tools/CHANGELOG.rst b/common/autoware_component_interface_tools/CHANGELOG.rst index b17fa431b27bb..bbe2c681e09c9 100644 --- a/common/autoware_component_interface_tools/CHANGELOG.rst +++ b/common/autoware_component_interface_tools/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_component_interface_tools ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_component_interface_tools/package.xml b/common/autoware_component_interface_tools/package.xml index e38b048e22ebe..20919e15acd13 100644 --- a/common/autoware_component_interface_tools/package.xml +++ b/common/autoware_component_interface_tools/package.xml @@ -2,7 +2,7 @@ autoware_component_interface_tools - 0.40.0 + 0.41.0 The autoware_component_interface_tools package Takagi, Isamu Apache License 2.0 diff --git a/common/autoware_component_interface_utils/CHANGELOG.rst b/common/autoware_component_interface_utils/CHANGELOG.rst index d19e72f19e469..750b2b246ca45 100644 --- a/common/autoware_component_interface_utils/CHANGELOG.rst +++ b/common/autoware_component_interface_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_component_interface_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_component_interface_utils/package.xml b/common/autoware_component_interface_utils/package.xml index 0bc51ee211d50..8a28af9c82acf 100755 --- a/common/autoware_component_interface_utils/package.xml +++ b/common/autoware_component_interface_utils/package.xml @@ -2,7 +2,7 @@ autoware_component_interface_utils - 0.40.0 + 0.41.0 The autoware_component_interface_utils package Takagi, Isamu Yukihiro Saito diff --git a/common/autoware_fake_test_node/CHANGELOG.rst b/common/autoware_fake_test_node/CHANGELOG.rst index 5bc3669a5aa27..7ba813f999f5e 100644 --- a/common/autoware_fake_test_node/CHANGELOG.rst +++ b/common/autoware_fake_test_node/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_fake_test_node ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_fake_test_node/package.xml b/common/autoware_fake_test_node/package.xml index 0ae881e4a41ba..039590b934227 100644 --- a/common/autoware_fake_test_node/package.xml +++ b/common/autoware_fake_test_node/package.xml @@ -2,7 +2,7 @@ autoware_fake_test_node - 0.40.0 + 0.41.0 A fake node that we can use in the integration-like cpp tests. Apex.AI, Inc. Tomoya Kimura diff --git a/common/autoware_global_parameter_loader/CHANGELOG.rst b/common/autoware_global_parameter_loader/CHANGELOG.rst index d778018320271..26f9d8d8e71d8 100644 --- a/common/autoware_global_parameter_loader/CHANGELOG.rst +++ b/common/autoware_global_parameter_loader/CHANGELOG.rst @@ -1,7 +1,10 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package global_parameter_loader +Changelog for package autoware_global_parameter_loader ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_global_parameter_loader/package.xml b/common/autoware_global_parameter_loader/package.xml index 8238e5852284f..9d2c5be940d8c 100644 --- a/common/autoware_global_parameter_loader/package.xml +++ b/common/autoware_global_parameter_loader/package.xml @@ -2,7 +2,7 @@ autoware_global_parameter_loader - 0.40.0 + 0.41.0 The autoware_global_parameter_loader package Ryohsuke Mitsudome Apache License 2.0 diff --git a/common/autoware_glog_component/CHANGELOG.rst b/common/autoware_glog_component/CHANGELOG.rst index e9176b6524049..2df0c43506d7d 100644 --- a/common/autoware_glog_component/CHANGELOG.rst +++ b/common/autoware_glog_component/CHANGELOG.rst @@ -1,7 +1,10 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package glog_component +Changelog for package autoware_glog_component ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_glog_component/package.xml b/common/autoware_glog_component/package.xml index eab95fa8944d3..f80511f8eb17a 100644 --- a/common/autoware_glog_component/package.xml +++ b/common/autoware_glog_component/package.xml @@ -2,7 +2,7 @@ autoware_glog_component - 0.40.0 + 0.41.0 The autoware_glog_component package Takamasa Horibe Apache License 2.0 diff --git a/common/autoware_goal_distance_calculator/CHANGELOG.rst b/common/autoware_goal_distance_calculator/CHANGELOG.rst index df190270e02ab..ad226c45ce31a 100644 --- a/common/autoware_goal_distance_calculator/CHANGELOG.rst +++ b/common/autoware_goal_distance_calculator/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_goal_distance_calculator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_goal_distance_calculator)!: tier4_debug_msgs to autoware_internal_debug_msgs for autoware_goal_distance_calculator (`#9833 `_) +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/common/autoware_goal_distance_calculator/Readme.md b/common/autoware_goal_distance_calculator/Readme.md index 28a1e2fada086..77a71bb23c2bc 100644 --- a/common/autoware_goal_distance_calculator/Readme.md +++ b/common/autoware_goal_distance_calculator/Readme.md @@ -17,12 +17,12 @@ This node publishes deviation of self-pose from goal pose. ### Output -| Name | Type | Description | -| ------------------------ | --------------------------------------- | ------------------------------------------------------------- | -| `deviation/lateral` | `tier4_debug_msgs::msg::Float64Stamped` | publish lateral deviation of self-pose from goal pose[m] | -| `deviation/longitudinal` | `tier4_debug_msgs::msg::Float64Stamped` | publish longitudinal deviation of self-pose from goal pose[m] | -| `deviation/yaw` | `tier4_debug_msgs::msg::Float64Stamped` | publish yaw deviation of self-pose from goal pose[rad] | -| `deviation/yaw_deg` | `tier4_debug_msgs::msg::Float64Stamped` | publish yaw deviation of self-pose from goal pose[deg] | +| Name | Type | Description | +| ------------------------ | --------------------------------------------------- | ------------------------------------------------------------- | +| `deviation/lateral` | `autoware_internal_debug_msgs::msg::Float64Stamped` | publish lateral deviation of self-pose from goal pose[m] | +| `deviation/longitudinal` | `autoware_internal_debug_msgs::msg::Float64Stamped` | publish longitudinal deviation of self-pose from goal pose[m] | +| `deviation/yaw` | `autoware_internal_debug_msgs::msg::Float64Stamped` | publish yaw deviation of self-pose from goal pose[rad] | +| `deviation/yaw_deg` | `autoware_internal_debug_msgs::msg::Float64Stamped` | publish yaw deviation of self-pose from goal pose[deg] | ## Parameters diff --git a/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator_node.hpp index f6f617292ab15..60d33b6ddc7af 100644 --- a/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/autoware_goal_distance_calculator/include/autoware/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #include diff --git a/common/autoware_goal_distance_calculator/package.xml b/common/autoware_goal_distance_calculator/package.xml index 903d77515f262..60de6e02e7f45 100644 --- a/common/autoware_goal_distance_calculator/package.xml +++ b/common/autoware_goal_distance_calculator/package.xml @@ -2,7 +2,7 @@ autoware_goal_distance_calculator - 0.40.0 + 0.41.0 The autoware_goal_distance_calculator package Taiki Tanaka Apache License 2.0 @@ -10,12 +10,12 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs autoware_planning_msgs autoware_universe_utils geometry_msgs rclcpp rclcpp_components - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/common/autoware_goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/autoware_goal_distance_calculator/src/goal_distance_calculator_node.cpp index c00a2e9bb452d..0e38dfc170f2e 100644 --- a/common/autoware_goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/autoware_goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -100,12 +100,13 @@ void GoalDistanceCalculatorNode::onTimer() using autoware::universe_utils::rad2deg; const auto & deviation = output.goal_deviation; - debug_publisher_.publish( + debug_publisher_.publish( "deviation/lateral", deviation.lateral); - debug_publisher_.publish( + debug_publisher_.publish( "deviation/longitudinal", deviation.longitudinal); - debug_publisher_.publish("deviation/yaw", deviation.yaw); - debug_publisher_.publish( + debug_publisher_.publish( + "deviation/yaw", deviation.yaw); + debug_publisher_.publish( "deviation/yaw_deg", rad2deg(deviation.yaw)); RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 1000, diff --git a/common/autoware_grid_map_utils/CHANGELOG.rst b/common/autoware_grid_map_utils/CHANGELOG.rst index 7a1b19b90a958..afb807859f8b6 100644 --- a/common/autoware_grid_map_utils/CHANGELOG.rst +++ b/common/autoware_grid_map_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_grid_map_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_grid_map_utils/package.xml b/common/autoware_grid_map_utils/package.xml index bf89abc507173..d745d007f8b37 100644 --- a/common/autoware_grid_map_utils/package.xml +++ b/common/autoware_grid_map_utils/package.xml @@ -2,7 +2,7 @@ autoware_grid_map_utils - 0.40.0 + 0.41.0 Utilities for the grid_map library Maxime CLEMENT Apache License 2.0 diff --git a/common/autoware_interpolation/CHANGELOG.rst b/common/autoware_interpolation/CHANGELOG.rst index 37fc48c525a49..e84799ae6be49 100644 --- a/common/autoware_interpolation/CHANGELOG.rst +++ b/common/autoware_interpolation/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_interpolation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_interpolation): remove clang compiler error (`#9711 `_) +* Contributors: Fumiya Watanabe, Ryuta Kambe + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_interpolation/package.xml b/common/autoware_interpolation/package.xml index 3bad757405ed4..0a8d103b31f09 100644 --- a/common/autoware_interpolation/package.xml +++ b/common/autoware_interpolation/package.xml @@ -2,7 +2,7 @@ autoware_interpolation - 0.40.0 + 0.41.0 The spline interpolation package Fumiya Watanabe Takayuki Murooka diff --git a/common/autoware_interpolation/test/src/test_interpolation_utils.cpp b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp index 2aa41b6fdef00..9131b458dd68b 100644 --- a/common/autoware_interpolation/test/src/test_interpolation_utils.cpp +++ b/common/autoware_interpolation/test/src/test_interpolation_utils.cpp @@ -18,8 +18,6 @@ #include -constexpr double epsilon = 1e-6; - TEST(interpolation_utils, isIncreasing) { // empty diff --git a/common/autoware_kalman_filter/CHANGELOG.rst b/common/autoware_kalman_filter/CHANGELOG.rst deleted file mode 100644 index a7e68f8843e74..0000000000000 --- a/common/autoware_kalman_filter/CHANGELOG.rst +++ /dev/null @@ -1,55 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_kalman_filter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(kalman_filter): prefix package and namespace with autoware (`#7787 `_) - * refactor(kalman_filter): prefix package and namespace with autoware - * move headers to include/autoware/ - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_kalman_filter/CMakeLists.txt b/common/autoware_kalman_filter/CMakeLists.txt deleted file mode 100644 index 076d2d3cad4e8..0000000000000 --- a/common/autoware_kalman_filter/CMakeLists.txt +++ /dev/null @@ -1,29 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_kalman_filter) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(eigen3_cmake_module REQUIRED) -find_package(Eigen3 REQUIRED) - -include_directories( - SYSTEM - ${EIGEN3_INCLUDE_DIR} -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/kalman_filter.cpp - src/time_delay_kalman_filter.cpp - include/autoware/kalman_filter/kalman_filter.hpp - include/autoware/kalman_filter/time_delay_kalman_filter.hpp -) - -if(BUILD_TESTING) - file(GLOB_RECURSE test_files test/*.cpp) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) - - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) -endif() - -ament_auto_package() diff --git a/common/autoware_kalman_filter/README.md b/common/autoware_kalman_filter/README.md deleted file mode 100644 index 7c0feb9c2a61a..0000000000000 --- a/common/autoware_kalman_filter/README.md +++ /dev/null @@ -1,9 +0,0 @@ -# kalman_filter - -## Purpose - -This common package contains the kalman filter with time delay and the calculation of the kalman filter. - -## Assumptions / Known limits - -TBD. diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp deleted file mode 100644 index 74db04f6e838b..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/kalman_filter.hpp +++ /dev/null @@ -1,214 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ - -#include -#include - -namespace autoware::kalman_filter -{ - -/** - * @file kalman_filter.h - * @brief kalman filter class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - KalmanFilter(); - - /** - * @brief constructor with initialization - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - KalmanFilter( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P); - - /** - * @brief destructor - */ - ~KalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param C coefficient matrix of x for measurement model - * @param Q covariance matrix for process model - * @param R covariance matrix for measurement model - * @param P initial covariance of estimated state - */ - bool init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P initial covariance of estimated state - */ - bool init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0); - - /** - * @brief set A of process model - * @param A coefficient matrix of x for process model - */ - void setA(const Eigen::MatrixXd & A); - - /** - * @brief set B of process model - * @param B coefficient matrix of u for process model - */ - void setB(const Eigen::MatrixXd & B); - - /** - * @brief set C of measurement model - * @param C coefficient matrix of x for measurement model - */ - void setC(const Eigen::MatrixXd & C); - - /** - * @brief set covariance matrix Q for process model - * @param Q covariance matrix for process model - */ - void setQ(const Eigen::MatrixXd & Q); - - /** - * @brief set covariance matrix R for measurement model - * @param R covariance matrix for measurement model - */ - void setR(const Eigen::MatrixXd & R); - - /** - * @brief get current kalman filter state - * @param x kalman filter state - */ - void getX(Eigen::MatrixXd & x) const; - - /** - * @brief get current kalman filter covariance - * @param P kalman filter covariance - */ - void getP(Eigen::MatrixXd & P) const; - - /** - * @brief get component of current kalman filter state - * @param i index of kalman filter state - * @return value of i's component of the kalman filter state x[i] - */ - double getXelement(unsigned int i) const; - - /** - * @brief calculate kalman filter state and covariance by prediction model with A, B, Q matrix. - * This is mainly for EKF with variable matrix. - * @param u input for model - * @param A coefficient matrix of x for process model - * @param B coefficient matrix of u for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance with prediction model with x, A, Q matrix. This is - * mainly for EKF with variable matrix. - * @param x_next predicted state - * @param A coefficient matrix of x for process model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A); - - /** - * @brief calculate kalman filter state by prediction model with A, B and Q being class member - * variables. - * @param u input for the model - * @return bool to check matrix operations are being performed properly - */ - bool predict(const Eigen::MatrixXd & u); - - /** - * @brief calculate kalman filter state by measurement model with y_pred, C and R matrix. This is - * mainly for EKF with variable matrix. - * @param y measured values - * @param y output values expected from measurement model - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @return bool to check matrix operations are being performed properly - */ - bool update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R matrix. This is mainly - * for EKF with variable matrix. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R); - - /** - * @brief calculate kalman filter state by measurement model with C and R being class member - * variables. - * @param y measured values - * @return bool to check matrix operations are being performed properly - */ - bool update(const Eigen::MatrixXd & y); - -protected: - Eigen::MatrixXd x_; //!< @brief current estimated state - Eigen::MatrixXd - A_; //!< @brief coefficient matrix of x for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd - B_; //!< @brief coefficient matrix of u for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd C_; //!< @brief coefficient matrix of x for measurement model y[k] = C * x[k] - Eigen::MatrixXd Q_; //!< @brief covariance matrix for process model x[k+1] = A*x[k] + B*u[k] - Eigen::MatrixXd R_; //!< @brief covariance matrix for measurement model y[k] = C * x[k] - Eigen::MatrixXd P_; //!< @brief covariance of estimated state -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp b/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp deleted file mode 100644 index 80375b7579e62..0000000000000 --- a/common/autoware_kalman_filter/include/autoware/kalman_filter/time_delay_kalman_filter.hpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ -#define AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include -#include - -#include - -namespace autoware::kalman_filter -{ -/** - * @file time_delay_kalman_filter.h - * @brief kalman filter with delayed measurement class - * @author Takamasa Horibe - * @date 2019.05.01 - */ - -class TimeDelayKalmanFilter : public KalmanFilter -{ -public: - /** - * @brief No initialization constructor. - */ - TimeDelayKalmanFilter(); - - /** - * @brief initialization of kalman filter - * @param x initial state - * @param P0 initial covariance of estimated state - * @param max_delay_step Maximum number of delay steps, which determines the dimension of the - * extended kalman filter - */ - void init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P, const int max_delay_step); - - /** - * @brief get latest time estimated state - */ - Eigen::MatrixXd getLatestX() const; - - /** - * @brief get latest time estimation covariance - */ - Eigen::MatrixXd getLatestP() const; - - /** - * @brief calculate kalman filter covariance by precision model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param x_next predicted state by prediction model - * @param A coefficient matrix of x for process model - * @param Q covariance matrix for process model - */ - bool predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q); - - /** - * @brief calculate kalman filter covariance by measurement model with time delay. This is mainly - * for EKF of nonlinear process model. - * @param y measured values - * @param C coefficient matrix of x for measurement model - * @param R covariance matrix for measurement model - * @param delay_step measurement delay - */ - bool updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step); - -private: - int max_delay_step_; //!< @brief maximum number of delay steps - int dim_x_; //!< @brief dimension of latest state - int dim_x_ex_; //!< @brief dimension of extended state with dime delay -}; -} // namespace autoware::kalman_filter -#endif // AUTOWARE__KALMAN_FILTER__TIME_DELAY_KALMAN_FILTER_HPP_ diff --git a/common/autoware_kalman_filter/package.xml b/common/autoware_kalman_filter/package.xml deleted file mode 100644 index e51bb06e9de43..0000000000000 --- a/common/autoware_kalman_filter/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - autoware_kalman_filter - 0.40.0 - The kalman filter package - Yukihiro Saito - Takeshi Ishita - Koji Minoda - Apache License 2.0 - - Takamasa Horibe - - ament_cmake_auto - autoware_cmake - - eigen - eigen3_cmake_module - - ament_cmake_cppcheck - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_kalman_filter/src/kalman_filter.cpp b/common/autoware_kalman_filter/src/kalman_filter.cpp deleted file mode 100644 index bbd963675f9e2..0000000000000 --- a/common/autoware_kalman_filter/src/kalman_filter.cpp +++ /dev/null @@ -1,161 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/kalman_filter.hpp" - -namespace autoware::kalman_filter -{ -KalmanFilter::KalmanFilter() -{ -} -KalmanFilter::KalmanFilter( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P) -{ - init(x, A, B, C, Q, R, P); -} -KalmanFilter::~KalmanFilter() -{ -} -bool KalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & C, const Eigen::MatrixXd & Q, const Eigen::MatrixXd & R, - const Eigen::MatrixXd & P) -{ - if ( - x.cols() == 0 || x.rows() == 0 || A.cols() == 0 || A.rows() == 0 || B.cols() == 0 || - B.rows() == 0 || C.cols() == 0 || C.rows() == 0 || Q.cols() == 0 || Q.rows() == 0 || - R.cols() == 0 || R.rows() == 0 || P.cols() == 0 || P.rows() == 0) { - return false; - } - x_ = x; - A_ = A; - B_ = B; - C_ = C; - Q_ = Q; - R_ = R; - P_ = P; - return true; -} -bool KalmanFilter::init(const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0) -{ - if (x.cols() == 0 || x.rows() == 0 || P0.cols() == 0 || P0.rows() == 0) { - return false; - } - x_ = x; - P_ = P0; - return true; -} - -void KalmanFilter::setA(const Eigen::MatrixXd & A) -{ - A_ = A; -} -void KalmanFilter::setB(const Eigen::MatrixXd & B) -{ - B_ = B; -} -void KalmanFilter::setC(const Eigen::MatrixXd & C) -{ - C_ = C; -} -void KalmanFilter::setQ(const Eigen::MatrixXd & Q) -{ - Q_ = Q; -} -void KalmanFilter::setR(const Eigen::MatrixXd & R) -{ - R_ = R; -} -void KalmanFilter::getX(Eigen::MatrixXd & x) const -{ - x = x_; -} -void KalmanFilter::getP(Eigen::MatrixXd & P) const -{ - P = P_; -} -double KalmanFilter::getXelement(unsigned int i) const -{ - return x_(i); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - if ( - x_.rows() != x_next.rows() || A.cols() != P_.rows() || Q.cols() != Q.rows() || - A.rows() != Q.cols()) { - return false; - } - x_ = x_next; - P_ = A * P_ * A.transpose() + Q; - return true; -} -bool KalmanFilter::predict(const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A) -{ - return predict(x_next, A, Q_); -} - -bool KalmanFilter::predict( - const Eigen::MatrixXd & u, const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, - const Eigen::MatrixXd & Q) -{ - if (A.cols() != x_.rows() || B.cols() != u.rows()) { - return false; - } - const Eigen::MatrixXd x_next = A * x_ + B * u; - return predict(x_next, A, Q); -} -bool KalmanFilter::predict(const Eigen::MatrixXd & u) -{ - return predict(u, A_, B_, Q_); -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & y_pred, const Eigen::MatrixXd & C, - const Eigen::MatrixXd & R) -{ - if ( - P_.cols() != C.cols() || R.rows() != R.cols() || R.rows() != C.rows() || - y.rows() != y_pred.rows() || y.rows() != C.rows()) { - return false; - } - const Eigen::MatrixXd PCT = P_ * C.transpose(); - const Eigen::MatrixXd K = PCT * ((R + C * PCT).inverse()); - - if (isnan(K.array()).any() || isinf(K.array()).any()) { - return false; - } - - x_ = x_ + K * (y - y_pred); - P_ = P_ - K * (C * P_); - return true; -} - -bool KalmanFilter::update( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R) -{ - if (C.cols() != x_.rows()) { - return false; - } - const Eigen::MatrixXd y_pred = C * x_; - return update(y, y_pred, C, R); -} -bool KalmanFilter::update(const Eigen::MatrixXd & y) -{ - return update(y, C_, R_); -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp deleted file mode 100644 index 4d1dd8f33b7a6..0000000000000 --- a/common/autoware_kalman_filter/src/time_delay_kalman_filter.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -namespace autoware::kalman_filter -{ -TimeDelayKalmanFilter::TimeDelayKalmanFilter() -{ -} - -void TimeDelayKalmanFilter::init( - const Eigen::MatrixXd & x, const Eigen::MatrixXd & P0, const int max_delay_step) -{ - max_delay_step_ = max_delay_step; - dim_x_ = x.rows(); - dim_x_ex_ = dim_x_ * max_delay_step; - - x_ = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - P_ = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - - for (int i = 0; i < max_delay_step_; ++i) { - x_.block(i * dim_x_, 0, dim_x_, 1) = x; - P_.block(i * dim_x_, i * dim_x_, dim_x_, dim_x_) = P0; - } -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestX() const -{ - return x_.block(0, 0, dim_x_, 1); -} - -Eigen::MatrixXd TimeDelayKalmanFilter::getLatestP() const -{ - return P_.block(0, 0, dim_x_, dim_x_); -} - -bool TimeDelayKalmanFilter::predictWithDelay( - const Eigen::MatrixXd & x_next, const Eigen::MatrixXd & A, const Eigen::MatrixXd & Q) -{ - /* - * time delay model: - * - * [A 0 0] [P11 P12 P13] [Q 0 0] - * A = [I 0 0], P = [P21 P22 P23], Q = [0 0 0] - * [0 I 0] [P31 P32 P33] [0 0 0] - * - * covariance calculation in prediction : P = A * P * A' + Q - * - * [A*P11*A'*+Q A*P11 A*P12] - * P = [ P11*A' P11 P12] - * [ P21*A' P21 P22] - */ - - const int d_dim_x = dim_x_ex_ - dim_x_; - - /* slide states in the time direction */ - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, 1); - x_tmp.block(0, 0, dim_x_, 1) = x_next; - x_tmp.block(dim_x_, 0, d_dim_x, 1) = x_.block(0, 0, d_dim_x, 1); - x_ = x_tmp; - - /* update P with delayed measurement A matrix structure */ - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex_, dim_x_ex_); - P_tmp.block(0, 0, dim_x_, dim_x_) = A * P_.block(0, 0, dim_x_, dim_x_) * A.transpose() + Q; - P_tmp.block(0, dim_x_, dim_x_, d_dim_x) = A * P_.block(0, 0, dim_x_, d_dim_x); - P_tmp.block(dim_x_, 0, d_dim_x, dim_x_) = P_.block(0, 0, d_dim_x, dim_x_) * A.transpose(); - P_tmp.block(dim_x_, dim_x_, d_dim_x, d_dim_x) = P_.block(0, 0, d_dim_x, d_dim_x); - P_ = P_tmp; - - return true; -} - -bool TimeDelayKalmanFilter::updateWithDelay( - const Eigen::MatrixXd & y, const Eigen::MatrixXd & C, const Eigen::MatrixXd & R, - const int delay_step) -{ - if (delay_step >= max_delay_step_) { - std::cerr << "delay step is larger than max_delay_step. ignore update." << std::endl; - return false; - } - - const int dim_y = y.rows(); - - /* set measurement matrix */ - Eigen::MatrixXd C_ex = Eigen::MatrixXd::Zero(dim_y, dim_x_ex_); - C_ex.block(0, dim_x_ * delay_step, dim_y, dim_x_) = C; - - /* update */ - if (!update(y, C_ex, R)) { - return false; - } - - return true; -} -} // namespace autoware::kalman_filter diff --git a/common/autoware_kalman_filter/test/test_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_kalman_filter.cpp deleted file mode 100644 index 34e23ef9d06e2..0000000000000 --- a/common/autoware_kalman_filter/test/test_kalman_filter.cpp +++ /dev/null @@ -1,96 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/kalman_filter.hpp" - -#include - -using autoware::kalman_filter::KalmanFilter; - -TEST(kalman_filter, kf) -{ - KalmanFilter kf_; - - Eigen::MatrixXd x_t(2, 1); - x_t << 1, 2; - - Eigen::MatrixXd P_t(2, 2); - P_t << 1, 0, 0, 1; - - Eigen::MatrixXd Q_t(2, 2); - Q_t << 0.01, 0, 0, 0.01; - - Eigen::MatrixXd R_t(2, 2); - R_t << 0.09, 0, 0, 0.09; - - Eigen::MatrixXd C_t(2, 2); - C_t << 1, 0, 0, 1; - - Eigen::MatrixXd A_t(2, 2); - A_t << 1, 0, 0, 1; - - Eigen::MatrixXd B_t(2, 2); - B_t << 1, 0, 0, 1; - - // Initialize the filter and check if initialization was successful - EXPECT_TRUE(kf_.init(x_t, A_t, B_t, C_t, Q_t, R_t, P_t)); - - // Perform prediction - Eigen::MatrixXd u_t(2, 1); - u_t << 0.1, 0.1; - EXPECT_TRUE(kf_.predict(u_t)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_predict_expected = A_t * x_t + B_t * u_t; - Eigen::MatrixXd P_predict_expected = A_t * P_t * A_t.transpose() + Q_t; - - Eigen::MatrixXd x_predict; - kf_.getX(x_predict); - Eigen::MatrixXd P_predict; - kf_.getP(P_predict); - - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - - // Perform update - Eigen::MatrixXd y_t(2, 1); - y_t << 1.05, 2.05; - EXPECT_TRUE(kf_.update(y_t)); - - // Check the updated state and covariance matrix - const Eigen::MatrixXd PCT_t = P_predict_expected * C_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_t * x_predict_expected; - Eigen::MatrixXd x_update_expected = x_predict_expected + K_t * (y_t - y_pred); - Eigen::MatrixXd P_update_expected = P_predict_expected - K_t * (C_t * P_predict_expected); - - Eigen::MatrixXd x_update; - kf_.getX(x_update); - Eigen::MatrixXd P_update; - kf_.getP(P_update); - - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); -} - -int main(int argc, char * argv[]) -{ - testing::InitGoogleTest(&argc, argv); - bool result = RUN_ALL_TESTS(); - return result; -} diff --git a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp b/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp deleted file mode 100644 index 50c22fae123bc..0000000000000 --- a/common/autoware_kalman_filter/test/test_time_delay_kalman_filter.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2023 The Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/kalman_filter/time_delay_kalman_filter.hpp" - -#include - -using autoware::kalman_filter::TimeDelayKalmanFilter; - -TEST(time_delay_kalman_filter, td_kf) -{ - TimeDelayKalmanFilter td_kf_; - - Eigen::MatrixXd x_t(3, 1); - x_t << 1.0, 2.0, 3.0; - Eigen::MatrixXd P_t(3, 3); - P_t << 0.1, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.3; - const int max_delay_step = 5; - const int dim_x = x_t.rows(); - const int dim_x_ex = dim_x * max_delay_step; - // Initialize the filter - td_kf_.init(x_t, P_t, max_delay_step); - - // Check if initialization was successful - Eigen::MatrixXd x_init = td_kf_.getLatestX(); - Eigen::MatrixXd P_init = td_kf_.getLatestP(); - Eigen::MatrixXd x_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, 1); - Eigen::MatrixXd P_ex_t = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - for (int i = 0; i < max_delay_step; ++i) { - x_ex_t.block(i * dim_x, 0, dim_x, 1) = x_t; - P_ex_t.block(i * dim_x, i * dim_x, dim_x, dim_x) = P_t; - } - - EXPECT_EQ(x_init.rows(), 3); - EXPECT_EQ(x_init.cols(), 1); - EXPECT_EQ(P_init.rows(), 3); - EXPECT_EQ(P_init.cols(), 3); - EXPECT_NEAR(x_init(0, 0), x_t(0, 0), 1e-5); - EXPECT_NEAR(x_init(1, 0), x_t(1, 0), 1e-5); - EXPECT_NEAR(x_init(2, 0), x_t(2, 0), 1e-5); - EXPECT_NEAR(P_init(0, 0), P_t(0, 0), 1e-5); - EXPECT_NEAR(P_init(1, 1), P_t(1, 1), 1e-5); - EXPECT_NEAR(P_init(2, 2), P_t(2, 2), 1e-5); - - // Define prediction parameters - Eigen::MatrixXd A_t(3, 3); - A_t << 2.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 2.0; - Eigen::MatrixXd Q_t(3, 3); - Q_t << 0.01, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.03; - Eigen::MatrixXd x_next(3, 1); - x_next << 2.0, 4.0, 6.0; - - // Perform prediction - EXPECT_TRUE(td_kf_.predictWithDelay(x_next, A_t, Q_t)); - - // Check the prediction state and covariance matrix - Eigen::MatrixXd x_predict = td_kf_.getLatestX(); - Eigen::MatrixXd P_predict = td_kf_.getLatestP(); - Eigen::MatrixXd x_tmp = Eigen::MatrixXd::Zero(dim_x_ex, 1); - x_tmp.block(0, 0, dim_x, 1) = A_t * x_t; - x_tmp.block(dim_x, 0, dim_x_ex - dim_x, 1) = x_ex_t.block(0, 0, dim_x_ex - dim_x, 1); - x_ex_t = x_tmp; - Eigen::MatrixXd x_predict_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_tmp = Eigen::MatrixXd::Zero(dim_x_ex, dim_x_ex); - P_tmp.block(0, 0, dim_x, dim_x) = A_t * P_ex_t.block(0, 0, dim_x, dim_x) * A_t.transpose() + Q_t; - P_tmp.block(0, dim_x, dim_x, dim_x_ex - dim_x) = - A_t * P_ex_t.block(0, 0, dim_x, dim_x_ex - dim_x); - P_tmp.block(dim_x, 0, dim_x_ex - dim_x, dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x) * A_t.transpose(); - P_tmp.block(dim_x, dim_x, dim_x_ex - dim_x, dim_x_ex - dim_x) = - P_ex_t.block(0, 0, dim_x_ex - dim_x, dim_x_ex - dim_x); - P_ex_t = P_tmp; - Eigen::MatrixXd P_predict_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_predict(0, 0), x_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(x_predict(1, 0), x_predict_expected(1, 0), 1e-5); - EXPECT_NEAR(x_predict(2, 0), x_predict_expected(2, 0), 1e-5); - EXPECT_NEAR(P_predict(0, 0), P_predict_expected(0, 0), 1e-5); - EXPECT_NEAR(P_predict(1, 1), P_predict_expected(1, 1), 1e-5); - EXPECT_NEAR(P_predict(2, 2), P_predict_expected(2, 2), 1e-5); - - // Define update parameters - Eigen::MatrixXd C_t(3, 3); - C_t << 0.5, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.5; - Eigen::MatrixXd R_t(3, 3); - R_t << 0.001, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.003; - Eigen::MatrixXd y_t(3, 1); - y_t << 1.05, 2.05, 3.05; - const int delay_step = 2; // Choose an appropriate delay step - const int dim_y = y_t.rows(); - - // Perform update - EXPECT_TRUE(td_kf_.updateWithDelay(y_t, C_t, R_t, delay_step)); - - // Check the updated state and covariance matrix - Eigen::MatrixXd x_update = td_kf_.getLatestX(); - Eigen::MatrixXd P_update = td_kf_.getLatestP(); - - Eigen::MatrixXd C_ex_t = Eigen::MatrixXd::Zero(dim_y, dim_x_ex); - const Eigen::MatrixXd PCT_t = P_ex_t * C_ex_t.transpose(); - const Eigen::MatrixXd K_t = PCT_t * ((R_t + C_ex_t * PCT_t).inverse()); - const Eigen::MatrixXd y_pred = C_ex_t * x_ex_t; - x_ex_t = x_ex_t + K_t * (y_t - y_pred); - P_ex_t = P_ex_t - K_t * (C_ex_t * P_ex_t); - Eigen::MatrixXd x_update_expected = x_ex_t.block(0, 0, dim_x, 1); - Eigen::MatrixXd P_update_expected = P_ex_t.block(0, 0, dim_x, dim_x); - EXPECT_NEAR(x_update(0, 0), x_update_expected(0, 0), 1e-5); - EXPECT_NEAR(x_update(1, 0), x_update_expected(1, 0), 1e-5); - EXPECT_NEAR(x_update(2, 0), x_update_expected(2, 0), 1e-5); - EXPECT_NEAR(P_update(0, 0), P_update_expected(0, 0), 1e-5); - EXPECT_NEAR(P_update(1, 1), P_update_expected(1, 1), 1e-5); - EXPECT_NEAR(P_update(2, 2), P_update_expected(2, 2), 1e-5); -} diff --git a/common/autoware_motion_utils/CHANGELOG.rst b/common/autoware_motion_utils/CHANGELOG.rst index 187ddda4acd39..46ebcdb1054e3 100644 --- a/common/autoware_motion_utils/CHANGELOG.rst +++ b/common/autoware_motion_utils/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package autoware_motion_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(motion_utils): add detail and pass type to VirtualWall (`#9940 `_) +* fix(autoware_motion_utils): remove clang compiler error (`#9713 `_) +* feat(motion_utils): add planning factor interface (`#9676 `_) + * feat(motion_utils): add planning factor interface + * fix: use extern template + * fix: define function in header + --------- +* Contributors: Fumiya Watanabe, Mamoru Sobue, Ryuta Kambe, Satoshi OTA + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp deleted file mode 100644 index 5b701a73ac543..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/steering_factor_interface.hpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef AUTOWARE__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ -#define AUTOWARE__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ - -#include -#include -#include - -#include - -namespace autoware::motion_utils -{ - -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::SteeringFactor; -using SteeringFactorBehavior = SteeringFactor::_behavior_type; -using SteeringFactorStatus = SteeringFactor::_status_type; -using geometry_msgs::msg::Pose; - -class SteeringFactorInterface -{ -public: - [[nodiscard]] SteeringFactor get() const { return steering_factor_; } - void init(const SteeringFactorBehavior & behavior) { behavior_ = behavior; } - void reset() { steering_factor_.behavior = PlanningBehavior::UNKNOWN; } - - void set( - const std::array & pose, const std::array distance, - const uint16_t direction, const uint16_t status, const std::string & detail = ""); - -private: - SteeringFactorBehavior behavior_{SteeringFactor::UNKNOWN}; - SteeringFactor steering_factor_{}; -}; - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__FACTOR__STEERING_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp deleted file mode 100644 index 2fcde52ec7c81..0000000000000 --- a/common/autoware_motion_utils/include/autoware/motion_utils/factor/velocity_factor_interface.hpp +++ /dev/null @@ -1,57 +0,0 @@ - -// Copyright 2022-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__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ -#define AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ - -#include -#include -#include -#include - -#include -#include - -namespace autoware::motion_utils -{ -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using VelocityFactorBehavior = VelocityFactor::_behavior_type; -using VelocityFactorStatus = VelocityFactor::_status_type; -using geometry_msgs::msg::Pose; - -class VelocityFactorInterface -{ -public: - [[nodiscard]] VelocityFactor get() const { return velocity_factor_; } - void init(const VelocityFactorBehavior & behavior) { behavior_ = behavior; } - void reset() { velocity_factor_.behavior = PlanningBehavior::UNKNOWN; } - - template - void set( - const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, - const VelocityFactorStatus status, const std::string & detail = ""); - - void set( - const double & distance, const VelocityFactorStatus & status, const std::string & detail = ""); - -private: - VelocityFactorBehavior behavior_{VelocityFactor::UNKNOWN}; - VelocityFactor velocity_factor_{}; -}; - -} // namespace autoware::motion_utils - -#endif // AUTOWARE__MOTION_UTILS__FACTOR__VELOCITY_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp index ee642c93a378f..2b89fc19d1878 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/marker_helper.hpp @@ -40,6 +40,11 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", const bool is_driving_forward = true); +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward); + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp index 07fcbd9840c88..fd86c54d65be9 100644 --- a/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/autoware_motion_utils/include/autoware/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -29,12 +29,13 @@ namespace autoware::motion_utils { /// @brief type of virtual wall associated with different marker styles and namespace -enum VirtualWallType { stop, slowdown, deadline }; +enum VirtualWallType { stop, slowdown, deadline, pass }; /// @brief virtual wall to be visualized in rviz struct VirtualWall { geometry_msgs::msg::Pose pose{}; std::string text{}; + std::string detail{}; std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; diff --git a/common/autoware_motion_utils/package.xml b/common/autoware_motion_utils/package.xml index 029985a53f020..75dc56de5466d 100644 --- a/common/autoware_motion_utils/package.xml +++ b/common/autoware_motion_utils/package.xml @@ -2,7 +2,7 @@ autoware_motion_utils - 0.40.0 + 0.41.0 The autoware_motion_utils package Satoshi Ota Takayuki Murooka diff --git a/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp b/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp deleted file mode 100644 index 7f12e2972ec86..0000000000000 --- a/common/autoware_motion_utils/src/factor/steering_factor_interface.cpp +++ /dev/null @@ -1,34 +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 - -#include - -namespace autoware::motion_utils -{ -void SteeringFactorInterface::set( - const std::array & pose, const std::array distance, const uint16_t direction, - const uint16_t status, const std::string & detail) -{ - steering_factor_.pose = pose; - std::array converted_distance{0.0, 0.0}; - for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast(distance[i]); - steering_factor_.distance = converted_distance; - steering_factor_.behavior = behavior_; - steering_factor_.direction = direction; - steering_factor_.status = status; - steering_factor_.detail = detail; -} -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp b/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp deleted file mode 100644 index cfa3c91eac43e..0000000000000 --- a/common/autoware_motion_utils/src/factor/velocity_factor_interface.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2023-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 -#include - -#include -#include -#include - -#include -#include - -namespace autoware::motion_utils -{ -template -void VelocityFactorInterface::set( - const std::vector & points, const Pose & curr_pose, const Pose & stop_pose, - const VelocityFactorStatus status, const std::string & detail) -{ - const auto & curr_point = curr_pose.position; - const auto & stop_point = stop_pose.position; - velocity_factor_.behavior = behavior_; - velocity_factor_.pose = stop_pose; - velocity_factor_.distance = - static_cast(autoware::motion_utils::calcSignedArcLength(points, curr_point, stop_point)); - velocity_factor_.status = status; - velocity_factor_.detail = detail; -} - -void VelocityFactorInterface::set( - const double & distance, const VelocityFactorStatus & status, const std::string & detail) -{ - velocity_factor_.behavior = behavior_; - velocity_factor_.distance = static_cast(distance); - velocity_factor_.status = status; - velocity_factor_.detail = detail; -} - -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, - const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, - const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, - const VelocityFactorStatus, const std::string &); - -} // namespace autoware::motion_utils diff --git a/common/autoware_motion_utils/src/marker/marker_helper.cpp b/common/autoware_motion_utils/src/marker/marker_helper.cpp index 91ad1c41eecc3..44b621a53c5f1 100644 --- a/common/autoware_motion_utils/src/marker/marker_helper.cpp +++ b/common/autoware_motion_utils/src/marker/marker_helper.cpp @@ -54,7 +54,7 @@ inline visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray( { auto marker = createDefaultMarker( "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + createMarkerScale(0.0, 0.0, 1.0 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); marker.pose = vehicle_front_pose; marker.pose.position.z += 2.0; @@ -85,6 +85,41 @@ inline visualization_msgs::msg::MarkerArray createDeletedVirtualWallMarkerArray( return marker_array; } + +inline visualization_msgs::msg::MarkerArray createIntendedPassArrowMarkerArray( + const geometry_msgs::msg::Pose & vehicle_front_pose, const std::string & module_name, + const std::string & ns_prefix, const rclcpp::Time & now, const int32_t id, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::MarkerArray marker_array; + + // Arrow + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "direction", id, visualization_msgs::msg::Marker::ARROW, + createMarkerScale(2.5 /*length*/, 0.15 /*width*/, 1.0 /*height*/), color); + + marker.pose = vehicle_front_pose; + + marker_array.markers.push_back(marker); + } + + // Factor Text + { + auto marker = createDefaultMarker( + "map", now, ns_prefix + "factor_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.4 /*font size*/), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + + marker.pose = vehicle_front_pose; + marker.pose.position.z += 2.0; + marker.text = module_name; + + marker_array.markers.push_back(marker); + } + + return marker_array; +} + } // namespace namespace autoware::motion_utils @@ -125,6 +160,18 @@ visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( createMarkerColor(0.0, 1.0, 0.0, 0.5)); } +visualization_msgs::msg::MarkerArray createIntendedPassVirtualMarker( + const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) +{ + const auto pose_with_offset = autoware::universe_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + return createIntendedPassArrowMarkerArray( + pose_with_offset, module_name, ns_prefix + "intended_pass_", now, id, + createMarkerColor(0.77, 0.77, 0.77, 0.5)); +} + visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id) { diff --git a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp index 4fecaea1bb838..fb8708b5b7baa 100644 --- a/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/autoware_motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -63,10 +63,16 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( case deadline: create_fn = autoware::motion_utils::createDeadLineVirtualWallMarker; break; + case pass: + create_fn = autoware::motion_utils::createIntendedPassVirtualMarker; + break; } + const auto wall_text = virtual_wall.detail.empty() + ? virtual_wall.text + : virtual_wall.text + "(" + virtual_wall.detail + ")"; auto markers = create_fn( - virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns, virtual_wall.is_driving_forward); + virtual_wall.pose, wall_text, now, 0, virtual_wall.longitudinal_offset, virtual_wall.ns, + virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = static_cast(marker_count_per_namespace_[marker.ns].current++); marker_array.markers.push_back(marker); diff --git a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index dc828e885af64..d00053a7b4ff6 100644 --- a/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/autoware_motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -26,8 +26,6 @@ using autoware::universe_utils::createPoint; using autoware::universe_utils::createQuaternionFromRPY; using autoware_planning_msgs::msg::Trajectory; -constexpr double epsilon = 1e-6; - geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) { diff --git a/common/autoware_object_recognition_utils/CHANGELOG.rst b/common/autoware_object_recognition_utils/CHANGELOG.rst index 920106de22082..20f538b7c3e8e 100644 --- a/common/autoware_object_recognition_utils/CHANGELOG.rst +++ b/common/autoware_object_recognition_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_object_recognition_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_object_recognition_utils/package.xml b/common/autoware_object_recognition_utils/package.xml index 05327766008f2..1f0a28c0db3f7 100644 --- a/common/autoware_object_recognition_utils/package.xml +++ b/common/autoware_object_recognition_utils/package.xml @@ -2,7 +2,7 @@ autoware_object_recognition_utils - 0.40.0 + 0.41.0 The autoware_object_recognition_utils package Takayuki Murooka Shunsuke Miura diff --git a/common/autoware_osqp_interface/CHANGELOG.rst b/common/autoware_osqp_interface/CHANGELOG.rst deleted file mode 100644 index d400b77f4bf61..0000000000000 --- a/common/autoware_osqp_interface/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_osqp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/common/autoware_osqp_interface/CMakeLists.txt b/common/autoware_osqp_interface/CMakeLists.txt deleted file mode 100644 index b770af659e52a..0000000000000 --- a/common/autoware_osqp_interface/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_osqp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(OSQP_INTERFACE_LIB_SRC - src/csc_matrix_conv.cpp - src/osqp_interface.cpp -) - -set(OSQP_INTERFACE_LIB_HEADERS - include/autoware/osqp_interface/csc_matrix_conv.hpp - include/autoware/osqp_interface/osqp_interface.hpp - include/autoware/osqp_interface/visibility_control.hpp -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${OSQP_INTERFACE_LIB_SRC} - ${OSQP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor -) - -# crucial so downstream package builds because autoware_osqp_interface exposes osqp.hpp -ament_export_include_directories("${OSQP_INCLUDE_DIR}") -# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a -ament_export_libraries(osqp::osqp) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_osqp_interface/design/osqp_interface-design.md b/common/autoware_osqp_interface/design/osqp_interface-design.md deleted file mode 100644 index 887a4b4d9af3f..0000000000000 --- a/common/autoware_osqp_interface/design/osqp_interface-design.md +++ /dev/null @@ -1,70 +0,0 @@ -# Interface for the OSQP library - -This is the design document for the `autoware_osqp_interface` package. - -## Purpose / Use cases - - - - -This packages provides a C++ interface for the [OSQP library](https://osqp.org/docs/solver/index.html). - -## Design - - - - -The class `OSQPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the OSQP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - - - - -The interface can be used in several ways: - -1. Initialize the interface WITHOUT data. Load the problem formulation at the optimization call. - - ```cpp - osqp_interface = OSQPInterface(); - osqp_interface.optimize(P, A, q, l, u); - ``` - -2. Initialize the interface WITH data. - - ```cpp - osqp_interface = OSQPInterface(P, A, q, l, u); - osqp_interface.optimize(); - ``` - -3. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - osqp_interface = OSQPInterface(P, A, q, l, u); - osqp_interface.optimize(); - osqp.initializeProblem(P_new, A_new, q_new, l_new, u_new); - osqp_interface.optimize(); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - std::tuple, std::vector> result = osqp_interface.optimize(); - std::vector param = std::get<0>(result); - double x_0 = param[0]; - double x_1 = param[1]; - ``` - -## References / External links - - - -- OSQP library: - -## Related issues - - diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp deleted file mode 100644 index 8c21553152d70..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/csc_matrix_conv.hpp +++ /dev/null @@ -1,47 +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 AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ -#define AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ - -#include "autoware/osqp_interface/visibility_control.hpp" -#include "osqp/glob_opts.h" // for 'c_int' type ('long' or 'long long') - -#include - -#include - -namespace autoware::osqp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct OSQP_INTERFACE_PUBLIC CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector m_vals; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector m_row_idxs; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector m_col_idxs; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -OSQP_INTERFACE_PUBLIC CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); -/// \brief Print the given CSC matrix to the standard output -OSQP_INTERFACE_PUBLIC void printCSCMatrix(const CSC_Matrix & csc_mat); - -} // namespace autoware::osqp_interface - -#endif // AUTOWARE__OSQP_INTERFACE__CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp deleted file mode 100644 index ff3013bd61514..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/osqp_interface.hpp +++ /dev/null @@ -1,194 +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 AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ -#define AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "autoware/osqp_interface/visibility_control.hpp" -#include "osqp/osqp.h" - -#include -#include - -#include -#include -#include -#include -#include - -namespace autoware::osqp_interface -{ -constexpr c_float INF = 1e30; - -/** - * Implementation of a native C++ interface for the OSQP solver. - * - */ -class OSQP_INTERFACE_PUBLIC OSQPInterface -{ -private: - std::unique_ptr> m_work; - std::unique_ptr m_settings; - std::unique_ptr m_data; - // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo m_latest_work_info; - // Number of parameters to optimize - int64_t m_param_n; - // Flag to check if the current work exists - bool m_work_initialized = false; - // Exitflag - int64_t m_exitflag; - - // Runs the solver on the stored problem. - std::tuple, std::vector, int64_t, int64_t, int64_t> solve(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - -public: - /// \brief Constructor without problem formulation - explicit OSQPInterface( - const c_float eps_abs = std::numeric_limits::epsilon(), const bool polish = true); - /// \brief Constructor with problem setup - /// \param P: (n,n) matrix defining relations between parameters. - /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs); - ~OSQPInterface(); - - /**************** - * OPTIMIZATION - ****************/ - /// \brief Solves the stored convex quadratic program (QP) problem using the OSQP solver. - // - /// \return The function returns a tuple containing the solution as two float vectors. - /// \return The first element of the tuple contains the 'primal' solution. - /// \return The second element contains the 'lagrange multiplier' solution. - /// \return The third element contains an integer with solver polish status information. - - /// \details How to use: - /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - /// \details 2. Initialize the interface and set up the problem. - /// \details osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6); - /// \details 3. Call the optimization function. - /// \details std::tuple, std::vector> result; - /// \details result = osqp_interface.optimize(); - /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); - /// \details double x_0 = param[0]; - /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize(); - - /// \brief Solves convex quadratic programs (QPs) using the OSQP solver. - /// \return The function returns a tuple containing the solution as two float vectors. - /// \return The first element of the tuple contains the 'primal' solution. - /// \return The second element contains the 'lagrange multiplier' solution. - /// \return The third element contains an integer with solver polish status information. - /// \details How to use: - /// \details 1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem. - /// \details 2. Initialize the interface. - /// \details osqp_interface = OSQPInterface(1e-6); - /// \details 3. Call the optimization function with the problem formulation. - /// \details std::tuple, std::vector> result; - /// \details result = osqp_interface.optimize(P, A, q, l, u, 1e-6); - /// \details 4. Access the optimized parameters. - /// \details std::vector param = std::get<0>(result); - /// \details double x_0 = param[0]; - /// \details double x_1 = param[1]; - std::tuple, std::vector, int64_t, int64_t, int64_t> optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - /// \brief Converts the input data and sets up the workspace object. - /// \param P (n,n) matrix defining relations between parameters. - /// \param A (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q (n) vector defining the linear cost of the problem. - /// \param l (m) vector defining the lower bound problem constraint. - /// \param u (m) vector defining the upper bound problem constraint. - int64_t initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - int64_t initializeProblem( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - void updateEpsAbs(const double eps_abs); - void updateEpsRel(const double eps_rel); - void updateMaxIter(const int iter); - void updateVerbose(const bool verbose); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - void updateCheckTermination(const int check_termination); - - /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(m_latest_work_info.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(m_latest_work_info.status); - } - /// \brief Get the status value for the latest problem solved - inline int64_t getStatus() const { return static_cast(m_latest_work_info.status_val); } - /// \brief Get the status polish for the latest problem solved - inline int64_t getStatusPolish() const - { - return static_cast(m_latest_work_info.status_polish); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return m_latest_work_info.run_time; } - /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return m_latest_work_info.obj_val; } - /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return m_exitflag; } - - void logUnsolvedStatus(const std::string & prefix_message = "") const; -}; - -} // namespace autoware::osqp_interface - -#endif // AUTOWARE__OSQP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp b/common/autoware_osqp_interface/include/autoware/osqp_interface/visibility_control.hpp deleted file mode 100644 index a6cdaa8a0a74e..0000000000000 --- a/common/autoware_osqp_interface/include/autoware/osqp_interface/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 AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ -#define AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ - -//////////////////////////////////////////////////////////////////////////////// -#if defined(__WIN32) -#if defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#define OSQP_INTERFACE_PUBLIC __declspec(dllexport) -#define OSQP_INTERFACE_LOCAL -#else // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#define OSQP_INTERFACE_PUBLIC __declspec(dllimport) -#define OSQP_INTERFACE_LOCAL -#endif // defined(OSQP_INTERFACE_BUILDING_DLL) || defined(OSQP_INTERFACE_EXPORTS) -#elif defined(__linux__) -#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) -#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) -#elif defined(__APPLE__) -#define OSQP_INTERFACE_PUBLIC __attribute__((visibility("default"))) -#define OSQP_INTERFACE_LOCAL __attribute__((visibility("hidden"))) -#else -#error "Unsupported Build Configuration" -#endif - -#endif // AUTOWARE__OSQP_INTERFACE__VISIBILITY_CONTROL_HPP_ diff --git a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp b/common/autoware_osqp_interface/src/csc_matrix_conv.cpp deleted file mode 100644 index c6e1a0a42d938..0000000000000 --- a/common/autoware_osqp_interface/src/csc_matrix_conv.cpp +++ /dev/null @@ -1,135 +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. - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::osqp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.m_vals) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.m_row_idxs) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.m_col_idxs) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} - -} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/src/osqp_interface.cpp b/common/autoware_osqp_interface/src/osqp_interface.cpp deleted file mode 100644 index ceeae626222ca..0000000000000 --- a/common/autoware_osqp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,435 +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. - -#include "autoware/osqp_interface/osqp_interface.hpp" - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::osqp_interface -{ -OSQPInterface::OSQPInterface(const c_float eps_abs, const bool polish) -: m_work{nullptr, OSQPWorkspaceDeleter} -{ - m_settings = std::make_unique(); - m_data = std::make_unique(); - - if (m_settings) { - osqp_set_default_settings(m_settings.get()); - m_settings->alpha = 1.6; // Change alpha parameter - m_settings->eps_rel = 1.0E-4; - m_settings->eps_abs = eps_abs; - m_settings->eps_prim_inf = 1.0E-4; - m_settings->eps_dual_inf = 1.0E-4; - m_settings->warm_start = true; - m_settings->max_iter = 4000; - m_settings->verbose = false; - m_settings->polish = polish; - } - m_exitflag = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const c_float eps_abs) -: OSQPInterface(eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (m_data->P) free(m_data->P); - if (m_data->A) free(m_data->A); -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); -} - -void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) -{ - osqp_update_P( - m_work.get(), P_csc.m_vals.data(), OSQP_NULL, static_cast(P_csc.m_vals.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); - return; -} - -void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) -{ - osqp_update_A( - m_work.get(), A_csc.m_vals.data(), OSQP_NULL, static_cast(A_csc.m_vals.size())); -} - -void OSQPInterface::updateQ(const std::vector & q_new) -{ - std::vector q_tmp(q_new.begin(), q_new.end()); - double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(m_work.get(), q_dyn); -} - -void OSQPInterface::updateL(const std::vector & l_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(m_work.get(), l_dyn); -} - -void OSQPInterface::updateU(const std::vector & u_new) -{ - std::vector u_tmp(u_new.begin(), u_new.end()); - double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(m_work.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - std::vector u_tmp(u_new.begin(), u_new.end()); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - osqp_update_bounds(m_work.get(), l_dyn, u_dyn); -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - m_settings->eps_abs = eps_abs; // for default setting - if (m_work_initialized) { - osqp_update_eps_abs(m_work.get(), eps_abs); // for current work - } -} - -void OSQPInterface::updateEpsRel(const double eps_rel) -{ - m_settings->eps_rel = eps_rel; // for default setting - if (m_work_initialized) { - osqp_update_eps_rel(m_work.get(), eps_rel); // for current work - } -} - -void OSQPInterface::updateMaxIter(const int max_iter) -{ - m_settings->max_iter = max_iter; // for default setting - if (m_work_initialized) { - osqp_update_max_iter(m_work.get(), max_iter); // for current work - } -} - -void OSQPInterface::updateVerbose(const bool is_verbose) -{ - m_settings->verbose = is_verbose; // for default setting - if (m_work_initialized) { - osqp_update_verbose(m_work.get(), is_verbose); // for current work - } -} - -void OSQPInterface::updateRhoInterval(const int rho_interval) -{ - m_settings->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - m_settings->rho = rho; - if (m_work_initialized) { - osqp_update_rho(m_work.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - m_settings->alpha = alpha; - if (m_work_initialized) { - osqp_update_alpha(m_work.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - m_settings->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - m_settings->polish = polish; - if (m_work_initialized) { - osqp_update_polish(m_work.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - m_settings->polish_refine_iter = polish_refine_iter; - if (m_work_initialized) { - osqp_update_polish_refine_iter(m_work.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - m_settings->check_termination = check_termination; - if (m_work_initialized) { - osqp_update_check_termination(m_work.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!m_work_initialized) { - return false; - } - - const auto result = osqp_warm_start_x(m_work.get(), primal_variables.data()); - if (result != 0) { - std::cerr << "Failed to set primal variables for warm start" << std::endl; - return false; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!m_work_initialized) { - return false; - } - - const auto result = osqp_warm_start_y(m_work.get(), dual_variables.data()); - if (result != 0) { - std::cerr << "Failed to set dual variables for warm start" << std::endl; - return false; - } - - return true; -} - -int64_t OSQPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - return initializeProblem(P_csc, A_csc, q, l, u); -} - -int64_t OSQPInterface::initializeProblem( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - m_param_n = static_cast(q.size()); - m_data->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - m_data->n = m_param_n; - if (m_data->P) free(m_data->P); - m_data->P = csc_matrix( - m_data->n, m_data->n, static_cast(P_csc.m_vals.size()), P_csc.m_vals.data(), - P_csc.m_row_idxs.data(), P_csc.m_col_idxs.data()); - m_data->q = q_dyn; - if (m_data->A) free(m_data->A); - m_data->A = csc_matrix( - m_data->m, m_data->n, static_cast(A_csc.m_vals.size()), A_csc.m_vals.data(), - A_csc.m_row_idxs.data(), A_csc.m_col_idxs.data()); - m_data->l = l_dyn; - m_data->u = u_dyn; - - // Setup workspace - OSQPWorkspace * workspace; - m_exitflag = osqp_setup(&workspace, m_data.get(), m_settings.get()); - m_work.reset(workspace); - m_work_initialized = true; - - return m_exitflag; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::solve() -{ - // Solve Problem - osqp_solve(m_work.get()); - - /******************** - * EXTRACT SOLUTION - ********************/ - double * sol_x = m_work->solution->x; - double * sol_y = m_work->solution->y; - std::vector sol_primal(sol_x, sol_x + m_param_n); - std::vector sol_lagrange_multiplier(sol_y, sol_y + m_data->m); - - int64_t status_polish = m_work->info->status_polish; - int64_t status_solution = m_work->info->status_val; - int64_t status_iteration = m_work->info->iter; - - // Result tuple - std::tuple, std::vector, int64_t, int64_t, int64_t> result = - std::make_tuple( - sol_primal, sol_lagrange_multiplier, status_polish, status_solution, status_iteration); - - m_latest_work_info = *(m_work->info); - - return result; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize() -{ - // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); - return result; -} - -std::tuple, std::vector, int64_t, int64_t, int64_t> -OSQPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // Allocate memory for problem - initializeProblem(P, A, q, l, u); - - // Run the solver on the stored problem representation. - std::tuple, std::vector, int64_t, int64_t, int64_t> result = solve(); - - m_work.reset(); - m_work_initialized = false; - - return result; -} - -void OSQPInterface::logUnsolvedStatus(const std::string & prefix_message) const -{ - const int status = getStatus(); - if (status == 1) { - // No need to log since optimization was solved. - return; - } - - // create message - std::string output_message = ""; - if (prefix_message != "") { - output_message = prefix_message + " "; - } - - const auto status_message = getStatusMessage(); - output_message += "Optimization failed due to " + status_message; - - // log with warning - RCLCPP_WARN(rclcpp::get_logger("osqp_interface"), "%s", output_message.c_str()); -} -} // namespace autoware::osqp_interface diff --git a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index a63f979a966bf..0000000000000 --- a/common/autoware_osqp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +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. - -#include "autoware/osqp_interface/csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.m_vals.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_vals[0], 1.0); - ASSERT_EQ(rect_m1.m_row_idxs.size(), size_t(1)); - EXPECT_EQ(rect_m1.m_row_idxs[0], c_int(0)); - ASSERT_EQ(rect_m1.m_col_idxs.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[1], c_int(0)); - EXPECT_EQ(rect_m1.m_col_idxs[2], c_int(1)); - - Eigen::MatrixXd rect2(2, 4); - rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; - - const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.m_vals.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_vals[0], 1.0); - EXPECT_EQ(rect_m2.m_vals[1], 6.0); - EXPECT_EQ(rect_m2.m_vals[2], 3.0); - EXPECT_EQ(rect_m2.m_vals[3], 7.0); - ASSERT_EQ(rect_m2.m_row_idxs.size(), size_t(4)); - EXPECT_EQ(rect_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_row_idxs[2], c_int(0)); - EXPECT_EQ(rect_m2.m_row_idxs[3], c_int(1)); - ASSERT_EQ(rect_m2.m_col_idxs.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(rect_m2.m_col_idxs[1], c_int(1)); - EXPECT_EQ(rect_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(rect_m2.m_col_idxs[3], c_int(4)); - EXPECT_EQ(rect_m2.m_col_idxs[4], c_int(4)); - - // Example from http://netlib.org/linalg/html_templates/node92.html - Eigen::MatrixXd square2(6, 6); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; - - const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(19)); - EXPECT_EQ(square_m2.m_vals[0], 10.0); - EXPECT_EQ(square_m2.m_vals[1], 3.0); - EXPECT_EQ(square_m2.m_vals[2], 3.0); - EXPECT_EQ(square_m2.m_vals[3], 9.0); - EXPECT_EQ(square_m2.m_vals[4], 7.0); - EXPECT_EQ(square_m2.m_vals[5], 8.0); - EXPECT_EQ(square_m2.m_vals[6], 4.0); - EXPECT_EQ(square_m2.m_vals[7], 8.0); - EXPECT_EQ(square_m2.m_vals[8], 8.0); - EXPECT_EQ(square_m2.m_vals[9], 7.0); - EXPECT_EQ(square_m2.m_vals[10], 7.0); - EXPECT_EQ(square_m2.m_vals[11], 9.0); - EXPECT_EQ(square_m2.m_vals[12], -2.0); - EXPECT_EQ(square_m2.m_vals[13], 5.0); - EXPECT_EQ(square_m2.m_vals[14], 9.0); - EXPECT_EQ(square_m2.m_vals[15], 2.0); - EXPECT_EQ(square_m2.m_vals[16], 3.0); - EXPECT_EQ(square_m2.m_vals[17], 13.0); - EXPECT_EQ(square_m2.m_vals[18], -1.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(19)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[3], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[4], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[5], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[6], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[7], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[8], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[9], c_int(2)); - EXPECT_EQ(square_m2.m_row_idxs[10], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[11], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[12], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[13], c_int(3)); - EXPECT_EQ(square_m2.m_row_idxs[14], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[15], c_int(5)); - EXPECT_EQ(square_m2.m_row_idxs[16], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[17], c_int(4)); - EXPECT_EQ(square_m2.m_row_idxs[18], c_int(5)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(3)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(7)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(9)); - EXPECT_EQ(square_m2.m_col_idxs[4], c_int(12)); - EXPECT_EQ(square_m2.m_col_idxs[5], c_int(16)); - EXPECT_EQ(square_m2.m_col_idxs[6], c_int(19)); -} -TEST(TestCscMatrixConv, Trapezoidal) -{ - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m1.m_vals[0], 1.0); - EXPECT_EQ(square_m1.m_vals[1], 2.0); - EXPECT_EQ(square_m1.m_vals[2], 4.0); - ASSERT_EQ(square_m1.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[1], c_int(0)); - EXPECT_EQ(square_m1.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m1.m_col_idxs.size(), size_t(3)); - EXPECT_EQ(square_m1.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m1.m_col_idxs[1], c_int(1)); - EXPECT_EQ(square_m1.m_col_idxs[2], c_int(3)); - - const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.m_vals.size(), size_t(3)); - EXPECT_EQ(square_m2.m_vals[0], 2.0); - EXPECT_EQ(square_m2.m_vals[1], 5.0); - EXPECT_EQ(square_m2.m_vals[2], 6.0); - ASSERT_EQ(square_m2.m_row_idxs.size(), size_t(3)); - EXPECT_EQ(square_m2.m_row_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_row_idxs[1], c_int(1)); - EXPECT_EQ(square_m2.m_row_idxs[2], c_int(1)); - ASSERT_EQ(square_m2.m_col_idxs.size(), size_t(4)); - EXPECT_EQ(square_m2.m_col_idxs[0], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[1], c_int(0)); - EXPECT_EQ(square_m2.m_col_idxs[2], c_int(2)); - EXPECT_EQ(square_m2.m_col_idxs[3], c_int(3)); - - try { - const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - using autoware::osqp_interface::printCSCMatrix; - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_osqp_interface/test/test_osqp_interface.cpp b/common/autoware_osqp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f65b07e6d792d..0000000000000 --- a/common/autoware_osqp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,164 +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. - -#include "autoware/osqp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - using autoware::osqp_interface::calCSCMatrix; - using autoware::osqp_interface::calCSCMatrixTrapezoidal; - using autoware::osqp_interface::CSC_Matrix; - - auto check_result = - [](const std::tuple, std::vector, int, int, int> & result) { - EXPECT_EQ(std::get<2>(result), 1); // polish succeeded - EXPECT_EQ(std::get<3>(result), 1); // solution succeeded - - static const auto ep = 1.0e-8; - - const auto prime_val = std::get<0>(result); - ASSERT_EQ(prime_val.size(), size_t(2)); - EXPECT_NEAR(prime_val[0], 0.3, ep); - EXPECT_NEAR(prime_val[1], 0.7, ep); - - const auto dual_val = std::get<1>(result); - ASSERT_EQ(dual_val.size(), size_t(4)); - EXPECT_NEAR(dual_val[0], -2.9, ep); - EXPECT_NEAR(dual_val[1], 0.0, ep); - EXPECT_NEAR(dual_val[2], 0.2, ep); - EXPECT_NEAR(dual_val[3], 0.0, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::osqp_interface::INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::osqp_interface::INF}; - - { - // Define problem during optimization - autoware::osqp_interface::OSQPInterface osqp; - std::tuple, std::vector, int, int, int> result = - osqp.optimize(P, A, q, l, u); - check_result(result); - } - - { - // Define problem during initialization - autoware::osqp_interface::OSQPInterface osqp(P, A, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = osqp.optimize(); - check_result(result); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini, A_ini, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - osqp.initializeProblem(P, A, q, l, u); - result = osqp.optimize(); - check_result(result); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - autoware::osqp_interface::OSQPInterface osqp(P_csc, A_csc, q, l, u, 1e-6); - std::tuple, std::vector, int, int, int> result = osqp.optimize(); - check_result(result); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - osqp.initializeProblem(P_csc, A_csc, q, l, u); - result = osqp.optimize(); - check_result(result); - } - - // add warm startup - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::osqp_interface::OSQPInterface osqp(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini, 1e-6); - osqp.optimize(); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - osqp.initializeProblem(P_csc, A_csc, q, l, u); - result = osqp.optimize(); - check_result(result); - - osqp.updateCheckTermination(1); - const auto primal_val = std::get<0>(result); - const auto dual_val = std::get<1>(result); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - result = osqp.optimize(); - check_result(result); - EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace diff --git a/common/autoware_path_distance_calculator/CHANGELOG.rst b/common/autoware_path_distance_calculator/CHANGELOG.rst index cd4c6ae5ce49b..d5121521c3acf 100644 --- a/common/autoware_path_distance_calculator/CHANGELOG.rst +++ b/common/autoware_path_distance_calculator/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_path_distance_calculator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/common/autoware_path_distance_calculator/package.xml b/common/autoware_path_distance_calculator/package.xml index 6d207f6ed795d..150ca10754e24 100644 --- a/common/autoware_path_distance_calculator/package.xml +++ b/common/autoware_path_distance_calculator/package.xml @@ -2,7 +2,7 @@ autoware_path_distance_calculator - 0.40.0 + 0.41.0 The autoware_path_distance_calculator package Takagi, Isamu Apache License 2.0 diff --git a/common/autoware_point_types/CHANGELOG.rst b/common/autoware_point_types/CHANGELOG.rst deleted file mode 100644 index 5e26002677a36..0000000000000 --- a/common/autoware_point_types/CHANGELOG.rst +++ /dev/null @@ -1,150 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_point_types -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(autoware_point_types): prefix namespace with autoware::point_types (`#9169 `_) -* feat: migrating pointcloud types (`#6996 `_) - * feat: changed most of sensing to the new type - * chore: started applying changes to the perception stack - * feat: confirmed operation until centerpoint - * feat: reverted to the original implementation of pointpainting - * chore: forgot to push a header - * feat: also implemented the changes for the subsample filters that were out of scope before - * fix: some point type changes were missing from the latest merge from main - * chore: removed unused code, added comments, and brought back a removed publish - * chore: replaced pointcloud_raw for pointcloud_raw_ex to avoid extra processing time in the drivers - * feat: added memory layout checks - * chore: updated documentation regarding the point types - * chore: added hyperlinks to the point definitions. will be valid only once the PR is merged - * fix: fixed compilation due to moving the utilities files to the base library - * chore: separated the utilities functions due to a dependency issue - * chore: forgot that perception also uses the filter class - * feature: adapted the undistortion tests to the new point type - --------- - Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> - Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> -* chore: updated maintainers for the autoware_point_types package (`#7797 `_) -* docs(common): adding .pages file (`#7148 `_) - * docs(common): adding .pages file - * fix naming - * fix naming - * fix naming - * include main page plus explanation to autoware tools - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Esteve Fernandez, Kenzo Lobos Tsunekawa, Yutaka Kondo, Zulfaqar Azmi - -0.26.0 (2024-04-03) -------------------- -* build: mark autoware_cmake as (`#3616 `_) - * build: mark autoware_cmake as - with , autoware_cmake is automatically exported with ament_target_dependencies() (unecessary) - * style(pre-commit): autofix - * chore: fix pre-commit errors - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: Kenji Miyake -* feat: isolate gtests in all packages (`#693 `_) -* chore: upgrade cmake_minimum_required to 3.14 (`#856 `_) -* refactor: use autoware cmake (`#849 `_) - * remove autoware_auto_cmake - * add build_depend of autoware_cmake - * use autoware_cmake in CMakeLists.txt - * fix bugs - * fix cmake lint errors -* feat: add blockage diagnostics (`#461 `_) - * feat!: add blockage diagnostic - * fix: typo - * docs: add documentation - * ci(pre-commit): autofix - * fix: typo - * ci(pre-commit): autofix - * fix: typo - * chore: add adjustable param - * ci(pre-commit): autofix - * feat!: add blockage diagnostic - * fix: typo - * docs: add documentation - * ci(pre-commit): autofix - * fix: typo - * ci(pre-commit): autofix - * fix: typo - * chore: add adjustable param - * ci(pre-commit): autofix - * chore: rearrange header file - * chore: fix typo - * chore: rearrange header - * fix: revert accident change - * chore: fix typo - * docs: add limits - * chore: check overflow - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* ci: check include guard (`#438 `_) - * ci: check include guard - * apply pre-commit - * Update .pre-commit-config.yaml - Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> - * fix: pre-commit - Co-authored-by: Kenji Miyake - Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> -* feat: add point_types for wrapper (`#784 `_) (`#215 `_) - * add point_types - * Revert "add point_types" - This reverts commit 5810000cd1cbd876bc22372e2bb74ccaca06187b. - * create autoware_point_types pkg - * add include - * add cmath - * fix author - * fix bug - * define epsilon as argument - * add test - * remove unnamed namespace - * update test - * fix test name - * use std::max - * change comparison method - * remove unnencessary line - * fix test - * fix comparison method name - Co-authored-by: Taichi Higashide -* Contributors: Daisuke Nishimatsu, Kenji Miyake, Maxime CLEMENT, Takagi, Isamu, Vincent Richard, badai nguyen diff --git a/common/autoware_point_types/CMakeLists.txt b/common/autoware_point_types/CMakeLists.txt deleted file mode 100644 index c149f79dab71f..0000000000000 --- a/common/autoware_point_types/CMakeLists.txt +++ /dev/null @@ -1,25 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_point_types) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -include_directories( - include - SYSTEM - ${PCL_INCLUDE_DIRS} -) - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_autoware_point_types - test/test_point_types.cpp - ) - target_include_directories(test_autoware_point_types - PRIVATE include - ) - ament_target_dependencies(test_autoware_point_types - point_cloud_msg_wrapper - ) -endif() - -ament_auto_package() diff --git a/common/autoware_point_types/README.md b/common/autoware_point_types/README.md deleted file mode 100644 index 92f19d2bc353a..0000000000000 --- a/common/autoware_point_types/README.md +++ /dev/null @@ -1 +0,0 @@ -# Autoware Point Types diff --git a/common/autoware_point_types/include/autoware/point_types/types.hpp b/common/autoware_point_types/include/autoware/point_types/types.hpp deleted file mode 100644 index 0fde62222e276..0000000000000 --- a/common/autoware_point_types/include/autoware/point_types/types.hpp +++ /dev/null @@ -1,188 +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. - -#ifndef AUTOWARE__POINT_TYPES__TYPES_HPP_ -#define AUTOWARE__POINT_TYPES__TYPES_HPP_ - -#include - -#include - -#include -#include - -namespace autoware::point_types -{ -template -bool float_eq(const T a, const T b, const T eps = 10e-6) -{ - return std::fabs(a - b) < eps; -} - -struct PointXYZI -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - float intensity{0.0F}; - friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity); - } -}; - -enum ReturnType : uint8_t { - INVALID = 0, - SINGLE_STRONGEST, - SINGLE_LAST, - DUAL_STRONGEST_FIRST, - DUAL_STRONGEST_LAST, - DUAL_WEAK_FIRST, - DUAL_WEAK_LAST, - DUAL_ONLY, -}; - -struct PointXYZIRC -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - std::uint8_t intensity{0U}; - std::uint8_t return_type{0U}; - std::uint16_t channel{0U}; - - friend bool operator==(const PointXYZIRC & p1, const PointXYZIRC & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && - p1.return_type == p2.return_type && p1.channel == p2.channel; - } -}; - -struct PointXYZIRADRT -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - float intensity{0.0F}; - uint16_t ring{0U}; - float azimuth{0.0F}; - float distance{0.0F}; - uint8_t return_type{0U}; - double time_stamp{0.0}; - friend bool operator==(const PointXYZIRADRT & p1, const PointXYZIRADRT & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && float_eq(p1.intensity, p2.intensity) && - p1.ring == p2.ring && float_eq(p1.azimuth, p2.azimuth) && - float_eq(p1.distance, p2.distance) && p1.return_type == p2.return_type && - float_eq(p1.time_stamp, p2.time_stamp); - } -}; - -struct PointXYZIRCAEDT -{ - float x{0.0F}; - float y{0.0F}; - float z{0.0F}; - std::uint8_t intensity{0U}; - std::uint8_t return_type{0U}; - std::uint16_t channel{0U}; - float azimuth{0.0F}; - float elevation{0.0F}; - float distance{0.0F}; - std::uint32_t time_stamp{0U}; - - friend bool operator==(const PointXYZIRCAEDT & p1, const PointXYZIRCAEDT & p2) noexcept - { - return float_eq(p1.x, p2.x) && float_eq(p1.y, p2.y) && - float_eq(p1.z, p2.z) && p1.intensity == p2.intensity && - p1.return_type == p2.return_type && p1.channel == p2.channel && - float_eq(p1.azimuth, p2.azimuth) && float_eq(p1.distance, p2.distance) && - p1.time_stamp == p2.time_stamp; - } -}; - -enum class PointXYZIIndex { X, Y, Z, Intensity }; -enum class PointXYZIRCIndex { X, Y, Z, Intensity, ReturnType, Channel }; -enum class PointXYZIRADRTIndex { - X, - Y, - Z, - Intensity, - Ring, - Azimuth, - Distance, - ReturnType, - TimeStamp -}; -enum class PointXYZIRCAEDTIndex { - X, - Y, - Z, - Intensity, - ReturnType, - Channel, - Azimuth, - Elevation, - Distance, - TimeStamp -}; - -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(azimuth); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(elevation); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(distance); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(return_type); -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(time_stamp); - -LIDAR_UTILS__DEFINE_FIELD_GENERATOR_FOR_MEMBER(channel); - -using PointXYZIRCGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - field_return_type_generator, field_channel_generator>; - -using PointXYZIRADRTGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - point_cloud_msg_wrapper::field_ring_generator, field_azimuth_generator, field_distance_generator, - field_return_type_generator, field_time_stamp_generator>; - -using PointXYZIRCAEDTGenerator = std::tuple< - point_cloud_msg_wrapper::field_x_generator, point_cloud_msg_wrapper::field_y_generator, - point_cloud_msg_wrapper::field_z_generator, point_cloud_msg_wrapper::field_intensity_generator, - field_return_type_generator, field_channel_generator, field_azimuth_generator, - field_elevation_generator, field_distance_generator, field_time_stamp_generator>; - -} // namespace autoware::point_types - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRC, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( - std::uint8_t, return_type, return_type)(std::uint16_t, channel, channel)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRADRT, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(std::uint16_t, ring, ring)( - float, azimuth, azimuth)(float, distance, distance)(std::uint8_t, return_type, return_type)( - double, time_stamp, time_stamp)) - -POINT_CLOUD_REGISTER_POINT_STRUCT( - autoware::point_types::PointXYZIRCAEDT, - (float, x, x)(float, y, y)(float, z, z)(std::uint8_t, intensity, intensity)( - std::uint8_t, return_type, - return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( - float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) -#endif // AUTOWARE__POINT_TYPES__TYPES_HPP_ diff --git a/common/autoware_point_types/package.xml b/common/autoware_point_types/package.xml deleted file mode 100644 index e35e6a63de648..0000000000000 --- a/common/autoware_point_types/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - autoware_point_types - 0.40.0 - The point types definition to use point_cloud_msg_wrapper - David Wong - Max Schmeller - Apache License 2.0 - - ament_cmake_core - ament_cmake_export_dependencies - ament_cmake_test - autoware_cmake - - ament_cmake_core - ament_cmake_test - - ament_cmake_copyright - ament_cmake_cppcheck - ament_cmake_lint_cmake - ament_cmake_xmllint - pcl_ros - point_cloud_msg_wrapper - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - point_cloud_msg_wrapper - - - ament_cmake - - diff --git a/common/autoware_point_types/test/test_point_types.cpp b/common/autoware_point_types/test/test_point_types.cpp deleted file mode 100644 index 08696a9948f60..0000000000000 --- a/common/autoware_point_types/test/test_point_types.cpp +++ /dev/null @@ -1,66 +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 "autoware/point_types/types.hpp" - -#include - -#include - -TEST(PointEquality, PointXYZI) -{ - using autoware::point_types::PointXYZI; - - PointXYZI pt0{0, 1, 2, 3}; - PointXYZI pt1{0, 1, 2, 3}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, PointXYZIRADRT) -{ - using autoware::point_types::PointXYZIRADRT; - - PointXYZIRADRT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8}; - PointXYZIRADRT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, PointXYZIRCAEDT) -{ - using autoware::point_types::PointXYZIRCAEDT; - - PointXYZIRCAEDT pt0{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; - PointXYZIRCAEDT pt1{0, 1, 2, 3, 4, 5, 6, 7, 8, 9}; - EXPECT_EQ(pt0, pt1); -} - -TEST(PointEquality, FloatEq) -{ - // test template - EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); - EXPECT_TRUE(autoware::point_types::float_eq(1, 1)); - - // test floating point error - EXPECT_TRUE(autoware::point_types::float_eq(1, 1 + std::numeric_limits::epsilon())); - - // test difference of sign - EXPECT_FALSE(autoware::point_types::float_eq(2, -2)); - EXPECT_FALSE(autoware::point_types::float_eq(-2, 2)); - - // small value difference - EXPECT_FALSE(autoware::point_types::float_eq(2, 2 + 10e-6)); - - // expect same value if epsilon is larger than difference - EXPECT_TRUE(autoware::point_types::float_eq(2, 2 + 10e-6, 10e-5)); -} diff --git a/common/autoware_polar_grid/CHANGELOG.rst b/common/autoware_polar_grid/CHANGELOG.rst index 72a53e6cef4b7..9f4396678ab12 100644 --- a/common/autoware_polar_grid/CHANGELOG.rst +++ b/common/autoware_polar_grid/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_polar_grid ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/common/autoware_polar_grid/package.xml b/common/autoware_polar_grid/package.xml index 16d6a72985ec9..c7ab2e8252965 100644 --- a/common/autoware_polar_grid/package.xml +++ b/common/autoware_polar_grid/package.xml @@ -2,7 +2,7 @@ autoware_polar_grid - 0.40.0 + 0.41.0 The autoware_polar_grid package Yukihiro Saito Apache License 2.0 diff --git a/common/autoware_pyplot/CHANGELOG.rst b/common/autoware_pyplot/CHANGELOG.rst index c3b1f9c17f7d5..970aa3361e4dd 100644 --- a/common/autoware_pyplot/CHANGELOG.rst +++ b/common/autoware_pyplot/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_pyplot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_test_utils): add visualization (`#9603 `_) +* Contributors: Fumiya Watanabe, Mamoru Sobue + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_pyplot/CMakeLists.txt b/common/autoware_pyplot/CMakeLists.txt index 6922d5d9306f7..5871929676a2d 100644 --- a/common/autoware_pyplot/CMakeLists.txt +++ b/common/autoware_pyplot/CMakeLists.txt @@ -15,8 +15,12 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} STATIC DIRECTORY src ) +target_compile_options(${PROJECT_NAME} PUBLIC "-fPIC") target_link_libraries(${PROJECT_NAME} ${Python3_LIBRARIES} pybind11::embed) +# NOTE(soblin): this is workaround for propagating the include of "Python.h" to user modules to avoid "'Python.h' not found" +ament_export_include_directories(${Python3_INCLUDE_DIRS}) + if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) diff --git a/common/autoware_pyplot/package.xml b/common/autoware_pyplot/package.xml index 2f3f3014d4907..1877699b70fc1 100644 --- a/common/autoware_pyplot/package.xml +++ b/common/autoware_pyplot/package.xml @@ -2,7 +2,7 @@ autoware_pyplot - 0.40.0 + 0.41.0 C++ interface for matplotlib based on pybind11 Mamoru Sobue Yukinari Hisaki diff --git a/common/autoware_qp_interface/CHANGELOG.rst b/common/autoware_qp_interface/CHANGELOG.rst deleted file mode 100644 index f76dac861e1e6..0000000000000 --- a/common/autoware_qp_interface/CHANGELOG.rst +++ /dev/null @@ -1,81 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_qp_interface -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - common (`#9564 `_) -* fix: fix package names in changelog files (`#9500 `_) -* fix(autoware_osqp_interface): fix clang-tidy errors (`#9440 `_) -* 0.39.0 -* update changelog -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Ryuta Kambe, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* Merge commit '6a1ddbd08bd' into release-0.39.0 -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* refactor(qp_interface): prefix package and namespace with autoware (`#9236 `_) -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* fix(qp_interface): fix unreadVariable (`#8349 `_) - * fix:unreadVariable - * fix:unreadVariable - * fix:unreadVariable - --------- -* perf(velocity_smoother): use ProxQP for faster optimization (`#8028 `_) - * perf(velocity_smoother): use ProxQP for faster optimization - * consider max_iteration - * disable warm start - * fix test - --------- -* refactor(qp_interface): clean up the code (`#8029 `_) - * refactor qp_interface - * variable names: m_XXX -> XXX\_ - * update - * update - --------- -* fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions (`#7855 `_) - * fix(fake_test_node, osqp_interface, qp_interface): remove unnecessary cppcheck inline suppressions - * style(pre-commit): autofix - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> -* Contributors: Ryuta Kambe, Takayuki Murooka, Yutaka Kondo, kobayu858 - -0.26.0 (2024-04-03) -------------------- -* feat(qp_interface): support proxqp (`#3699 `_) - * feat(qp_interface): add proxqp interface - * update - --------- -* feat(qp_interface): add new package which will contain various qp solvers (`#3697 `_) -* Contributors: Takayuki Murooka diff --git a/common/autoware_qp_interface/CMakeLists.txt b/common/autoware_qp_interface/CMakeLists.txt deleted file mode 100644 index 1df75d2d719a0..0000000000000 --- a/common/autoware_qp_interface/CMakeLists.txt +++ /dev/null @@ -1,64 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_qp_interface) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(Eigen3 REQUIRED) -find_package(proxsuite REQUIRED) - -# after find_package(osqp_vendor) in ament_auto_find_build_dependencies -find_package(osqp REQUIRED) -get_target_property(OSQP_INCLUDE_SUB_DIR osqp::osqp INTERFACE_INCLUDE_DIRECTORIES) -get_filename_component(OSQP_INCLUDE_DIR ${OSQP_INCLUDE_SUB_DIR} PATH) - -set(QP_INTERFACE_LIB_SRC - src/qp_interface.cpp - src/osqp_interface.cpp - src/osqp_csc_matrix_conv.cpp - src/proxqp_interface.cpp -) - -set(QP_INTERFACE_LIB_HEADERS - include/autoware/qp_interface/qp_interface.hpp - include/autoware/qp_interface/osqp_interface.hpp - include/autoware/qp_interface/osqp_csc_matrix_conv.hpp - include/autoware/qp_interface/proxqp_interface.hpp -) - -ament_auto_add_library(${PROJECT_NAME} SHARED - ${QP_INTERFACE_LIB_SRC} - ${QP_INTERFACE_LIB_HEADERS} -) -target_compile_options(${PROJECT_NAME} PRIVATE -Wno-error=old-style-cast) - -target_include_directories(${PROJECT_NAME} - SYSTEM PUBLIC - "${OSQP_INCLUDE_DIR}" - "${EIGEN3_INCLUDE_DIR}" -) - -ament_target_dependencies(${PROJECT_NAME} - Eigen3 - osqp_vendor - proxsuite -) - -# crucial so downstream package builds because osqp_interface exposes osqp.hpp -ament_export_include_directories("${OSQP_INCLUDE_DIR}") -# crucial so the linking order is correct in a downstream package: libosqp_interface.a should come before libosqp.a -ament_export_libraries(osqp::osqp) - -if(BUILD_TESTING) - set(TEST_SOURCES - test/test_osqp_interface.cpp - test/test_csc_matrix_conv.cpp - test/test_proxqp_interface.cpp - ) - set(TEST_OSQP_INTERFACE_EXE test_osqp_interface) - ament_add_ros_isolated_gtest(${TEST_OSQP_INTERFACE_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_OSQP_INTERFACE_EXE} ${PROJECT_NAME}) -endif() - -ament_auto_package(INSTALL_TO_SHARE -) diff --git a/common/autoware_qp_interface/design/qp_interface-design.md b/common/autoware_qp_interface/design/qp_interface-design.md deleted file mode 100644 index edc7fa9845206..0000000000000 --- a/common/autoware_qp_interface/design/qp_interface-design.md +++ /dev/null @@ -1,48 +0,0 @@ -# Interface for QP solvers - -This is the design document for the `autoware_qp_interface` package. - -## Purpose / Use cases - -This packages provides a C++ interface for QP solvers. -Currently, supported QP solvers are - -- [OSQP library](https://osqp.org/docs/solver/index.html) - -## Design - -The class `QPInterface` takes a problem formulation as Eigen matrices and vectors, converts these objects into -C-style Compressed-Column-Sparse matrices and dynamic arrays, loads the data into the QP workspace dataholder, and runs the optimizer. - -## Inputs / Outputs / API - -The interface can be used in several ways: - -1. Initialize the interface, and load the problem formulation at the optimization call. - - ```cpp - QPInterface qp_interface; - qp_interface.optimize(P, A, q, l, u); - ``` - -2. WARM START OPTIMIZATION by modifying the problem formulation between optimization runs. - - ```cpp - QPInterface qp_interface(true); - qp_interface.optimize(P, A, q, l, u); - qp_interface.optimize(P_new, A_new, q_new, l_new, u_new); - ``` - - The optimization results are returned as a vector by the optimization function. - - ```cpp - const auto solution = qp_interface.optimize(); - double x_0 = solution[0]; - double x_1 = solution[1]; - ``` - -## References / External links - -- OSQP library: - -## Related issues diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp deleted file mode 100644 index 9575d9d18c69f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_csc_matrix_conv.hpp +++ /dev/null @@ -1,46 +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 AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ - -#include "osqp/glob_opts.h" - -#include - -#include - -namespace autoware::qp_interface -{ -/// \brief Compressed-Column-Sparse Matrix -struct CSC_Matrix -{ - /// Vector of non-zero values. Ex: [4,1,1,2] - std::vector vals_; - /// Vector of row index corresponding to values. Ex: [0, 1, 0, 1] (Eigen: 'inner') - std::vector row_idxs_; - /// Vector of 'val' indices where each column starts. Ex: [0, 2, 4] (Eigen: 'outer') - std::vector col_idxs_; -}; - -/// \brief Calculate CSC matrix from Eigen matrix -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat); -/// \brief Calculate upper trapezoidal CSC matrix from square Eigen matrix -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat); -/// \brief Print the given CSC matrix to the standard output -void printCSCMatrix(const CSC_Matrix & csc_mat); - -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_CSC_MATRIX_CONV_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp deleted file mode 100644 index a5777dd545e9f..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/osqp_interface.hpp +++ /dev/null @@ -1,147 +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 AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "autoware/qp_interface/qp_interface.hpp" -#include "osqp/osqp.h" - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -constexpr c_float OSQP_INF = 1e30; - -class OSQPInterface : public QPInterface -{ -public: - /// \brief Constructor without problem formulation - OSQPInterface( - const bool enable_warm_start = false, const int max_iteration = 20000, - const c_float eps_abs = std::numeric_limits::epsilon(), - const c_float eps_rel = std::numeric_limits::epsilon(), const bool polish = true, - const bool verbose = false); - /// \brief Constructor with problem setup - /// \param P: (n,n) matrix defining relations between parameters. - /// \param A: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - /// \param q: (n) vector defining the linear cost of the problem. - /// \param l: (m) vector defining the lower bound problem constraint. - /// \param u: (m) vector defining the upper bound problem constraint. - /// \param eps_abs: Absolute convergence tolerance. - OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, - const bool enable_warm_start = false, - const c_float eps_abs = std::numeric_limits::epsilon()); - ~OSQPInterface(); - - static void OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept; - - std::vector optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - int getPolishStatus() const; - std::vector getDualSolution() const; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - - // Updates problem parameters while keeping solution in memory. - // - // Args: - // P_new: (n,n) matrix defining relations between parameters. - // A_new: (m,n) matrix defining parameter constraints relative to the lower and upper bound. - // q_new: (n) vector defining the linear cost of the problem. - // l_new: (m) vector defining the lower bound problem constraint. - // u_new: (m) vector defining the upper bound problem constraint. - void updateP(const Eigen::MatrixXd & P_new); - void updateCscP(const CSC_Matrix & P_csc); - void updateA(const Eigen::MatrixXd & A_new); - void updateCscA(const CSC_Matrix & A_csc); - void updateQ(const std::vector & q_new); - void updateL(const std::vector & l_new); - void updateU(const std::vector & u_new); - void updateBounds(const std::vector & l_new, const std::vector & u_new); - - void updateMaxIter(const int iter); - void updateRhoInterval(const int rho_interval); - void updateRho(const double rho); - void updateAlpha(const double alpha); - void updateScaling(const int scaling); - void updatePolish(const bool polish); - void updatePolishRefinementIteration(const int polish_refine_iter); - void updateCheckTermination(const int check_termination); - - /// \brief Get the number of iteration taken to solve the problem - inline int64_t getTakenIter() const { return static_cast(latest_work_info_.iter); } - /// \brief Get the status message for the latest problem solved - inline std::string getStatusMessage() const - { - return static_cast(latest_work_info_.status); - } - /// \brief Get the runtime of the latest problem solved - inline double getRunTime() const { return latest_work_info_.run_time; } - /// \brief Get the objective value the latest problem solved - inline double getObjVal() const { return latest_work_info_.obj_val; } - /// \brief Returns flag asserting interface condition (Healthy condition: 0). - inline int64_t getExitFlag() const { return exitflag_; } - - // Setter functions for warm start - bool setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables); - bool setPrimalVariables(const std::vector & primal_variables); - bool setDualVariables(const std::vector & dual_variables); - -private: - std::unique_ptr> work_; - std::unique_ptr settings_; - std::unique_ptr data_; - // store last work info since work is cleaned up at every execution to prevent memory leak. - OSQPInfo latest_work_info_; - // Number of parameters to optimize - int64_t param_n_; - // Flag to check if the current work exists - bool work__initialized = false; - // Exitflag - int64_t exitflag_; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - void initializeCSCProblemImpl( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u); - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__OSQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp deleted file mode 100644 index 324da4b18c6fd..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/proxqp_interface.hpp +++ /dev/null @@ -1,57 +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 AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ - -#include "autoware/qp_interface/qp_interface.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -class ProxQPInterface : public QPInterface -{ -public: - explicit ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, - const double eps_rel, const bool verbose = false); - - int getIterationNumber() const override; - bool isSolved() const override; - std::string getStatus() const override; - - void updateEpsAbs(const double eps_abs) override; - void updateEpsRel(const double eps_rel) override; - void updateVerbose(const bool verbose) override; - -private: - proxsuite::proxqp::Settings settings_{}; - std::shared_ptr> qp_ptr_{nullptr}; - - void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) override; - - std::vector optimizeImpl() override; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__PROXQP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp b/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp deleted file mode 100644 index be3c598512bf6..0000000000000 --- a/common/autoware_qp_interface/include/autoware/qp_interface/qp_interface.hpp +++ /dev/null @@ -1,61 +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 AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ -#define AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ - -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -class QPInterface -{ -public: - explicit QPInterface(const bool enable_warm_start) : enable_warm_start_(enable_warm_start) {} - - std::vector optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual bool isSolved() const = 0; - virtual int getIterationNumber() const = 0; - virtual std::string getStatus() const = 0; - - virtual void updateEpsAbs([[maybe_unused]] const double eps_abs) = 0; - virtual void updateEpsRel([[maybe_unused]] const double eps_rel) = 0; - virtual void updateVerbose([[maybe_unused]] const bool verbose) {} - -protected: - bool enable_warm_start_{false}; - - void initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u); - - virtual void initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) = 0; - - virtual std::vector optimizeImpl() = 0; - - std::optional variables_num_{std::nullopt}; - std::optional constraints_num_{std::nullopt}; -}; -} // namespace autoware::qp_interface - -#endif // AUTOWARE__QP_INTERFACE__QP_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp b/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp deleted file mode 100644 index 15314a9e4a5fa..0000000000000 --- a/common/autoware_qp_interface/src/osqp_csc_matrix_conv.cpp +++ /dev/null @@ -1,134 +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 "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include -#include - -#include -#include -#include - -namespace autoware::qp_interface -{ -CSC_Matrix calCSCMatrix(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i < rows; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -CSC_Matrix calCSCMatrixTrapezoidal(const Eigen::MatrixXd & mat) -{ - const size_t elem = static_cast(mat.nonZeros()); - const Eigen::Index rows = mat.rows(); - const Eigen::Index cols = mat.cols(); - - if (rows != cols) { - throw std::invalid_argument("Matrix must be square (n, n)"); - } - - std::vector vals; - vals.reserve(elem); - std::vector row_idxs; - row_idxs.reserve(elem); - std::vector col_idxs; - col_idxs.reserve(elem); - - // Construct CSC matrix arrays - c_float val; - Eigen::Index trap_last_idx = 0; - c_int elem_count = 0; - - col_idxs.push_back(0); - - for (Eigen::Index j = 0; j < cols; j++) { // col iteration - for (Eigen::Index i = 0; i <= trap_last_idx; i++) { // row iteration - // Get values of nonzero elements - val = mat(i, j); - if (std::fabs(val) < 1e-9) { - continue; - } - elem_count += 1; - - // Store values - vals.push_back(val); - row_idxs.push_back(i); - } - - col_idxs.push_back(elem_count); - trap_last_idx += 1; - } - - CSC_Matrix csc_matrix = {vals, row_idxs, col_idxs}; - - return csc_matrix; -} - -void printCSCMatrix(const CSC_Matrix & csc_mat) -{ - std::cout << "["; - for (const c_float & val : csc_mat.vals_) { - std::cout << val << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & row : csc_mat.row_idxs_) { - std::cout << row << ", "; - } - std::cout << "]\n"; - - std::cout << "["; - for (const c_int & col : csc_mat.col_idxs_) { - std::cout << col << ", "; - } - std::cout << "]\n"; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/osqp_interface.cpp b/common/autoware_qp_interface/src/osqp_interface.cpp deleted file mode 100644 index fbb8e71f4c251..0000000000000 --- a/common/autoware_qp_interface/src/osqp_interface.cpp +++ /dev/null @@ -1,394 +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 "autoware/qp_interface/osqp_interface.hpp" - -#include "autoware/qp_interface/osqp_csc_matrix_conv.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::qp_interface -{ -OSQPInterface::OSQPInterface( - const bool enable_warm_start, const int max_iteration, const c_float eps_abs, - const c_float eps_rel, const bool polish, const bool verbose) -: QPInterface(enable_warm_start), work_{nullptr, OSQPWorkspaceDeleter} -{ - settings_ = std::make_unique(); - data_ = std::make_unique(); - - if (settings_) { - osqp_set_default_settings(settings_.get()); - settings_->alpha = 1.6; // Change alpha parameter - settings_->eps_rel = eps_rel; - settings_->eps_abs = eps_abs; - settings_->eps_prim_inf = 1.0E-4; - settings_->eps_dual_inf = 1.0E-4; - settings_->warm_start = enable_warm_start; - settings_->max_iter = max_iteration; - settings_->verbose = verbose; - settings_->polish = polish; - } - exitflag_ = 0; -} - -OSQPInterface::OSQPInterface( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeProblem(P, A, q, l, u); -} - -OSQPInterface::OSQPInterface( - const CSC_Matrix & P, const CSC_Matrix & A, const std::vector & q, - const std::vector & l, const std::vector & u, const bool enable_warm_start, - const c_float eps_abs) -: OSQPInterface(enable_warm_start, eps_abs) -{ - initializeCSCProblemImpl(P, A, q, l, u); -} - -OSQPInterface::~OSQPInterface() -{ - if (data_->P) free(data_->P); - if (data_->A) free(data_->A); -} - -void OSQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - initializeCSCProblemImpl(P_csc, A_csc, q, l, u); -} - -void OSQPInterface::initializeCSCProblemImpl( - CSC_Matrix P_csc, CSC_Matrix A_csc, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - // Dynamic float arrays - std::vector q_tmp(q.begin(), q.end()); - std::vector l_tmp(l.begin(), l.end()); - std::vector u_tmp(u.begin(), u.end()); - double * q_dyn = q_tmp.data(); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - - /********************** - * OBJECTIVE FUNCTION - **********************/ - param_n_ = static_cast(q.size()); - data_->m = static_cast(l.size()); - - /***************** - * POPULATE DATA - *****************/ - data_->n = param_n_; - if (data_->P) free(data_->P); - data_->P = csc_matrix( - data_->n, data_->n, static_cast(P_csc.vals_.size()), P_csc.vals_.data(), - P_csc.row_idxs_.data(), P_csc.col_idxs_.data()); - data_->q = q_dyn; - if (data_->A) free(data_->A); - data_->A = csc_matrix( - data_->m, data_->n, static_cast(A_csc.vals_.size()), A_csc.vals_.data(), - A_csc.row_idxs_.data(), A_csc.col_idxs_.data()); - data_->l = l_dyn; - data_->u = u_dyn; - - // Setup workspace - OSQPWorkspace * workspace; - exitflag_ = osqp_setup(&workspace, data_.get(), settings_.get()); - work_.reset(workspace); - work__initialized = true; -} - -void OSQPInterface::OSQPWorkspaceDeleter(OSQPWorkspace * ptr) noexcept -{ - if (ptr != nullptr) { - osqp_cleanup(ptr); - } -} - -void OSQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_->eps_abs = eps_abs; // for default setting - if (work__initialized) { - osqp_update_eps_abs(work_.get(), eps_abs); // for current work - } -} - -void OSQPInterface::updateEpsRel(const double eps_rel) -{ - settings_->eps_rel = eps_rel; // for default setting - if (work__initialized) { - osqp_update_eps_rel(work_.get(), eps_rel); // for current work - } -} - -void OSQPInterface::updateMaxIter(const int max_iter) -{ - settings_->max_iter = max_iter; // for default setting - if (work__initialized) { - osqp_update_max_iter(work_.get(), max_iter); // for current work - } -} - -void OSQPInterface::updateVerbose(const bool is_verbose) -{ - settings_->verbose = is_verbose; // for default setting - if (work__initialized) { - osqp_update_verbose(work_.get(), is_verbose); // for current work - } -} - -void OSQPInterface::updateRhoInterval(const int rho_interval) -{ - settings_->adaptive_rho_interval = rho_interval; // for default setting -} - -void OSQPInterface::updateRho(const double rho) -{ - settings_->rho = rho; - if (work__initialized) { - osqp_update_rho(work_.get(), rho); - } -} - -void OSQPInterface::updateAlpha(const double alpha) -{ - settings_->alpha = alpha; - if (work__initialized) { - osqp_update_alpha(work_.get(), alpha); - } -} - -void OSQPInterface::updateScaling(const int scaling) -{ - settings_->scaling = scaling; -} - -void OSQPInterface::updatePolish(const bool polish) -{ - settings_->polish = polish; - if (work__initialized) { - osqp_update_polish(work_.get(), polish); - } -} - -void OSQPInterface::updatePolishRefinementIteration(const int polish_refine_iter) -{ - if (polish_refine_iter < 0) { - std::cerr << "Polish refinement iterations must be positive" << std::endl; - return; - } - - settings_->polish_refine_iter = polish_refine_iter; - if (work__initialized) { - osqp_update_polish_refine_iter(work_.get(), polish_refine_iter); - } -} - -void OSQPInterface::updateCheckTermination(const int check_termination) -{ - if (check_termination < 0) { - std::cerr << "Check termination must be positive" << std::endl; - return; - } - - settings_->check_termination = check_termination; - if (work__initialized) { - osqp_update_check_termination(work_.get(), check_termination); - } -} - -bool OSQPInterface::setWarmStart( - const std::vector & primal_variables, const std::vector & dual_variables) -{ - return setPrimalVariables(primal_variables) && setDualVariables(dual_variables); -} - -bool OSQPInterface::setPrimalVariables(const std::vector & primal_variables) -{ - if (!work__initialized) { - return false; - } - - const auto result = osqp_warm_start_x(work_.get(), primal_variables.data()); - if (result != 0) { - std::cerr << "Failed to set primal variables for warm start" << std::endl; - return false; - } - - return true; -} - -bool OSQPInterface::setDualVariables(const std::vector & dual_variables) -{ - if (!work__initialized) { - return false; - } - - const auto result = osqp_warm_start_y(work_.get(), dual_variables.data()); - if (result != 0) { - std::cerr << "Failed to set dual variables for warm start" << std::endl; - return false; - } - - return true; -} - -void OSQPInterface::updateP(const Eigen::MatrixXd & P_new) -{ - /* - // Transform 'P' into an 'upper trapezoidal matrix' - Eigen::MatrixXd P_trap = P_new.triangularView(); - // Transform 'P' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix P_sparse = P_trap.sparseView(); - double *P_val_ptr = P_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int P_elem_N = P_sparse.nonZeros(); - */ - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P_new); - osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateCscP(const CSC_Matrix & P_csc) -{ - osqp_update_P(work_.get(), P_csc.vals_.data(), OSQP_NULL, static_cast(P_csc.vals_.size())); -} - -void OSQPInterface::updateA(const Eigen::MatrixXd & A_new) -{ - /* - // Transform 'A' into a sparse matrix and extract data as dynamic arrays - Eigen::SparseMatrix A_sparse = A_new.sparseView(); - double *A_val_ptr = A_sparse.valuePtr(); - // Convert dynamic 'int' arrays to 'c_int' arrays (OSQP input type) - c_int A_elem_N = A_sparse.nonZeros(); - */ - CSC_Matrix A_csc = calCSCMatrix(A_new); - osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); - return; -} - -void OSQPInterface::updateCscA(const CSC_Matrix & A_csc) -{ - osqp_update_A(work_.get(), A_csc.vals_.data(), OSQP_NULL, static_cast(A_csc.vals_.size())); -} - -void OSQPInterface::updateQ(const std::vector & q_new) -{ - std::vector q_tmp(q_new.begin(), q_new.end()); - double * q_dyn = q_tmp.data(); - osqp_update_lin_cost(work_.get(), q_dyn); -} - -void OSQPInterface::updateL(const std::vector & l_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - double * l_dyn = l_tmp.data(); - osqp_update_lower_bound(work_.get(), l_dyn); -} - -void OSQPInterface::updateU(const std::vector & u_new) -{ - std::vector u_tmp(u_new.begin(), u_new.end()); - double * u_dyn = u_tmp.data(); - osqp_update_upper_bound(work_.get(), u_dyn); -} - -void OSQPInterface::updateBounds( - const std::vector & l_new, const std::vector & u_new) -{ - std::vector l_tmp(l_new.begin(), l_new.end()); - std::vector u_tmp(u_new.begin(), u_new.end()); - double * l_dyn = l_tmp.data(); - double * u_dyn = u_tmp.data(); - osqp_update_bounds(work_.get(), l_dyn, u_dyn); -} - -int OSQPInterface::getIterationNumber() const -{ - return work_->info->iter; -} - -std::string OSQPInterface::getStatus() const -{ - return "OSQP_SOLVED"; -} - -bool OSQPInterface::isSolved() const -{ - return latest_work_info_.status_val == 1; -} - -int OSQPInterface::getPolishStatus() const -{ - return static_cast(latest_work_info_.status_polish); -} - -std::vector OSQPInterface::getDualSolution() const -{ - double * sol_y = work_->solution->y; - std::vector dual_solution(sol_y, sol_y + data_->m); - return dual_solution; -} - -std::vector OSQPInterface::optimizeImpl() -{ - osqp_solve(work_.get()); - - double * sol_x = work_->solution->x; - std::vector sol_primal(sol_x, sol_x + param_n_); - - latest_work_info_ = *(work_->info); - - if (!enable_warm_start_) { - work_.reset(); - work__initialized = false; - } - - return sol_primal; -} - -std::vector OSQPInterface::optimize( - CSC_Matrix P, CSC_Matrix A, const std::vector & q, const std::vector & l, - const std::vector & u) -{ - initializeCSCProblemImpl(P, A, q, l, u); - const auto result = optimizeImpl(); - - // show polish status if not successful - const int status_polish = static_cast(latest_work_info_.status_polish); - if (status_polish != 1) { - const auto msg = status_polish == 0 ? "unperformed" - : status_polish == -1 ? "unsuccessful" - : "unknown"; - std::cerr << "osqp polish process failed : " << msg << ". The result may be inaccurate" - << std::endl; - } - - return result; -} - -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/proxqp_interface.cpp b/common/autoware_qp_interface/src/proxqp_interface.cpp deleted file mode 100644 index a0ebbf0db0510..0000000000000 --- a/common/autoware_qp_interface/src/proxqp_interface.cpp +++ /dev/null @@ -1,157 +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 "autoware/qp_interface/proxqp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -using proxsuite::proxqp::QPSolverOutput; - -ProxQPInterface::ProxQPInterface( - const bool enable_warm_start, const int max_iteration, const double eps_abs, const double eps_rel, - const bool verbose) -: QPInterface(enable_warm_start) -{ - settings_.max_iter = max_iteration; - settings_.eps_abs = eps_abs; - settings_.eps_rel = eps_rel; - settings_.verbose = verbose; -} - -void ProxQPInterface::initializeProblemImpl( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - const size_t variables_num = q.size(); - const size_t constraints_num = l.size(); - - const bool enable_warm_start = [&]() { - if ( - !enable_warm_start_ // Warm start is designated - || !qp_ptr_ // QP pointer is initialized - // The number of variables is the same as the previous one. - || !variables_num_ || - *variables_num_ != variables_num - // The number of constraints is the same as the previous one - || !constraints_num_ || *constraints_num_ != constraints_num) { - return false; - } - return true; - }(); - - if (!enable_warm_start) { - qp_ptr_ = std::make_shared>( - variables_num, 0, constraints_num); - } - - settings_.initial_guess = - enable_warm_start ? proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT - : proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; - - qp_ptr_->settings = settings_; - - Eigen::SparseMatrix P_sparse(variables_num, constraints_num); - P_sparse = P.sparseView(); - - // NOTE: const std vector cannot be converted to eigen vector - std::vector non_const_q = q; - Eigen::VectorXd eigen_q = - Eigen::Map(non_const_q.data(), non_const_q.size()); - std::vector l_std_vec = l; - Eigen::VectorXd eigen_l = - Eigen::Map(l_std_vec.data(), l_std_vec.size()); - std::vector u_std_vec = u; - Eigen::VectorXd eigen_u = - Eigen::Map(u_std_vec.data(), u_std_vec.size()); - - if (enable_warm_start) { - qp_ptr_->update( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } else { - qp_ptr_->init( - P_sparse, eigen_q, proxsuite::nullopt, proxsuite::nullopt, A.sparseView(), eigen_l, eigen_u); - } -} - -void ProxQPInterface::updateEpsAbs(const double eps_abs) -{ - settings_.eps_abs = eps_abs; -} - -void ProxQPInterface::updateEpsRel(const double eps_rel) -{ - settings_.eps_rel = eps_rel; -} - -void ProxQPInterface::updateVerbose(const bool is_verbose) -{ - settings_.verbose = is_verbose; -} - -bool ProxQPInterface::isSolved() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED; - } - return false; -} - -int ProxQPInterface::getIterationNumber() const -{ - if (qp_ptr_) { - return qp_ptr_->results.info.iter; - } - return 0; -} - -std::string ProxQPInterface::getStatus() const -{ - if (qp_ptr_) { - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED) { - return "PROXQP_SOLVED"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_MAX_ITER_REACHED) { - return "PROXQP_MAX_ITER_REACHED"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE) { - return "PROXQP_PRIMAL_INFEASIBLE"; - } - // if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE) { - // return "PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; - // } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_DUAL_INFEASIBLE) { - return "PROXQP_DUAL_INFEASIBLE"; - } - if (qp_ptr_->results.info.status == QPSolverOutput::PROXQP_NOT_RUN) { - return "PROXQP_NOT_RUN"; - } - } - return "None"; -} - -std::vector ProxQPInterface::optimizeImpl() -{ - qp_ptr_->solve(); - - std::vector result; - for (Eigen::Index i = 0; i < qp_ptr_->results.x.size(); ++i) { - result.push_back(qp_ptr_->results.x[i]); - } - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/src/qp_interface.cpp b/common/autoware_qp_interface/src/qp_interface.cpp deleted file mode 100644 index f01e57772dc4f..0000000000000 --- a/common/autoware_qp_interface/src/qp_interface.cpp +++ /dev/null @@ -1,70 +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 "autoware/qp_interface/qp_interface.hpp" - -#include -#include -#include - -namespace autoware::qp_interface -{ -void QPInterface::initializeProblem( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - // check if arguments are valid - std::stringstream ss; - if (P.rows() != P.cols()) { - ss << "P.rows() and P.cols() are not the same. P.rows() = " << P.rows() - << ", P.cols() = " << P.cols(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != static_cast(q.size())) { - ss << "P.rows() and q.size() are not the same. P.rows() = " << P.rows() - << ", q.size() = " << q.size(); - throw std::invalid_argument(ss.str()); - } - if (P.rows() != A.cols()) { - ss << "P.rows() and A.cols() are not the same. P.rows() = " << P.rows() - << ", A.cols() = " << A.cols(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(l.size())) { - ss << "A.rows() and l.size() are not the same. A.rows() = " << A.rows() - << ", l.size() = " << l.size(); - throw std::invalid_argument(ss.str()); - } - if (A.rows() != static_cast(u.size())) { - ss << "A.rows() and u.size() are not the same. A.rows() = " << A.rows() - << ", u.size() = " << u.size(); - throw std::invalid_argument(ss.str()); - } - - initializeProblemImpl(P, A, q, l, u); - - variables_num_ = q.size(); - constraints_num_ = l.size(); -} - -std::vector QPInterface::optimize( - const Eigen::MatrixXd & P, const Eigen::MatrixXd & A, const std::vector & q, - const std::vector & l, const std::vector & u) -{ - initializeProblem(P, A, q, l, u); - const auto result = optimizeImpl(); - - return result; -} -} // namespace autoware::qp_interface diff --git a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp b/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp deleted file mode 100644 index 1f377a1a24535..0000000000000 --- a/common/autoware_qp_interface/test/test_csc_matrix_conv.cpp +++ /dev/null @@ -1,181 +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 "autoware/qp_interface/osqp_csc_matrix_conv.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include - -TEST(TestCscMatrixConv, Nominal) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd rect1(1, 2); - rect1 << 0.0, 1.0; - - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - ASSERT_EQ(rect_m1.vals_.size(), size_t(1)); - EXPECT_EQ(rect_m1.vals_[0], 1.0); - ASSERT_EQ(rect_m1.row_idxs_.size(), size_t(1)); - EXPECT_EQ(rect_m1.row_idxs_[0], c_int(0)); - ASSERT_EQ(rect_m1.col_idxs_.size(), size_t(3)); // nb of columns + 1 - EXPECT_EQ(rect_m1.col_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m1.col_idxs_[1], c_int(0)); - EXPECT_EQ(rect_m1.col_idxs_[2], c_int(1)); - - Eigen::MatrixXd rect2(2, 4); - rect2 << 1.0, 0.0, 3.0, 0.0, 0.0, 6.0, 7.0, 0.0; - - const CSC_Matrix rect_m2 = calCSCMatrix(rect2); - ASSERT_EQ(rect_m2.vals_.size(), size_t(4)); - EXPECT_EQ(rect_m2.vals_[0], 1.0); - EXPECT_EQ(rect_m2.vals_[1], 6.0); - EXPECT_EQ(rect_m2.vals_[2], 3.0); - EXPECT_EQ(rect_m2.vals_[3], 7.0); - ASSERT_EQ(rect_m2.row_idxs_.size(), size_t(4)); - EXPECT_EQ(rect_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(rect_m2.row_idxs_[2], c_int(0)); - EXPECT_EQ(rect_m2.row_idxs_[3], c_int(1)); - ASSERT_EQ(rect_m2.col_idxs_.size(), size_t(5)); // nb of columns + 1 - EXPECT_EQ(rect_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(rect_m2.col_idxs_[1], c_int(1)); - EXPECT_EQ(rect_m2.col_idxs_[2], c_int(2)); - EXPECT_EQ(rect_m2.col_idxs_[3], c_int(4)); - EXPECT_EQ(rect_m2.col_idxs_[4], c_int(4)); - - // Example from http://netlib.org/linalg/html_templates/node92.html - Eigen::MatrixXd square2(6, 6); - square2 << 10.0, 0.0, 0.0, 0.0, -2.0, 0.0, 3.0, 9.0, 0.0, 0.0, 0.0, 3.0, 0.0, 7.0, 8.0, 7.0, 0.0, - 0.0, 3.0, 0.0, 8.0, 7.0, 5.0, 0.0, 0.0, 8.0, 0.0, 9.0, 9.0, 13.0, 0.0, 4.0, 0.0, 0.0, 2.0, -1.0; - - const CSC_Matrix square_m2 = calCSCMatrix(square2); - ASSERT_EQ(square_m2.vals_.size(), size_t(19)); - EXPECT_EQ(square_m2.vals_[0], 10.0); - EXPECT_EQ(square_m2.vals_[1], 3.0); - EXPECT_EQ(square_m2.vals_[2], 3.0); - EXPECT_EQ(square_m2.vals_[3], 9.0); - EXPECT_EQ(square_m2.vals_[4], 7.0); - EXPECT_EQ(square_m2.vals_[5], 8.0); - EXPECT_EQ(square_m2.vals_[6], 4.0); - EXPECT_EQ(square_m2.vals_[7], 8.0); - EXPECT_EQ(square_m2.vals_[8], 8.0); - EXPECT_EQ(square_m2.vals_[9], 7.0); - EXPECT_EQ(square_m2.vals_[10], 7.0); - EXPECT_EQ(square_m2.vals_[11], 9.0); - EXPECT_EQ(square_m2.vals_[12], -2.0); - EXPECT_EQ(square_m2.vals_[13], 5.0); - EXPECT_EQ(square_m2.vals_[14], 9.0); - EXPECT_EQ(square_m2.vals_[15], 2.0); - EXPECT_EQ(square_m2.vals_[16], 3.0); - EXPECT_EQ(square_m2.vals_[17], 13.0); - EXPECT_EQ(square_m2.vals_[18], -1.0); - ASSERT_EQ(square_m2.row_idxs_.size(), size_t(19)); - EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[2], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[3], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[4], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[5], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[6], c_int(5)); - EXPECT_EQ(square_m2.row_idxs_[7], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[8], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[9], c_int(2)); - EXPECT_EQ(square_m2.row_idxs_[10], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[11], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[12], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[13], c_int(3)); - EXPECT_EQ(square_m2.row_idxs_[14], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[15], c_int(5)); - EXPECT_EQ(square_m2.row_idxs_[16], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[17], c_int(4)); - EXPECT_EQ(square_m2.row_idxs_[18], c_int(5)); - ASSERT_EQ(square_m2.col_idxs_.size(), size_t(7)); // nb of columns + 1 - EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[1], c_int(3)); - EXPECT_EQ(square_m2.col_idxs_[2], c_int(7)); - EXPECT_EQ(square_m2.col_idxs_[3], c_int(9)); - EXPECT_EQ(square_m2.col_idxs_[4], c_int(12)); - EXPECT_EQ(square_m2.col_idxs_[5], c_int(16)); - EXPECT_EQ(square_m2.col_idxs_[6], c_int(19)); -} -TEST(TestCscMatrixConv, Trapezoidal) -{ - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd square2(3, 3); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - square2 << 0.0, 2.0, 0.0, 4.0, 5.0, 6.0, 0.0, 0.0, 0.0; - rect1 << 0.0, 1.0; - - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - // Trapezoidal: skip the lower left triangle (2.0 in this example) - ASSERT_EQ(square_m1.vals_.size(), size_t(3)); - EXPECT_EQ(square_m1.vals_[0], 1.0); - EXPECT_EQ(square_m1.vals_[1], 2.0); - EXPECT_EQ(square_m1.vals_[2], 4.0); - ASSERT_EQ(square_m1.row_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m1.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m1.row_idxs_[1], c_int(0)); - EXPECT_EQ(square_m1.row_idxs_[2], c_int(1)); - ASSERT_EQ(square_m1.col_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m1.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m1.col_idxs_[1], c_int(1)); - EXPECT_EQ(square_m1.col_idxs_[2], c_int(3)); - - const CSC_Matrix square_m2 = calCSCMatrixTrapezoidal(square2); - ASSERT_EQ(square_m2.vals_.size(), size_t(3)); - EXPECT_EQ(square_m2.vals_[0], 2.0); - EXPECT_EQ(square_m2.vals_[1], 5.0); - EXPECT_EQ(square_m2.vals_[2], 6.0); - ASSERT_EQ(square_m2.row_idxs_.size(), size_t(3)); - EXPECT_EQ(square_m2.row_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.row_idxs_[1], c_int(1)); - EXPECT_EQ(square_m2.row_idxs_[2], c_int(1)); - ASSERT_EQ(square_m2.col_idxs_.size(), size_t(4)); - EXPECT_EQ(square_m2.col_idxs_[0], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[1], c_int(0)); - EXPECT_EQ(square_m2.col_idxs_[2], c_int(2)); - EXPECT_EQ(square_m2.col_idxs_[3], c_int(3)); - - try { - const CSC_Matrix rect_m1 = calCSCMatrixTrapezoidal(rect1); - FAIL() << "calCSCMatrixTrapezoidal should fail with non-square inputs"; - } catch (const std::invalid_argument & e) { - EXPECT_EQ(e.what(), std::string("Matrix must be square (n, n)")); - } -} -TEST(TestCscMatrixConv, Print) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - using autoware::qp_interface::printCSCMatrix; - Eigen::MatrixXd square1(2, 2); - Eigen::MatrixXd rect1(1, 2); - square1 << 1.0, 2.0, 2.0, 4.0; - rect1 << 0.0, 1.0; - const CSC_Matrix square_m1 = calCSCMatrixTrapezoidal(square1); - const CSC_Matrix rect_m1 = calCSCMatrix(rect1); - printCSCMatrix(square_m1); - printCSCMatrix(rect_m1); -} diff --git a/common/autoware_qp_interface/test/test_osqp_interface.cpp b/common/autoware_qp_interface/test/test_osqp_interface.cpp deleted file mode 100644 index f97cc2888afa0..0000000000000 --- a/common/autoware_qp_interface/test/test_osqp_interface.cpp +++ /dev/null @@ -1,170 +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 "autoware/qp_interface/osqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from https://github.com/osqp/osqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestOsqpInterface, BasicQp) -{ - using autoware::qp_interface::calCSCMatrix; - using autoware::qp_interface::calCSCMatrixTrapezoidal; - using autoware::qp_interface::CSC_Matrix; - - auto check_result = - [](const auto & solution, const std::string & status, const int polish_status) { - EXPECT_EQ(status, "OSQP_SOLVED"); - EXPECT_EQ(polish_status, 1); - - static const auto ep = 1.0e-8; - - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -autoware::qp_interface::OSQP_INF}; - const std::vector u = {1.0, 0.7, 0.7, autoware::qp_interface::OSQP_INF}; - - { - // Define problem during optimization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - // Define problem during initialization - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - const auto solution = osqp.QPInterface::optimize(P, A, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem - Eigen::MatrixXd P_ini = Eigen::MatrixXd::Zero(2, 2); - Eigen::MatrixXd A_ini = Eigen::MatrixXd::Zero(4, 2); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.QPInterface::optimize(P_ini, A_ini, q_ini, l_ini, u_ini); - } - - { - // Define problem during initialization with csc matrix - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - { - std::tuple, std::vector, int, int, int> result; - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(false, 4000, 1e-6); - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // add warm startup - { - // Dummy initial problem with csc matrix - CSC_Matrix P_ini_csc = calCSCMatrixTrapezoidal(Eigen::MatrixXd::Zero(2, 2)); - CSC_Matrix A_ini_csc = calCSCMatrix(Eigen::MatrixXd::Zero(4, 2)); - std::vector q_ini(2, 0.0); - std::vector l_ini(4, 0.0); - std::vector u_ini(4, 0.0); - autoware::qp_interface::OSQPInterface osqp(true, 4000, 1e-6); // enable warm start - osqp.optimize(P_ini_csc, A_ini_csc, q_ini, l_ini, u_ini); - - // Redefine problem before optimization - CSC_Matrix P_csc = calCSCMatrixTrapezoidal(P); - CSC_Matrix A_csc = calCSCMatrix(A); - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - - osqp.updateCheckTermination(1); - const auto primal_val = solution; - const auto dual_val = osqp.getDualSolution(); - for (size_t i = 0; i < primal_val.size(); ++i) { - std::cerr << primal_val.at(i) << std::endl; - } - osqp.setWarmStart(primal_val, dual_val); - } - - { - const auto solution = osqp.optimize(P_csc, A_csc, q, l, u); - const auto status = osqp.getStatus(); - const auto polish_status = osqp.getPolishStatus(); - check_result(solution, status, polish_status); - } - - // NOTE: This should be true, but currently optimize function reset the workspace, which - // disables warm start. - // EXPECT_EQ(osqp.getTakenIter(), 1); - } -} -} // namespace diff --git a/common/autoware_qp_interface/test/test_proxqp_interface.cpp b/common/autoware_qp_interface/test/test_proxqp_interface.cpp deleted file mode 100644 index e47af10c7aabd..0000000000000 --- a/common/autoware_qp_interface/test/test_proxqp_interface.cpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/qp_interface/proxqp_interface.hpp" -#include "gtest/gtest.h" - -#include - -#include -#include -#include -#include - -namespace -{ -// Problem taken from -// https://github.com/proxqp/proxqp/blob/master/tests/basic_qp/generate_problem.py -// -// min 1/2 * x' * P * x + q' * x -// s.t. lb <= A * x <= ub -// -// P = [4, 1], q = [1], A = [1, 1], lb = [ 1], ub = [1.0] -// [1, 2] [1] [1, 0] [ 0] [0.7] -// [0, 1] [ 0] [0.7] -// [0, 1] [-inf] [inf] -// -// The optimal solution is -// x = [0.3, 0.7]' -// y = [-2.9, 0.0, 0.2, 0.0]` -// obj = 1.88 - -TEST(TestProxqpInterface, BasicQp) -{ - auto check_result = [](const auto & solution, const std::string & status) { - EXPECT_EQ(status, "PROXQP_SOLVED"); - - static const auto ep = 1.0e-8; - ASSERT_EQ(solution.size(), size_t(2)); - EXPECT_NEAR(solution[0], 0.3, ep); - EXPECT_NEAR(solution[1], 0.7, ep); - }; - - const Eigen::MatrixXd P = (Eigen::MatrixXd(2, 2) << 4, 1, 1, 2).finished(); - const Eigen::MatrixXd A = (Eigen::MatrixXd(4, 2) << 1, 1, 1, 0, 0, 1, 0, 1).finished(); - const std::vector q = {1.0, 1.0}; - const std::vector l = {1.0, 0.0, 0.0, -std::numeric_limits::max()}; - const std::vector u = {1.0, 0.7, 0.7, std::numeric_limits::max()}; - - { - // Define problem during optimization - autoware::qp_interface::ProxQPInterface proxqp(false, 4000, 1e-9, 1e-9, false); - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - } - - { - // Define problem during optimization with warm start - autoware::qp_interface::ProxQPInterface proxqp(true, 4000, 1e-9, 1e-9, false); - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_NE(proxqp.getIterationNumber(), 1); - } - { - const auto solution = proxqp.QPInterface::optimize(P, A, q, l, u); - const auto status = proxqp.getStatus(); - check_result(solution, status); - EXPECT_EQ(proxqp.getIterationNumber(), 0); - } - } -} -} // namespace diff --git a/common/autoware_signal_processing/CHANGELOG.rst b/common/autoware_signal_processing/CHANGELOG.rst index b5b01e98e23a7..a9e05a6ff5c10 100644 --- a/common/autoware_signal_processing/CHANGELOG.rst +++ b/common/autoware_signal_processing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_signal_processing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_signal_processing/package.xml b/common/autoware_signal_processing/package.xml index b8d7c064112dd..00f934005f642 100644 --- a/common/autoware_signal_processing/package.xml +++ b/common/autoware_signal_processing/package.xml @@ -2,7 +2,7 @@ autoware_signal_processing - 0.40.0 + 0.41.0 The signal processing package Takayuki Murooka Takamasa Horibe diff --git a/common/autoware_test_utils/CHANGELOG.rst b/common/autoware_test_utils/CHANGELOG.rst index 950fdb09aa119..0eddee7112ec2 100644 --- a/common/autoware_test_utils/CHANGELOG.rst +++ b/common/autoware_test_utils/CHANGELOG.rst @@ -2,6 +2,24 @@ Changelog for package autoware_test_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset (`#9967 `_) + * feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset + * fix + --------- +* chore(autoware_test_utils): move rviz config to autoware_launch (`#9925 `_) +* feat(autoware_test_utils): add visualization and yaml dumper for PathWithLaneId (`#9841 `_) +* chore(autoware_test_utils): update test map (`#9664 `_) +* feat(autoware_test_utils): add visualization (`#9603 `_) +* refactor(autoware_test_utils): enhance makeMapBinMsg to accept package name and map filename parameters (`#9617 `_) + * feat: enhance makeMapBinMsg to accept package name and map filename parameters + * feat: set default package name to 'autoware_test_utils' in makeMapBinMsg and related functions + --------- +* Contributors: Fumiya Watanabe, Kyoichi Sugahara, Mamoru Sobue, Ryohsuke Mitsudome, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_test_utils/CMakeLists.txt b/common/autoware_test_utils/CMakeLists.txt index 8bfa44f7bf0ba..20b8f964f879f 100644 --- a/common/autoware_test_utils/CMakeLists.txt +++ b/common/autoware_test_utils/CMakeLists.txt @@ -25,5 +25,4 @@ ament_auto_package(INSTALL_TO_SHARE test_map test_data launch - rviz ) diff --git a/common/autoware_test_utils/config/sample_topic_snapshot.yaml b/common/autoware_test_utils/config/sample_topic_snapshot.yaml index d9eff5d579540..2ff746b84c3ac 100644 --- a/common/autoware_test_utils/config/sample_topic_snapshot.yaml +++ b/common/autoware_test_utils/config/sample_topic_snapshot.yaml @@ -24,6 +24,9 @@ fields: - name: dynamic_object type: PredictedObjects # autoware_perception_msgs::msg::PredictedObjects topic: /perception/object_recognition/objects -# - name: tracked_object -# type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects -# topic: /perception/object_recognition/tracking/objects + # - name: tracked_object + # type: TrackedObjects # autoware_perception_msgs::msg::TrackedObjects + # topic: /perception/object_recognition/tracking/objects + # - name: path_with_lane_id + # type: PathWithLaneId # tier4_planning_msgs::msg::PathWithLaneId + # topic: /planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index 3721dc67526b5..b98af2133f175 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -213,15 +213,21 @@ LaneletMapBin make_map_bin_msg( const std::string & absolute_path, const double center_line_resolution = 5.0); /** - * @brief Creates a LaneletMapBin message using a predefined Lanelet2 map file. + * @brief Creates a LaneletMapBin message using a Lanelet2 map file. * - * This function loads a lanelet2_map.osm from the test_map folder in the - * autoware_test_utils package, overwrites the centerline with a resolution of 5.0, + * This function loads a specified map file from the test_map folder in the + * specified package (or autoware_test_utils if no package is specified), + * overwrites the centerline with a resolution of 5.0, * and converts the map to a LaneletMapBin message. * + * @param package_name The name of the package containing the map file. If empty, defaults to + * "autoware_test_utils". + * @param map_filename The name of the map file (e.g. "lanelet2_map.osm") * @return A LaneletMapBin message containing the map data. */ -LaneletMapBin makeMapBinMsg(); +LaneletMapBin makeMapBinMsg( + const std::string & package_name = "autoware_test_utils", + const std::string & map_filename = "lanelet2_map.osm"); /** * @brief Creates an Odometry message with a specified shift. diff --git a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp index 5e07237e2c495..8375d3e731683 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/mock_data_parser.hpp @@ -59,6 +59,7 @@ using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::PathPoint; using builtin_interfaces::msg::Duration; using builtin_interfaces::msg::Time; using geometry_msgs::msg::Accel; @@ -97,6 +98,9 @@ Duration parse(const YAML::Node & node); template <> Time parse(const YAML::Node & node); +template <> +Point parse(const YAML::Node & node); + template <> std::vector parse(const YAML::Node & node); @@ -145,6 +149,15 @@ std::vector parse(const YAML::Node & node); template <> UUID parse(const YAML::Node & node); +template <> +PathPoint parse(const YAML::Node & node); + +template <> +PathPointWithLaneId parse(const YAML::Node & node); + +template <> +PathWithLaneId parse(const YAML::Node & node); + template <> PredictedPath parse(const YAML::Node & node); diff --git a/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp new file mode 100644 index 0000000000000..dbd3dd6638c95 --- /dev/null +++ b/common/autoware_test_utils/include/autoware_test_utils/visualization.hpp @@ -0,0 +1,328 @@ +// Copyright 2024 TIER IV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// NOTE(soblin): this file is intentionally inline to deal with link issue + +#ifndef AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ +#define AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::test_utils +{ + +struct PointConfig +{ + std::optional color{}; + std::optional marker{}; + std::optional marker_size{}; +}; + +struct LineConfig +{ + static constexpr const char * default_color = "blue"; + static LineConfig defaults() + { + return LineConfig{std::string(default_color), 1.0, "solid", std::nullopt}; + } + std::optional color{}; + std::optional linewidth{}; + std::optional linestyle{}; + std::optional label{}; +}; + +struct LaneConfig +{ + static LaneConfig defaults() { return LaneConfig{std::nullopt, LineConfig::defaults(), true}; } + + std::optional label{}; + std::optional line_config{}; + bool plot_centerline = true; // alpha{}; + std::optional color{}; + bool fill{true}; + std::optional linewidth{}; + std::optional point_config{}; +}; + +/** + * @brief plot the linestring by `axes.plot()` + * @param [in] config_opt argument for plotting the linestring. if valid, each field is used as the + * kwargs + */ +inline void plot_lanelet2_object( + const lanelet::ConstLineString3d & line, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + py::dict kwargs{}; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.color) { + kwargs["color"] = config.color.value(); + } + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + if (config.linestyle) { + kwargs["linestyle"] = config.linestyle.value(); + } + if (config.label) { + kwargs["label"] = config.label.value(); + } + } + std::vector xs; + for (const auto & p : line) { + xs.emplace_back(p.x()); + } + std::vector ys; + for (const auto & p : line) { + ys.emplace_back(p.y()); + } + axes.plot(Args(xs, ys), kwargs); +} + +/** + * @brief plot the left/right bounds and optionally centerline + * @param [in] args used for plotting the left/right bounds as + */ +inline void plot_lanelet2_object( + const lanelet::ConstLanelet & lanelet, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + const auto left = lanelet.leftBound(); + const auto right = lanelet.rightBound(); + + const auto line_config = [&]() -> std::optional { + if (!config_opt) { + return LineConfig{std::string(LineConfig::default_color)}; + } + return config_opt.value().line_config; + }(); + + if (config_opt) { + const auto & config = config_opt.value(); + + // plot lanelet centerline + if (config.plot_centerline) { + auto centerline_config = [&]() -> LineConfig { + if (!config.line_config) { + return LineConfig{"k", std::nullopt, "dashed"}; + } + auto cfg = config.line_config.value(); + cfg.color = "k"; + cfg.linestyle = "dashed"; + return cfg; + }(); + plot_lanelet2_object( + lanelet.centerline(), axes, std::make_optional(std::move(centerline_config))); + } + + // plot lanelet-id + const auto center = (left.front().basicPoint2d() + left.back().basicPoint2d() + + right.front().basicPoint2d() + right.back().basicPoint2d()) / + 4.0; + axes.text( + Args(center.x(), center.y(), std::to_string(lanelet.id())), Kwargs("clip_on"_a = true)); + } + + if (config_opt && config_opt.value().label) { + auto left_line_config_for_legend = line_config ? line_config.value() : LineConfig::defaults(); + left_line_config_for_legend.label = config_opt.value().label.value(); + + // plot left + plot_lanelet2_object(lanelet.leftBound(), axes, left_line_config_for_legend); + + // plot right + plot_lanelet2_object(lanelet.rightBound(), axes, line_config); + } else { + // plot left + plot_lanelet2_object(lanelet.leftBound(), axes, line_config); + + // plot right + plot_lanelet2_object(lanelet.rightBound(), axes, line_config); + } + + // plot centerline direction + const auto centerline_size = lanelet.centerline().size(); + const auto mid_index = centerline_size / 2; + const auto before = static_cast(std::max(0, mid_index - 1)); + const auto after = static_cast(std::min(centerline_size - 1, mid_index + 1)); + const auto p_before = lanelet.centerline()[before]; + const auto p_after = lanelet.centerline()[after]; + const double azimuth = std::atan2(p_after.y() - p_before.y(), p_after.x() - p_before.x()); + const auto & mid = lanelet.centerline()[mid_index]; + axes.quiver( + Args(mid.x(), mid.y(), std::cos(azimuth), std::sin(azimuth)), Kwargs("units"_a = "width")); +} + +/** + * @brief plot the polygon as matplotlib.patches.Polygon + * @param [in] config_opt argument for plotting the polygon. if valid, each field is used as the + * kwargs + */ +inline void plot_lanelet2_object( + const lanelet::ConstPolygon3d & polygon, autoware::pyplot::Axes & axes, + const std::optional & config_opt = std::nullopt) +{ + std::vector> xy(polygon.size()); + for (unsigned i = 0; i < polygon.size(); ++i) { + xy.at(i) = std::vector({polygon[i].x(), polygon[i].y()}); + } + py::dict kwargs; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.alpha) { + kwargs["alpha"] = config.alpha.value(); + } + if (config.color) { + kwargs["color"] = config.color.value(); + } + kwargs["fill"] = config.fill; + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + } + auto poly = autoware::pyplot::Polygon(Args(xy), kwargs); + axes.add_patch(Args(poly.unwrap())); +} + +struct DrivableAreaConfig +{ + static DrivableAreaConfig defaults() { return {"turquoise", 2.0}; } + std::optional color{}; + std::optional linewidth{}; +}; + +struct PathWithLaneIdConfig +{ + static PathWithLaneIdConfig defaults() + { + return {std::nullopt, "k", 1.0, std::nullopt, false, 1.0}; + } + std::optional label{}; + std::optional color{}; + std::optional linewidth{}; + std::optional da{}; + bool lane_id{}; // & config_opt = std::nullopt) +{ + py::dict kwargs{}; + if (config_opt) { + const auto & config = config_opt.value(); + if (config.label) { + kwargs["label"] = config.label.value(); + } + if (config.color) { + kwargs["color"] = config.color.value(); + } + if (config.linewidth) { + kwargs["linewidth"] = config.linewidth.value(); + } + } + std::vector xs; + std::vector ys; + std::vector yaw_cos; + std::vector yaw_sin; + std::vector> ids; + const bool plot_lane_id = config_opt ? config_opt.value().lane_id : false; + for (const auto & point : path.points) { + xs.push_back(point.point.pose.position.x); + ys.push_back(point.point.pose.position.y); + const auto th = autoware::universe_utils::getRPY(point.point.pose.orientation).z; + yaw_cos.push_back(std::cos(th)); + yaw_sin.push_back(std::sin(th)); + if (plot_lane_id) { + ids.emplace_back(); + for (const auto & id : point.lane_ids) { + ids.back().push_back(id); + } + } + } + // plot centerline + axes.plot(Args(xs, ys), kwargs); + const auto quiver_scale = + config_opt ? config_opt.value().quiver_size : PathWithLaneIdConfig::defaults().quiver_size; + const auto quiver_color = + config_opt ? (config_opt.value().color ? config_opt.value().color.value() : "k") : "k"; + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), Kwargs( + "angles"_a = "xy", "scale_units"_a = "xy", + "scale"_a = quiver_scale, "color"_a = quiver_color)); + if (plot_lane_id) { + for (size_t i = 0; i < xs.size(); ++i) { + std::stringstream ss; + const char * delimiter = ""; + for (const auto id : ids[i]) { + ss << std::exchange(delimiter, ",") << id; + } + axes.text(Args(xs[i], ys[i], ss.str()), Kwargs("clip_on"_a = true)); + } + } + // plot drivable area + if (config_opt && config_opt.value().da) { + auto plot_boundary = [&](const decltype(path.left_bound) & points) { + std::vector xs; + std::vector ys; + for (const auto & point : points) { + xs.push_back(point.x); + ys.push_back(point.y); + } + const auto & cfg = config_opt.value().da.value(); + py::dict kwargs{}; + if (cfg.color) { + kwargs["color"] = cfg.color.value(); + } + if (cfg.linewidth) { + kwargs["linewidth"] = cfg.linewidth.value(); + } + axes.plot(Args(xs, ys), kwargs); + }; + plot_boundary(path.left_bound); + plot_boundary(path.right_bound); + } +} + +} // namespace autoware::test_utils + +#endif // AUTOWARE_TEST_UTILS__VISUALIZATION_HPP_ diff --git a/common/autoware_test_utils/launch/psim_intersection.launch.xml b/common/autoware_test_utils/launch/psim_intersection.launch.xml index 723dd7352219d..dc3e75cca1d96 100644 --- a/common/autoware_test_utils/launch/psim_intersection.launch.xml +++ b/common/autoware_test_utils/launch/psim_intersection.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml index 7dd888a036f9d..58b71524e72b8 100644 --- a/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml +++ b/common/autoware_test_utils/launch/psim_road_shoulder.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/common/autoware_test_utils/package.xml b/common/autoware_test_utils/package.xml index 22572abb72b69..03846e620bedc 100644 --- a/common/autoware_test_utils/package.xml +++ b/common/autoware_test_utils/package.xml @@ -2,12 +2,13 @@ autoware_test_utils - 0.40.0 + 0.41.0 ROS 2 node for testing interface of the nodes in planning module Kyoichi Sugahara Takamasa Horibe Zulfaqar Azmi Mamoru Sobue + Yukinari Hisaki Apache License 2.0 Kyoichi Sugahara @@ -16,13 +17,14 @@ autoware_cmake ament_index_cpp - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_control_msgs autoware_lanelet2_extension autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_pyplot autoware_universe_utils autoware_vehicle_msgs lanelet2_io @@ -31,9 +33,7 @@ std_srvs tf2_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs - tier4_v2x_msgs unique_identifier_msgs yaml_cpp_vendor diff --git a/common/autoware_test_utils/rviz/psim_test_map.rviz b/common/autoware_test_utils/rviz/psim_test_map.rviz deleted file mode 100644 index 8475c94b86bb4..0000000000000 --- a/common/autoware_test_utils/rviz/psim_test_map.rviz +++ /dev/null @@ -1,4569 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 - Splitter Ratio: 0.557669460773468 - Tree Height: 225 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: ~ - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: AutowareDateTimePanel - Name: AutowareDateTimePanel - - Class: rviz_plugins::AutowareStatePanel - Name: AutowareStatePanel - - Class: rviz_plugins::SimulatedClockPanel - Name: SimulatedClockPanel - - Class: rviz_plugins::TrafficLightPublishPanel - Name: TrafficLightPublishPanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - - Class: rviz_common/Group - Displays: - - Alpha: 0.30000001192092896 - Class: rviz_default_plugins/RobotModel - Collision Enabled: false - Description File: "" - Description Source: Topic - Description Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /robot_description - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - ars408_front_center: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera0/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera1/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera1/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera2/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera2/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera3/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera3/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera4/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera4/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera5/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera5/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera6/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera6/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera7/camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera7/camera_optical_link: - Alpha: 1 - Show Axes: false - Show Trail: false - gnss_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_front_left: - Alpha: 1 - Show Axes: false - Show Trail: false - livox_front_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - livox_front_right: - Alpha: 1 - Show Axes: false - Show Trail: false - livox_front_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - sensor_kit_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tamagawa/imu_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_left_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_rear_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_right_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - velodyne_top_base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Mass Properties: - Inertia: false - Mass: false - Name: VehicleModel - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true - - Class: rviz_plugins/PolarGridDisplay - Color: 255; 255; 255 - Delta Range: 10 - Enabled: true - Max Alpha: 0.5 - Max Range: 100 - Max Wave Alpha: 0.5 - Min Alpha: 0.009999999776482582 - Min Wave Alpha: 0.009999999776482582 - Name: PolarGridDisplay - Reference Frame: base_link - Value: true - Wave Color: 255; 255; 255 - Wave Velocity: 40 - - Background Alpha: 0.5 - Background Color: 23; 28; 31 - Class: autoware_overlay_rviz_plugin/SignalDisplay - Dark Traffic Color: 255; 51; 51 - Enabled: true - Gear Topic Test: /vehicle/status/gear_status - Handle Angle Scale: 17 - Hazard Lights Topic: /vehicle/status/hazard_lights_status - Height: 100 - Left: 0 - Light Traffic Color: 255; 153; 153 - Name: SignalDisplay - Primary Color: 174; 174; 174 - Signal Color: 0; 230; 120 - Speed Limit Topic: /planning/scenario_planning/current_max_velocity - Speed Topic: /vehicle/status/velocity_status - Steering Topic: /vehicle/status/steering_status - Top: 10 - Traffic Topic: /planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal - Turn Signals Topic: /vehicle/status/turn_indicators_status - Value: true - Width: 550 - Enabled: true - Name: Vehicle - - Class: rviz_plugins/MrmSummaryOverlayDisplay - Enabled: false - Font Size: 10 - Left: 512 - Max Letter Num: 100 - Name: MRM Summary - Text Color: 25; 255; 240 - Top: 64 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /system/emergency/hazard_status - Value: false - Value height offset: 0 - Enabled: true - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - bus_stop_area: true - center_lane_line: false - center_line_arrows: false - curbstone: true - lane_start_bound: false - lanelet direction: true - lanelet_id: false - left_lane_bound: true - no_parking_area: true - parking_lots: true - parking_space: true - partitions: true - right_lane_bound: true - road_lanelets: false - shoulder_center_lane_line: false - shoulder_center_line_arrows: true - shoulder_lane_start_bound: true - shoulder_left_lane_bound: true - shoulder_right_lane_bound: true - shoulder_road_lanelets: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.4000000059604645 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - Alpha: 0.9990000128746033 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: MeasurementRange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/lidar/crop_box_filter/crop_box_polygon - Value: false - Enabled: true - Name: LiDAR - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 233; 185; 110 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: false - Position: - Alpha: 0.20000000298023224 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Head Length: 0.699999988079071 - Head Radius: 1.2000000476837158 - Name: PoseWithCovariance - Shaft Length: 1 - Shaft Radius: 0.5 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /sensing/gnss/pose_with_covariance - Value: true - Enabled: false - Name: GNSS - Enabled: true - Name: Sensing - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 170; 255 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovInitial - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/initial_pose_with_covariance - Value: false - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz_default_plugins/PoseWithCovariance - Color: 0; 255; 0 - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: false - Head Length: 0.4000000059604645 - Head Radius: 0.6000000238418579 - Name: PoseWithCovAligned - Shaft Length: 0.6000000238418579 - Shaft Radius: 0.30000001192092896 - Shape: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose_with_covariance - Value: false - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: false - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 0; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Initial - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /localization/util/downsample/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: false - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: Aligned - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MonteCarloInitialPose - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/monte_carlo_initial_pose_marker - Value: true - Enabled: true - Name: NDT - - Class: rviz_common/Group - Displays: - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: PoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: EKF - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 15 - Min Value: -2 - Value: false - Axis: Z - Channel Name: z - Class: rviz_default_plugins/PointCloud2 - Color: 200; 200; 200 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 15 - Min Color: 0; 0; 0 - Min Intensity: -5 - Name: NoGroundPointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/obstacle_segmentation/pointcloud - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Name: Segmentation - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: DetectedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Detection - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: TrackedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Tracking - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Class: autoware_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 119; 11; 32 - Name: PredictedObjects - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 192; 203 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 30; 144; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Maneuver - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/prediction/maneuver - Value: false - Enabled: true - Name: Prediction - Enabled: true - Name: ObjectRecognition - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Image - Enabled: false - Max Value: 1 - Median window: 5 - Min Value: 0 - Name: RecognitionResultOnImage - Normalize Range: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light/debug/rois - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MapBasedDetectionResult - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers - Value: true - Enabled: true - Name: TrafficLight - - Class: rviz_common/Group - Displays: - - Alpha: 0.5 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/occupancy_grid_map/map_updates - Use Timestamp: false - Value: true - Enabled: false - Name: OccupancyGrid - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: RouteArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/route_marker - Value: true - - Alpha: 0.9990000128746033 - Axes Length: 1 - Axes Radius: 0.30000001192092896 - Class: rviz_default_plugins/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.5 - Name: GoalPose - Shaft Length: 3 - Shaft Radius: 0.20000000298023224 - Shape: Axes - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/echo_back_goal_pose - Value: true - - Background Alpha: 0.5 - Background Color: 23; 28; 31 - Class: autoware_mission_details_overlay_rviz_plugin/MissionDetailsDisplay - Enabled: true - Height: 100 - Name: MissionDetailsDisplay - Remaining Distance and Time Topic: /planning/mission_remaining_distance_time - Right: 10 - Text Color: 194; 194; 194 - Top: 10 - Value: true - Width: 170 - Enabled: true - Name: MissionPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: ScenarioTrajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/trajectory - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.9990000128746033 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: Path - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/path - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.4000000059604645 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.4000000059604645 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_AvoidanceByLC - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/avoidance_by_lane_change - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/static_obstacle_avoidance - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_LaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/lane_change_right - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_LaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/lane_change_left - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_GoalPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/goal_planner - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: PathReference_StartPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_reference/start_planner - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_AvoidanceByLC - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/avoidance_by_lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: (old)PathChangeCandidate_LaneChange - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_LaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeRight - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/external_request_lane_change_right - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_ExternalRequestLaneChangeLeft - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/external_request_lane_change_left - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_Avoidance - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/static_obstacle_avoidance - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_StartPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/start_planner - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: true - Name: PathChangeCandidate_GoalPlanner - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/path_candidate/goal_planner - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.30000001192092896 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.30000001192092896 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Bound - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/bound - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StaticObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/static_obstacle_avoidance - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (AvoidanceByLC) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/avoidance_by_lane_change - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (LaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (LaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ExtLaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ExtLaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/external_request_lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (GoalPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/goal_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StartPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/virtual_wall/start_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (BlindSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Crosswalk) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Walkway) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/walkway - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DetectionArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (Intersection) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MergeFromPrivateArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoStoppingArea) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OcclusionSpot) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (StopLine) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (TrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (VirtualTrafficLight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (RunOut) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SpeedBump) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/speed_bump - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (NoDrivableLane) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_drivable_lane - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Arrow - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Blind Spot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: VirtualTrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StopLine - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: OcclusionSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RunOut - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StaticObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/static_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LeftLaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: RightLaneChange - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: LaneFollowing - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_following - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: GoalPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/goal_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SideShift - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/side_shift - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: SpeedBump - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/speed_bump - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: DynamicObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/dynamic_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: NoDrivableLane - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_drivable_lane - Value: false - Enabled: false - Name: DebugMarker - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (StaticObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/static_obstacle_avoidance - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (AvoidanceByLC) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/avoidance_by_lane_change - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (LaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (LaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (ExtLaneChangeLeft) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_left - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (ExtLaneChangeRight) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/external_request_lane_change_right - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (GoalPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/goal_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (StartPlanner) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/start_planner - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Info (DynamicObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/info/dynamic_obstacle_avoidance - Value: false - Enabled: false - Name: InfoMarker - Enabled: true - Name: BehaviorPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/trajectory - Value: false - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 0.9990000128746033 - Constant Width: false - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 0.9990000128746033 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (SurroundObstacle) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleAvoidance) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleCruise) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (MotionVelocitySmoother) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/velocity_smoother/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (OutOfLane) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/virtual_walls - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (ObstacleVelocityLimiter) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/virtual_walls - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (DynamicObstacleStop) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/virtual_walls - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SurroundObstacleCheck - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker - Value: true - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 25; 255; 0 - Enabled: false - Name: Footprint - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 239; 41; 41 - Enabled: false - Name: FootprintOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset - Value: false - - Alpha: 1 - Class: rviz_default_plugins/Polygon - Color: 10; 21; 255 - Enabled: false - Name: FootprintRecoverOffset - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset - Value: false - Enabled: false - Name: SurroundObstacleChecker - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: CruiseVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: SlowDownVirtualWall - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/slow_down/virtual_wall - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DebugMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/marker - Value: true - Enabled: false - Name: ObstacleCruise - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: ObstacleAvoidance - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: OutOfLane - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/out_of_lane/debug_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: ObstacleVelocityLimiter - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/obstacle_velocity_limiter/debug_markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DynamicObstacleStop - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/motion_planning/motion_velocity_planner/dynamic_obstacle_stop/debug_markers - Value: true - Enabled: false - Name: MotionVelocityPlanner - Enabled: false - Name: DebugMarker - Enabled: true - Name: MotionPlanning - Enabled: true - Name: LaneDriving - - Class: rviz_common/Group - Displays: - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: false - Enabled: false - Name: Costmap - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid - Update Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates - Use Timestamp: false - Value: false - - Alpha: 0.9990000128746033 - Arrow Length: 0.30000001192092896 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 255; 25; 0 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PartialPoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (3D) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array - Value: true - - Alpha: 0.9990000128746033 - Arrow Length: 0.5 - Axes Length: 0.30000001192092896 - Axes Radius: 0.009999999776482582 - Class: rviz_default_plugins/PoseArray - Color: 0; 0; 255 - Enabled: true - Head Length: 0.10000000149011612 - Head Radius: 0.20000000298023224 - Name: PoseArray - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.05000000074505806 - Shape: Arrow (Flat) - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array - Value: true - Enabled: true - Name: Parking - - Class: rviz_plugins/PoseWithUuidStamped - Enabled: true - Length: 1.5 - Name: ModifiedGoal - Radius: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/scenario_planning/modified_goal - UUID: - Scale: 0.30000001192092896 - Value: false - Value: true - Enabled: true - Name: ScenarioPlanning - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: PlanningErrorMarker - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker - Value: true - Enabled: false - Name: Diagnostic - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Predicted Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/predicted_trajectory_in_frenet_coordinate - Value: true - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Constant Width: true - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: true - Width: 0.05000000074505806 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: false - Name: Resampled Reference Trajectory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/resampled_reference_trajectory - Value: false - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: false - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Constant Width: true - Fade Out Distance: 0 - Max Velocity Color: 0; 230; 120 - Mid Velocity Color: 32; 138; 174 - Min Velocity Color: 63; 46; 227 - Value: false - Width: 0.20000000298023224 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: true - View Text Slope: - Scale: 0.30000001192092896 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: VirtualWall (AEB) - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/autonomous_emergency_braking/virtual_wall - Value: true - Enabled: true - Name: VirtualWall - - Class: rviz_default_plugins/Marker - Enabled: false - Name: Stop Reason - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/longitudinal/stop_reason - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/MPC - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/mpc_follower/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/PurePursuit - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/trajectory_follower/controller_node_exe/debug/markers - Value: false - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: Debug/AEB - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /control/autonomous_emergency_braking/debug/markers - Value: false - Enabled: true - Name: Control - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: ~ - Enabled: true - Name: Map - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/Camera - Enabled: true - Far Plane Distance: 100 - Image Rendering: background and overlay - Name: PointcloudOnCamera - Overlay Alpha: 0.5 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/camera/camera6/image_raw - Value: true - Visibility: - Control: - Debug/AEB: true - Debug/MPC: true - Debug/PurePursuit: true - Predicted Trajectory: true - Resampled Reference Trajectory: true - Stop Reason: true - Value: false - VirtualWall: - Value: true - VirtualWall (AEB): true - Debug: - Control: true - Localization: - "": true - Value: true - Map: true - Perception: - "": true - Value: true - Planning: - "": - "": true - Value: true - Value: true - Sensing: - ConcatenatePointCloud: true - RadarRawObjects(white): true - Value: true - Value: true - Localization: - EKF: - PoseHistory: true - Value: true - NDT: - Aligned: true - Initial: true - MonteCarloInitialPose: true - PoseHistory: true - PoseWithCovAligned: true - PoseWithCovInitial: true - Value: true - Value: false - Map: - Lanelet2VectorMap: true - PointCloudMap: true - Value: false - Perception: - ObjectRecognition: - Detection: - DetectedObjects: true - Value: true - Prediction: - Maneuver: true - PredictedObjects: true - Value: true - Tracking: - TrackedObjects: true - Value: true - Value: true - OccupancyGrid: - Map: true - Value: true - Segmentation: - NoGroundPointCloud: true - Value: true - TrafficLight: - MapBasedDetectionResult: true - RecognitionResultOnImage: true - Value: true - Value: false - Planning: - Diagnostic: - PlanningErrorMarker: true - Value: true - MissionPlanning: - GoalPose: true - MissionDetailsDisplay: true - RouteArea: true - Value: true - ScenarioPlanning: - LaneDriving: - BehaviorPlanning: - (old)PathChangeCandidate_LaneChange: true - Bound: true - DebugMarker: - Arrow: true - Blind Spot: true - Crosswalk: true - DetectionArea: true - DynamicObstacleAvoidance: true - GoalPlanner: true - Intersection: true - LaneFollowing: true - LeftLaneChange: true - NoDrivableLane: true - NoStoppingArea: true - OcclusionSpot: true - RightLaneChange: true - RunOut: true - SideShift: true - SpeedBump: true - StartPlanner: true - StaticObstacleAvoidance: true - StopLine: true - TrafficLight: true - Value: true - VirtualTrafficLight: true - InfoMarker: - Info (AvoidanceByLC): true - Info (DynamicObstacleAvoidance): true - Info (ExtLaneChangeLeft): true - Info (ExtLaneChangeRight): true - Info (GoalPlanner): true - Info (LaneChangeLeft): true - Info (LaneChangeRight): true - Info (StartPlanner): true - Info (StaticObstacleAvoidance): true - Value: true - Path: true - PathChangeCandidate_Avoidance: true - PathChangeCandidate_AvoidanceByLC: true - PathChangeCandidate_ExternalRequestLaneChangeLeft: true - PathChangeCandidate_ExternalRequestLaneChangeRight: true - PathChangeCandidate_GoalPlanner: true - PathChangeCandidate_LaneChangeLeft: true - PathChangeCandidate_LaneChangeRight: true - PathChangeCandidate_StartPlanner: true - PathReference_Avoidance: true - PathReference_AvoidanceByLC: true - PathReference_GoalPlanner: true - PathReference_LaneChangeLeft: true - PathReference_LaneChangeRight: true - PathReference_StartPlanner: true - Value: true - VirtualWall: - Value: true - VirtualWall (AvoidanceByLC): true - VirtualWall (BlindSpot): true - VirtualWall (Crosswalk): true - VirtualWall (DetectionArea): true - VirtualWall (ExtLaneChangeLeft): true - VirtualWall (ExtLaneChangeRight): true - VirtualWall (GoalPlanner): true - VirtualWall (Intersection): true - VirtualWall (LaneChangeLeft): true - VirtualWall (LaneChangeRight): true - VirtualWall (MergeFromPrivateArea): true - VirtualWall (NoDrivableLane): true - VirtualWall (NoStoppingArea): true - VirtualWall (OcclusionSpot): true - VirtualWall (RunOut): true - VirtualWall (SpeedBump): true - VirtualWall (StartPlanner): true - VirtualWall (StaticObstacleAvoidance): true - VirtualWall (StopLine): true - VirtualWall (TrafficLight): true - VirtualWall (VirtualTrafficLight): true - VirtualWall (Walkway): true - MotionPlanning: - DebugMarker: - MotionVelocityPlanner: - DynamicObstacleStop: true - ObstacleVelocityLimiter: true - OutOfLane: true - Value: true - ObstacleAvoidance: true - ObstacleCruise: - CruiseVirtualWall: true - DebugMarker: true - SlowDownVirtualWall: true - Value: true - ObstacleStop: true - SurroundObstacleChecker: - Footprint: true - FootprintOffset: true - FootprintRecoverOffset: true - SurroundObstacleCheck: true - Value: true - Value: true - Trajectory: true - Value: true - VirtualWall: - Value: true - VirtualWall (DynamicObstacleStop): true - VirtualWall (MotionVelocitySmoother): true - VirtualWall (ObstacleAvoidance): true - VirtualWall (ObstacleCruise): true - VirtualWall (ObstacleStop): true - VirtualWall (ObstacleVelocityLimiter): true - VirtualWall (OutOfLane): true - VirtualWall (SurroundObstacle): true - Value: true - ModifiedGoal: true - Parking: - Costmap: true - PartialPoseArray: true - PoseArray: true - Value: true - ScenarioTrajectory: true - Value: true - Value: false - Sensing: - GNSS: - PoseWithCovariance: true - Value: true - LiDAR: - ConcatenatePointCloud: true - MeasurementRange: true - Value: true - Value: true - System: - Grid: true - MRM Summary: true - TF: true - Value: false - Vehicle: - PolarGridDisplay: true - SignalDisplay: true - Value: true - VehicleModel: true - Value: true - Zoom Factor: 1 - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 5 - Min Value: -1 - Value: false - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: AxisColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: ConcatenatePointCloud - Position Transformer: XYZ - Selectable: false - Size (Pixels): 3 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/lidar/concatenated/pointcloud - Use Fixed Frame: false - Use rainbow: true - Value: true - - BUS: - Alpha: 0.5 - Color: 255; 255; 255 - CAR: - Alpha: 0.5 - Color: 255; 255; 255 - CYCLIST: - Alpha: 0.5 - Color: 255; 255; 255 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.029999999329447746 - MOTORCYCLE: - Alpha: 0.5 - Color: 255; 255; 255 - Name: RadarRawObjects(white) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.5 - Color: 255; 255; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.5 - Color: 255; 255; 255 - TRUCK: - Alpha: 0.5 - Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /sensing/radar/detected_objects - UNKNOWN: - Alpha: 0.5 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - Enabled: false - Name: Sensing - - Class: rviz_common/Group - Displays: - - Alpha: 0.9990000128746033 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 85; 255; 0 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 85; 255; 127 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NDT pointclouds - Position Transformer: XYZ - Selectable: false - Size (Pixels): 10 - Size (m): 0.5 - Style: Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/points_aligned - Use Fixed Frame: true - Use rainbow: true - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: "" - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: NDTLoadedPCDMap - Position Transformer: "" - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/debug/loaded_pointcloud_map - Use Fixed Frame: true - Use rainbow: true - Value: true - - Buffer Size: 200 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 170; 255; 127 - Value: true - Width: 0.10000000149011612 - Name: NDTPoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_estimator/pose - Value: true - - Buffer Size: 1000 - Class: rviz_plugins::PoseHistory - Enabled: true - Line: - Alpha: 0.9990000128746033 - Color: 0; 255; 255 - Value: true - Width: 0.10000000149011612 - Name: EKFPoseHistory - Topic: - Depth: 5 - Durability Policy: Volatile - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /localization/pose_twist_fusion_filter/pose - Value: true - Enabled: true - Name: Localization - - Class: rviz_common/Group - Displays: - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Name: Centerpoint(red1) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 138; 128 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Name: CenterpointROIFusion(red2) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/roi_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 82; 82 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Name: CenterpointValidator(red3) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/centerpoint/validation/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 213; 0; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - CAR: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Name: PointPainting(light_green1) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - TRUCK: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 178; 255; 89 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - CAR: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Name: PointPaintingROIFusion(light_green2) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - TRUCK: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/roi_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 118; 255; 3 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - CAR: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Name: PointPaintingValidator(light_green3) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - TRUCK: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/pointpainting/validation/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 100; 221; 23 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Name: DetectionByTracker(orange) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/detection_by_tracker/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 145; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - CAR: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Name: CameraLidarFusion(purple) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - TRUCK: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/clustering/camera_lidar_fusion/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 213; 0; 249 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Name: RadarFarObjects(white) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/radar/far_objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 255; 255 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - CAR: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Class: autoware_perception_rviz_plugin/DetectedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Name: Detection(yellow) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - TRUCK: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/detection/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 255; 234; 0 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - CAR: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Class: autoware_perception_rviz_plugin/TrackedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Dynamic Status: All - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Name: Tracking(green) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - TRUCK: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/tracking/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 0; 230; 118 - Value: true - Visualization Type: Normal - - BUS: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - CAR: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - CYCLIST: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Class: autoware_perception_rviz_plugin/PredictedObjects - Confidence Interval: 95% - Display Acceleration: true - Display Existence Probability: false - Display Label: true - Display Pose Covariance: true - Display Predicted Path Confidence: true - Display Predicted Paths: true - Display Twist: true - Display Twist Covariance: false - Display UUID: true - Display Velocity: true - Display Yaw Covariance: false - Display Yaw Rate: false - Display Yaw Rate Covariance: false - Enabled: true - Line Width: 0.10000000149011612 - MOTORCYCLE: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Name: Prediction(light_blue) - Namespaces: - {} - Object Fill Type: skeleton - PEDESTRIAN: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Polygon Type: 3d - TRAILER: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - TRUCK: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Best Effort - Value: /perception/object_recognition/objects - UNKNOWN: - Alpha: 0.9990000128746033 - Color: 0; 176; 255 - Value: true - Visualization Type: Normal - Enabled: true - Name: Perception - - Class: rviz_common/Group - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: LaneChangeLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/lane_change_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: LaneChangeRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/lane_change_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/static_obstacle_avoidance_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceByLCLeft - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/avoidance_by_lc_left - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: AvoidanceByLCRight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/avoidance_by_lc_right - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: StartPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/start_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: GoalPlanner - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/goal_planner - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Crosswalk - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/crosswalk - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Intersection - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/intersection - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: BlindSpot - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/blind_spot - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: TrafficLight - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/traffic_light - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: DetectionArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/detection_area - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: NoStoppingArea - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/debug/objects_of_interest/no_stopping_area - Value: true - Enabled: true - Name: Objects Of Interest - Enabled: true - Name: Planning - - Class: rviz_common/Group - Displays: ~ - Enabled: true - Name: Control - Enabled: false - Name: Debug - Enabled: true - Global Options: - Background Color: 15; 20; 23 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Class: tier4_adapi_rviz_plugins::RouteTool - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /rviz/routing/rough_goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 3 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareDateTimePanel: - collapsed: false - AutowareStatePanel: - collapsed: false - Displays: - collapsed: false - Height: 939 - Hide Left Dock: false - Hide Right Dock: false - PointcloudOnCamera: - collapsed: false - QMainWindow State: 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 - RecognitionResultOnImage: - collapsed: false - Selection: - collapsed: false - SimulatedClockPanel: - collapsed: false - Tool Properties: - collapsed: false - TrafficLightPublishPanel: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 34 diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index d42a5219e374c..08c4a3b5e9264 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -180,10 +180,9 @@ LaneletMapBin make_map_bin_msg( return map_bin_msg; } -LaneletMapBin makeMapBinMsg() +LaneletMapBin makeMapBinMsg(const std::string & package_name, const std::string & map_filename) { - return make_map_bin_msg( - get_absolute_path_to_lanelet_map("autoware_test_utils", "lanelet2_map.osm")); + return make_map_bin_msg(get_absolute_path_to_lanelet_map(package_name, map_filename)); } Odometry makeOdometry(const double shift) diff --git a/common/autoware_test_utils/src/mock_data_parser.cpp b/common/autoware_test_utils/src/mock_data_parser.cpp index 6e7002501bf30..29aed75357441 100644 --- a/common/autoware_test_utils/src/mock_data_parser.cpp +++ b/common/autoware_test_utils/src/mock_data_parser.cpp @@ -65,6 +65,16 @@ std::array parse(const YAML::Node & node) return msg; } +template <> +Point parse(const YAML::Node & node) +{ + Point geom_point; + geom_point.x = node["x"].as(); + geom_point.y = node["y"].as(); + geom_point.z = node["z"].as(); + return geom_point; +} + template <> std::vector parse(const YAML::Node & node) { @@ -285,6 +295,46 @@ Shape parse(const YAML::Node & node) return msg; } +template <> +PathPoint parse(const YAML::Node & node) +{ + PathPoint point; + point.pose = parse(node["pose"]); + point.longitudinal_velocity_mps = node["longitudinal_velocity_mps"].as(); + point.lateral_velocity_mps = node["lateral_velocity_mps"].as(); + point.heading_rate_rps = node["heading_rate_rps"].as(); + point.is_final = node["is_final"].as(); + return point; +} + +template <> +PathPointWithLaneId parse(const YAML::Node & node) +{ + PathPointWithLaneId point; + point.point = parse(node["point"]); + for (const auto & lane_id_node : node["lane_ids"]) { + point.lane_ids.push_back(lane_id_node.as()); + } + return point; +} + +template <> +PathWithLaneId parse(const YAML::Node & node) +{ + PathWithLaneId path; + path.header = parse
(node["header"]); + for (const auto & point_node : node["points"]) { + path.points.push_back(parse(point_node)); + } + for (const auto & left_bound_node : node["left_bound"]) { + path.left_bound.push_back(parse(left_bound_node)); + } + for (const auto & right_bound_node : node["right_bound"]) { + path.right_bound.push_back(parse(right_bound_node)); + } + return path; +} + template <> PredictedPath parse(const YAML::Node & node) { diff --git a/common/autoware_test_utils/src/topic_snapshot_saver.cpp b/common/autoware_test_utils/src/topic_snapshot_saver.cpp index b2cb2a17c9621..bd9af2d5672b8 100644 --- a/common/autoware_test_utils/src/topic_snapshot_saver.cpp +++ b/common/autoware_test_utils/src/topic_snapshot_saver.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include @@ -46,7 +47,8 @@ using MessageType = std::variant< autoware_adapi_v1_msgs::msg::OperationModeState, // 3 autoware_planning_msgs::msg::LaneletRoute, // 4 autoware_perception_msgs::msg::TrafficLightGroupArray, // 5 - autoware_perception_msgs::msg::TrackedObjects // 6 + autoware_perception_msgs::msg::TrackedObjects, // 6 + tier4_planning_msgs::msg::PathWithLaneId // 7 >; std::optional get_topic_index(const std::string & name) @@ -72,6 +74,9 @@ std::optional get_topic_index(const std::string & name) if (name == "TrackedObjects") { return 6; } + if (name == "PathWithLaneId") { + return 7; + } return std::nullopt; } @@ -196,6 +201,7 @@ class TopicSnapShotSaver REGISTER_CALLBACK(4); REGISTER_CALLBACK(5); REGISTER_CALLBACK(6); + REGISTER_CALLBACK(7); } } @@ -243,6 +249,7 @@ class TopicSnapShotSaver REGISTER_WRITE_TYPE(4); REGISTER_WRITE_TYPE(5); REGISTER_WRITE_TYPE(6); + REGISTER_WRITE_TYPE(7); } const std::string desc = std::string(R"(# @@ -253,7 +260,7 @@ class TopicSnapShotSaver # map_path_uri: package:/// # fields(this is array) # - name: -# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD} +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} # topic: # )"); diff --git a/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm b/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm index c5c5b7b1fc657..9f28ed79a91ae 100644 --- a/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm +++ b/common/autoware_test_utils/test_map/intersection/lanelet2_map.osm @@ -1,6 +1,6 @@ - + @@ -8002,391 +8002,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -10004,16 +9619,6 @@ - - - - - - - - - - @@ -15586,6 +15191,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -18669,142 +18449,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -19364,12 +19008,6 @@ - - - - - - @@ -21131,6 +20769,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -22269,15 +21969,6 @@ - - - - - - - - - @@ -22853,6 +22544,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/common/autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm b/common/autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm index 09c6297f83752..2c86dad61875f 100644 --- a/common/autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm +++ b/common/autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm @@ -1,6 +1,6 @@ - + @@ -2336,31 +2336,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - @@ -2771,316 +2746,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -3831,41 +3496,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -47975,9 +47605,9 @@ - - - + + + @@ -48018,8 +47648,8 @@ - - + + @@ -48053,9 +47683,9 @@ - - - + + + @@ -48108,19 +47738,19 @@ - - - + + + - - - + + + - - - + + + @@ -48128,15 +47758,15 @@ - + - - + + - - - + + + @@ -48180,9 +47810,9 @@ - - - + + + @@ -48243,14 +47873,14 @@ - - - + + + - - - + + + @@ -48258,9 +47888,9 @@ - - - + + + @@ -48333,9 +47963,9 @@ - - - + + + @@ -48343,6 +47973,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -49132,99 +48827,6 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -49658,17 +49260,6 @@ - - - - - - - - - - - @@ -64187,15 +63778,6 @@ - - - - - - - - - @@ -64239,19 +63821,6 @@ - - - - - - - - - - - - - @@ -64330,6 +63899,22 @@ + + + + + + + + + + + + + + + + @@ -64478,7 +64063,7 @@ - + @@ -67744,15 +67329,14 @@ - - - - - - + + + + + diff --git a/common/autoware_testing/CHANGELOG.rst b/common/autoware_testing/CHANGELOG.rst index 09ff6bdc4cf04..5ee6a2c9556de 100644 --- a/common/autoware_testing/CHANGELOG.rst +++ b/common/autoware_testing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_testing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/common/autoware_testing/package.xml b/common/autoware_testing/package.xml index a5df1302b682d..d94b90b6821f4 100644 --- a/common/autoware_testing/package.xml +++ b/common/autoware_testing/package.xml @@ -2,7 +2,7 @@ autoware_testing - 0.40.0 + 0.41.0 Tools for handling standard tests based on ros_testing Adam Dabrowski Tomoya Kimura diff --git a/common/autoware_time_utils/CHANGELOG.rst b/common/autoware_time_utils/CHANGELOG.rst index b18c3dd42cbc6..3e4d9f7de343a 100644 --- a/common/autoware_time_utils/CHANGELOG.rst +++ b/common/autoware_time_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_time_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_time_utils/package.xml b/common/autoware_time_utils/package.xml index 278143e9ac4f8..fccd4e158d6ba 100644 --- a/common/autoware_time_utils/package.xml +++ b/common/autoware_time_utils/package.xml @@ -2,7 +2,7 @@ autoware_time_utils - 0.40.0 + 0.41.0 Simple conversion methods to/from std::chrono to simplify algorithm development Christopher Ho Tomoya Kimura diff --git a/common/autoware_traffic_light_recognition_marker_publisher/CHANGELOG.rst b/common/autoware_traffic_light_recognition_marker_publisher/CHANGELOG.rst index 27a61bf71b1e8..3c9df1fcfd4b6 100644 --- a/common/autoware_traffic_light_recognition_marker_publisher/CHANGELOG.rst +++ b/common/autoware_traffic_light_recognition_marker_publisher/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_traffic_light_recognition_marker_publisher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_traffic_light_recognition_marker_publisher/package.xml b/common/autoware_traffic_light_recognition_marker_publisher/package.xml index af670dc19baf1..b3f240b4b1ac3 100644 --- a/common/autoware_traffic_light_recognition_marker_publisher/package.xml +++ b/common/autoware_traffic_light_recognition_marker_publisher/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_recognition_marker_publisher - 0.40.0 + 0.41.0 The autoware_traffic_light_recognition_marker_publisher package Tomoya Kimura Takeshi Miura diff --git a/common/autoware_traffic_light_utils/CHANGELOG.rst b/common/autoware_traffic_light_utils/CHANGELOG.rst index 0230e43574de8..fd69102c5043a 100644 --- a/common/autoware_traffic_light_utils/CHANGELOG.rst +++ b/common/autoware_traffic_light_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_traffic_light_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_traffic_light_utils/package.xml b/common/autoware_traffic_light_utils/package.xml index 645e92e2d992f..68a0eb05dc201 100644 --- a/common/autoware_traffic_light_utils/package.xml +++ b/common/autoware_traffic_light_utils/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_utils - 0.40.0 + 0.41.0 The autoware_traffic_light_utils package Kotaro Uetake Shunsuke Miura diff --git a/common/autoware_trajectory/CHANGELOG.rst b/common/autoware_trajectory/CHANGELOG.rst index 307dd8eb3dcf5..ba5c731b0b4ce 100644 --- a/common/autoware_trajectory/CHANGELOG.rst +++ b/common/autoware_trajectory/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_trajectory ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_trajectory/package.xml b/common/autoware_trajectory/package.xml index eaa5fca80ff46..230e83bbc3550 100644 --- a/common/autoware_trajectory/package.xml +++ b/common/autoware_trajectory/package.xml @@ -2,7 +2,7 @@ autoware_trajectory - 0.40.0 + 0.41.0 The autoware_trajectory package Yukinari Hisaki Takayuki Murooka diff --git a/common/autoware_universe_utils/CHANGELOG.rst b/common/autoware_universe_utils/CHANGELOG.rst index 31f45a8ecbfce..66eb541ae38b3 100644 --- a/common/autoware_universe_utils/CHANGELOG.rst +++ b/common/autoware_universe_utils/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package autoware_universe_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(autoware_universe_utils): add missing 's' in the class of diagnostics_interface (`#9777 `_) +* feat(behavior_path_planner): use autoware internal stamped messages (`#9750 `_) + * feat(behavior_path_planner): use autoware internal stamped messages + * fix universe_utils + --------- +* feat!: move diagnostics_module from localization_util to unverse_utils (`#9714 `_) + * feat!: move diagnostics_module from localization_util to unverse_utils + * remove diagnostics module from localization_util + * style(pre-commit): autofix + * minor fix in pose_initializer + * add test + * style(pre-commit): autofix + * remove unnecessary declaration + * module -> interface + * remove unnecessary equal expression + * revert the remove of template function + * style(pre-commit): autofix + * use overload instead + * include what you use -- test_diagnostics_interface.cpp + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* fix(autoware_universe_utils): fix bug in test (`#9710 `_) +* Contributors: Fumiya Watanabe, Ryuta Kambe, Takayuki Murooka, kminoda + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_universe_utils/CMakeLists.txt b/common/autoware_universe_utils/CMakeLists.txt index f295662e69091..72486b5de8818 100644 --- a/common/autoware_universe_utils/CMakeLists.txt +++ b/common/autoware_universe_utils/CMakeLists.txt @@ -21,6 +21,7 @@ ament_auto_add_library(autoware_universe_utils SHARED src/geometry/sat_2d.cpp src/math/sin_table.cpp src/math/trigonometry.cpp + src/ros/diagnostics_interface.cpp src/ros/msg_operation.cpp src/ros/marker_helper.cpp src/ros/logger_level_configure.cpp diff --git a/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp index 8420f930e0ce9..5878641676d9f 100644 --- a/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/debug_traits.hpp @@ -15,6 +15,16 @@ #ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ #define AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -84,6 +94,58 @@ template <> struct is_debug_message : std::true_type { }; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message +: std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message +: std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; } // namespace autoware::universe_utils::debug_traits #endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_TRAITS_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp similarity index 70% rename from localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp rename to common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp index 8c19c94127630..db370eb46ef43 100644 --- a/localization/autoware_localization_util/include/autoware/localization_util/diagnostics_module.hpp +++ b/common/autoware_universe_utils/include/autoware/universe_utils/ros/diagnostics_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#ifndef AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ #include @@ -22,16 +22,18 @@ #include #include -namespace autoware::localization_util +namespace autoware::universe_utils { -class DiagnosticsModule +class DiagnosticsInterface { public: - DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + DiagnosticsInterface(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 add_key_value(const std::string & key, const std::string & value); + void add_key_value(const std::string & key, bool value); void update_level_and_message(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); @@ -46,7 +48,7 @@ class DiagnosticsModule }; template -void DiagnosticsModule::add_key_value(const std::string & key, const T & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -54,11 +56,6 @@ void DiagnosticsModule::add_key_value(const std::string & key, const T & 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::universe_utils -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__DIAGNOSTICS_MODULE_HPP_ +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__DIAGNOSTICS_INTERFACE_HPP_ diff --git a/common/autoware_universe_utils/package.xml b/common/autoware_universe_utils/package.xml index d04e1a57e78ab..2415ce6e4a8c1 100644 --- a/common/autoware_universe_utils/package.xml +++ b/common/autoware_universe_utils/package.xml @@ -2,7 +2,7 @@ autoware_universe_utils - 0.40.0 + 0.41.0 The autoware_universe_utils package Takamasa Horibe Takayuki Murooka @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_internal_msgs autoware_perception_msgs autoware_planning_msgs diff --git a/localization/autoware_localization_util/src/diagnostics_module.cpp b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp similarity index 76% rename from localization/autoware_localization_util/src/diagnostics_module.cpp rename to common/autoware_universe_utils/src/ros/diagnostics_interface.cpp index 2b68dbf4f5890..e4d1ec8494113 100644 --- a/localization/autoware_localization_util/src/diagnostics_module.cpp +++ b/common/autoware_universe_utils/src/ros/diagnostics_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include @@ -21,9 +21,9 @@ #include #include -namespace autoware::localization_util +namespace autoware::universe_utils { -DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +DiagnosticsInterface::DiagnosticsInterface(rclcpp::Node * node, const std::string & diagnostic_name) : clock_(node->get_clock()) { diagnostics_pub_ = @@ -34,7 +34,7 @@ DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & di diagnostics_status_msg_.hardware_id = node->get_name(); } -void DiagnosticsModule::clear() +void DiagnosticsInterface::clear() { diagnostics_status_msg_.values.clear(); diagnostics_status_msg_.values.shrink_to_fit(); @@ -43,7 +43,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsInterface::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), @@ -56,8 +56,7 @@ void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key } } -template <> -void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) +void DiagnosticsInterface::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -65,8 +64,7 @@ void DiagnosticsModule::add_key_value(const std::string & key, const std::string add_key_value(key_value); } -template <> -void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) +void DiagnosticsInterface::add_key_value(const std::string & key, bool value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; @@ -74,7 +72,7 @@ void DiagnosticsModule::add_key_value(const std::string & key, const bool & valu add_key_value(key_value); } -void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) +void DiagnosticsInterface::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,12 +85,12 @@ void DiagnosticsModule::update_level_and_message(const int8_t level, const std:: } } -void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +void DiagnosticsInterface::publish(const rclcpp::Time & publish_time_stamp) { diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsInterface::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; @@ -105,4 +103,4 @@ diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_arra return diagnostics_msg; } -} // namespace autoware::localization_util +} // namespace autoware::universe_utils diff --git a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp index df05b698693d6..5c8f96ddee67f 100644 --- a/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp +++ b/common/autoware_universe_utils/test/src/math/test_trigonometry.cpp @@ -18,6 +18,7 @@ #include #include +#include TEST(trigonometry, sin) { @@ -60,11 +61,15 @@ float normalize_angle(double angle) TEST(trigonometry, opencv_fast_atan2) { + std::random_device rd; + std::mt19937 gen(rd()); + + // Generate random x and y between -10.0 and 10.0 as float + std::uniform_real_distribution dis(-10.0f, 10.0f); + for (int i = 0; i < 100; ++i) { - // Generate random x and y between -10 and 10 - std::srand(0); - float x = static_cast(std::rand()) / RAND_MAX * 20.0 - 10.0; - float y = static_cast(std::rand()) / RAND_MAX * 20.0 - 10.0; + const float x = dis(gen); + const float y = dis(gen); float fast_atan = autoware::universe_utils::opencv_fast_atan2(y, x); float std_atan = normalize_angle(std::atan2(y, x)); diff --git a/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp new file mode 100644 index 0000000000000..6ec4fccf78b43 --- /dev/null +++ b/common/autoware_universe_utils/test/src/ros/test_diagnostics_interface.cpp @@ -0,0 +1,179 @@ +// Copyright 2024 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 "autoware/universe_utils/ros/diagnostics_interface.hpp" + +#include + +#include +#include +#include + +#include + +#include +#include + +using autoware::universe_utils::DiagnosticsInterface; + +class TestDiagnosticsInterface : public ::testing::Test +{ +protected: + void SetUp() override + { + // Create a test node + node_ = std::make_shared("test_diagnostics_interface"); + } + + // Automatically destroyed at the end of each test + std::shared_ptr node_; +}; + +/* + * Test clear(): + * Verify that calling clear() resets DiagnosticStatus values, level, and message. + */ +TEST_F(TestDiagnosticsInterface, ClearTest) +{ + DiagnosticsInterface module(node_.get(), "test_diagnostic"); + + // Add some key-value pairs and update level/message + module.add_key_value("param1", 42); + module.update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Something is not OK"); + + // Call clear() + module.clear(); + + // After calling clear(), publish and check the contents of the message + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + + // Publish the message + module.publish(node_->now()); + + // Spin to allow the subscriber to receive messages + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // After clear(), key-value pairs should be empty + EXPECT_TRUE(status.values.empty()); + + // After clear(), level should be OK (=0) + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + // After clear(), message is empty internally, + // but "OK" is set during publishing when level == OK + EXPECT_EQ(status.message, "OK"); +} + +/* + * Test add_key_value(): + * Verify that adding the same key updates its value instead of adding a duplicate. + */ +TEST_F(TestDiagnosticsInterface, AddKeyValueTest) +{ + DiagnosticsInterface module(node_.get(), "test_diagnostic"); + + // Call the template version of add_key_value() with different types + module.add_key_value("string_key", std::string("initial_value")); + module.add_key_value("int_key", 123); + module.add_key_value("bool_key", true); + + // Overwrite the value for "string_key" + module.add_key_value("string_key", std::string("updated_value")); + + // Capture the published message to verify its contents + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + module.publish(node_->now()); + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // Expect 3 key-value pairs + ASSERT_EQ(status.values.size(), 3U); + + // Helper lambda to find a value by key + auto find_value = [&](const std::string & key) -> std::string { + for (const auto & kv : status.values) { + if (kv.key == key) { + return kv.value; + } + } + return ""; + }; + + EXPECT_EQ(find_value("string_key"), "updated_value"); + EXPECT_EQ(find_value("int_key"), "123"); + EXPECT_EQ(find_value("bool_key"), "True"); + + // Status level should still be OK + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + // Message should be "OK" + EXPECT_EQ(status.message, "OK"); +} + +/* + * Test update_level_and_message(): + * Verify that the level is updated to the highest severity and + * that messages are concatenated if level > OK. + */ +TEST_F(TestDiagnosticsInterface, UpdateLevelAndMessageTest) +{ + DiagnosticsInterface module(node_.get(), "test_diagnostic"); + + // Initial status is level=OK(0), message="" + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::OK, "Initial OK"); + // Update to WARN (1) + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::WARN, "Low battery"); + // Update to ERROR (2) + module.update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "Sensor failure"); + // Another WARN (1) after ERROR should not downgrade the overall level + module.update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, "Should not override error"); + + diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr received_msg; + auto sub = node_->create_subscription( + "/diagnostics", 10, + [&](diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg) { received_msg = msg; }); + + module.publish(node_->now()); + rclcpp::spin_some(node_); + + ASSERT_NE(nullptr, received_msg); + ASSERT_FALSE(received_msg->status.empty()); + const auto & status = received_msg->status.front(); + + // Final level should be ERROR (2) + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + + // The message should contain all parts that were added when level > OK. + // Thus, we expect something like: + // "Low battery; Sensor failure; Should not override error" + const std::string & final_message = status.message; + EXPECT_FALSE(final_message.find("Initial OK") != std::string::npos); + EXPECT_TRUE(final_message.find("Low battery") != std::string::npos); + EXPECT_TRUE(final_message.find("Sensor failure") != std::string::npos); + EXPECT_TRUE(final_message.find("Should not override error") != std::string::npos); +} diff --git a/common/autoware_vehicle_info_utils/CHANGELOG.rst b/common/autoware_vehicle_info_utils/CHANGELOG.rst index a02256d5f9cea..f25de9a548809 100644 --- a/common/autoware_vehicle_info_utils/CHANGELOG.rst +++ b/common/autoware_vehicle_info_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_vehicle_info_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/common/autoware_vehicle_info_utils/package.xml b/common/autoware_vehicle_info_utils/package.xml index b6a2f1ad5154a..f419fb9d0d47e 100644 --- a/common/autoware_vehicle_info_utils/package.xml +++ b/common/autoware_vehicle_info_utils/package.xml @@ -2,7 +2,7 @@ autoware_vehicle_info_utils - 0.40.0 + 0.41.0 The autoware_vehicle_info_utils package diff --git a/common/tier4_api_utils/CHANGELOG.rst b/common/tier4_api_utils/CHANGELOG.rst index d9b94eeacc79b..acc839db224cf 100644 --- a/common/tier4_api_utils/CHANGELOG.rst +++ b/common/tier4_api_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_api_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/common/tier4_api_utils/package.xml b/common/tier4_api_utils/package.xml index 3e7f99b8736c5..228d9d8598749 100644 --- a/common/tier4_api_utils/package.xml +++ b/common/tier4_api_utils/package.xml @@ -2,7 +2,7 @@ tier4_api_utils - 0.40.0 + 0.41.0 The tier4_api_utils package Takagi, Isamu Apache License 2.0 diff --git a/control/autoware_autonomous_emergency_braking/CHANGELOG.rst b/control/autoware_autonomous_emergency_braking/CHANGELOG.rst index 578ffc14bae84..b4cb2cbe9abc7 100644 --- a/control/autoware_autonomous_emergency_braking/CHANGELOG.rst +++ b/control/autoware_autonomous_emergency_braking/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_autonomous_emergency_braking ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml index e22a0f099e2f0..b7f04abca584f 100644 --- a/control/autoware_autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -2,7 +2,7 @@ autoware_autonomous_emergency_braking - 0.40.0 + 0.41.0 Autonomous Emergency Braking package as a ROS 2 node Takamasa Horibe Tomoya Kimura diff --git a/control/autoware_collision_detector/CHANGELOG.rst b/control/autoware_collision_detector/CHANGELOG.rst index a108327961ac3..b890f50c447a0 100644 --- a/control/autoware_collision_detector/CHANGELOG.rst +++ b/control/autoware_collision_detector/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_collision_detector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_collision_detector/package.xml b/control/autoware_collision_detector/package.xml index 680c20b8094e7..c573b811027fb 100644 --- a/control/autoware_collision_detector/package.xml +++ b/control/autoware_collision_detector/package.xml @@ -2,7 +2,7 @@ autoware_collision_detector - 0.40.0 + 0.41.0 The collision_detector package Kyoichi Sugahara diff --git a/control/control_performance_analysis/CHANGELOG.rst b/control/autoware_control_performance_analysis/CHANGELOG.rst similarity index 96% rename from control/control_performance_analysis/CHANGELOG.rst rename to control/autoware_control_performance_analysis/CHANGELOG.rst index 67b4f15ddde9f..35d94b7033a79 100644 --- a/control/control_performance_analysis/CHANGELOG.rst +++ b/control/autoware_control_performance_analysis/CHANGELOG.rst @@ -1,6 +1,37 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package control_performance_analysis -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_control_performance_analysis +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `control_performance_analysis` (`#9982 `_) + * feat(control_performance_analysis): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(control_performance_analysis): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(control_performance_analysis): fix include paths + * To follow the previous commit + * rename: `control_performance_analysis` => `autoware_control_performance_analysis` + * style(pre-commit): autofix + * bug(autoware_control_performance_analysis): fix inconsistent namespacing + * style(pre-commit): autofix + * update(autoware_control_performance_analysis): `README.md` + * bug(autoware_control_performance_analysis): fix critical bugs that contaminate topic name + * style(pre-commit): autofix + * fix: update package name for error_rqt_multiplot.xml + * fix: update package name for control_performance_plot.py + * docs(autoware_control_performance_analysis): update package name in README and CHANGELOG.rst + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome + Co-authored-by: SakodaShintaro +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/control/control_performance_analysis/CMakeLists.txt b/control/autoware_control_performance_analysis/CMakeLists.txt similarity index 78% rename from control/control_performance_analysis/CMakeLists.txt rename to control/autoware_control_performance_analysis/CMakeLists.txt index 10a305e1f7dbc..6ea320d2d402e 100644 --- a/control/control_performance_analysis/CMakeLists.txt +++ b/control/autoware_control_performance_analysis/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(control_performance_analysis) +project(autoware_control_performance_analysis) find_package(autoware_cmake REQUIRED) autoware_package() @@ -19,7 +19,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces( - control_performance_analysis + ${PROJECT_NAME} "msg/Error.msg" "msg/ErrorStamped.msg" "msg/DrivingMonitorStamped.msg" @@ -40,12 +40,12 @@ ament_auto_add_library( if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) rosidl_target_interfaces(control_performance_analysis_node - control_performance_analysis "rosidl_typesupport_cpp") + ${PROJECT_NAME} "rosidl_typesupport_cpp") rosidl_target_interfaces(control_performance_analysis_core - control_performance_analysis "rosidl_typesupport_cpp") + ${PROJECT_NAME} "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( - cpp_typesupport_target control_performance_analysis "rosidl_typesupport_cpp") + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") target_link_libraries(control_performance_analysis_node "${cpp_typesupport_target}") target_link_libraries(control_performance_analysis_core "${cpp_typesupport_target}") @@ -58,8 +58,8 @@ target_link_libraries( rclcpp_components_register_node( control_performance_analysis_node - PLUGIN "control_performance_analysis::ControlPerformanceAnalysisNode" - EXECUTABLE control_performance_analysis_exe + PLUGIN "autoware::control_performance_analysis::ControlPerformanceAnalysisNode" + EXECUTABLE ${PROJECT_NAME}_node ) ament_auto_package( diff --git a/control/control_performance_analysis/README.md b/control/autoware_control_performance_analysis/README.md similarity index 89% rename from control/control_performance_analysis/README.md rename to control/autoware_control_performance_analysis/README.md index e87efbe179eee..e57126b28cb5f 100644 --- a/control/control_performance_analysis/README.md +++ b/control/autoware_control_performance_analysis/README.md @@ -1,13 +1,13 @@ -# control_performance_analysis +# autoware_control_performance_analysis ## Purpose -`control_performance_analysis` is the package to analyze the tracking performance of a control module and monitor the driving status of the vehicle. +`autoware_control_performance_analysis` is the package to analyze the tracking performance of a control module and monitor the driving status of the vehicle. This package is used as a tool to quantify the results of the control module. That's why it doesn't interfere with the core logic of autonomous driving. -Based on the various input from planning, control, and vehicle, it publishes the result of analysis as `control_performance_analysis::msg::ErrorStamped` defined in this package. +Based on the various input from planning, control, and vehicle, it publishes the result of analysis as `autoware_control_performance_analysis::msg::ErrorStamped` defined in this package. All results in `ErrorStamped` message are calculated in Frenet Frame of curve. Errors and velocity errors are calculated by using paper below. @@ -35,14 +35,14 @@ Error acceleration calculations are made based on the velocity calculations abov ### Output topics -| Name | Type | Description | -| --------------------------------------- | -------------------------------------------------------- | --------------------------------------------------- | -| `/control_performance/performance_vars` | control_performance_analysis::msg::ErrorStamped | The result of the performance analysis. | -| `/control_performance/driving_status` | control_performance_analysis::msg::DrivingMonitorStamped | Driving status (acceleration, jerk etc.) monitoring | +| Name | Type | Description | +| --------------------------------------- | ----------------------------------------------------------------- | --------------------------------------------------- | +| `/control_performance/performance_vars` | autoware_control_performance_analysis::msg::ErrorStamped | The result of the performance analysis. | +| `/control_performance/driving_status` | autoware_control_performance_analysis::msg::DrivingMonitorStamped | Driving status (acceleration, jerk etc.) monitoring | ### Outputs -#### control_performance_analysis::msg::DrivingMonitorStamped +#### autoware_control_performance_analysis::msg::DrivingMonitorStamped | Name | Type | Description | | ---------------------------- | ----- | --------------------------------------------------------------------- | @@ -53,7 +53,7 @@ Error acceleration calculations are made based on the velocity calculations abov | `desired_steering_angle` | float | $[ \mathrm{rad} ]$ | | `controller_processing_time` | float | Timestamp between last two control command messages $[ \mathrm{ms} ]$ | -#### control_performance_analysis::msg::ErrorStamped +#### autoware_control_performance_analysis::msg::ErrorStamped | Name | Type | Description | | ------------------------------------------ | ----- | ----------------------------------------------------------------------------------------------------------------------------------------- | diff --git a/control/control_performance_analysis/config/control_performance_analysis.param.yaml b/control/autoware_control_performance_analysis/config/control_performance_analysis.param.yaml similarity index 100% rename from control/control_performance_analysis/config/control_performance_analysis.param.yaml rename to control/autoware_control_performance_analysis/config/control_performance_analysis.param.yaml diff --git a/control/control_performance_analysis/config/controller_monitor.xml b/control/autoware_control_performance_analysis/config/controller_monitor.xml similarity index 100% rename from control/control_performance_analysis/config/controller_monitor.xml rename to control/autoware_control_performance_analysis/config/controller_monitor.xml diff --git a/control/control_performance_analysis/config/error_rqt_multiplot.xml b/control/autoware_control_performance_analysis/config/error_rqt_multiplot.xml similarity index 96% rename from control/control_performance_analysis/config/error_rqt_multiplot.xml rename to control/autoware_control_performance_analysis/config/error_rqt_multiplot.xml index 8597cbdaa253e..b75115929456f 100644 --- a/control/control_performance_analysis/config/error_rqt_multiplot.xml +++ b/control/autoware_control_performance_analysis/config/error_rqt_multiplot.xml @@ -36,7 +36,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped error/lateral_error @@ -49,7 +49,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped @@ -110,7 +110,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped error/error_energy @@ -123,7 +123,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped @@ -184,7 +184,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped error/lateral_error_velocity @@ -197,7 +197,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped @@ -260,7 +260,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped error/heading_error @@ -273,7 +273,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped @@ -334,7 +334,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped error/control_effort_energy @@ -347,7 +347,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped @@ -408,7 +408,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped error/lateral_error_acceleration @@ -421,7 +421,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped @@ -484,7 +484,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped error/value_approximation @@ -497,7 +497,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped @@ -558,7 +558,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped error/curvature_estimate @@ -571,7 +571,7 @@ 0 /control_performance/performance_vars - control_performance_analysis/ErrorStamped + autoware_control_performance_analysis/ErrorStamped diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_core.hpp similarity index 80% rename from control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp rename to control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_core.hpp index db0afc134ff79..4f6b9c237279f 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_core.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// Copyright 2021 - 2025 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. @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ -#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ +#ifndef AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ +#define AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ +#include "autoware/control_performance_analysis/control_performance_analysis_utils.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "control_performance_analysis/control_performance_analysis_utils.hpp" -#include "control_performance_analysis/msg/driving_monitor_stamped.hpp" -#include "control_performance_analysis/msg/error_stamped.hpp" -#include "control_performance_analysis/msg/float_stamped.hpp" +#include "autoware_control_performance_analysis/msg/driving_monitor_stamped.hpp" +#include "autoware_control_performance_analysis/msg/error_stamped.hpp" +#include "autoware_control_performance_analysis/msg/float_stamped.hpp" #include #include @@ -37,15 +37,15 @@ #include #include -namespace control_performance_analysis +namespace autoware::control_performance_analysis { using autoware_control_msgs::msg::Control; +using autoware_control_performance_analysis::msg::DrivingMonitorStamped; +using autoware_control_performance_analysis::msg::Error; +using autoware_control_performance_analysis::msg::ErrorStamped; +using autoware_control_performance_analysis::msg::FloatStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_vehicle_msgs::msg::SteeringReport; -using control_performance_analysis::msg::DrivingMonitorStamped; -using control_performance_analysis::msg::Error; -using control_performance_analysis::msg::ErrorStamped; -using control_performance_analysis::msg::FloatStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::Twist; @@ -129,6 +129,6 @@ class ControlPerformanceAnalysisCore rclcpp::Logger logger_{rclcpp::get_logger("control_performance_analysis")}; rclcpp::Clock clock_{RCL_ROS_TIME}; }; -} // namespace control_performance_analysis +} // namespace autoware::control_performance_analysis -#endif // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ +#endif // AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_CORE_HPP_ diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_node.hpp similarity index 79% rename from control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp rename to control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_node.hpp index 16e383916eb6d..973574d47c50f 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// Copyright 2021 - 2025 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ -#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ +#ifndef AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ +#define AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ -#include "control_performance_analysis/control_performance_analysis_core.hpp" -#include "control_performance_analysis/msg/driving_monitor_stamped.hpp" -#include "control_performance_analysis/msg/error_stamped.hpp" +#include "autoware/control_performance_analysis/control_performance_analysis_core.hpp" +#include "autoware_control_performance_analysis/msg/driving_monitor_stamped.hpp" +#include "autoware_control_performance_analysis/msg/error_stamped.hpp" #include #include @@ -34,13 +34,13 @@ #include #include -namespace control_performance_analysis +namespace autoware::control_performance_analysis { using autoware_control_msgs::msg::Control; +using autoware_control_performance_analysis::msg::DrivingMonitorStamped; +using autoware_control_performance_analysis::msg::ErrorStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_vehicle_msgs::msg::SteeringReport; -using control_performance_analysis::msg::DrivingMonitorStamped; -using control_performance_analysis::msg::ErrorStamped; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; @@ -92,6 +92,6 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Algorithm std::unique_ptr control_performance_core_ptr_; }; -} // namespace control_performance_analysis +} // namespace autoware::control_performance_analysis -#endif // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ +#endif // AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_NODE_HPP_ diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp b/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_utils.hpp similarity index 86% rename from control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp rename to control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_utils.hpp index fb939d825b653..2199a0b20d7c1 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_utils.hpp +++ b/control/autoware_control_performance_analysis/include/autoware/control_performance_analysis/control_performance_analysis_utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// Copyright 2021 - 2025 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ -#define CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ +#ifndef AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ +#define AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ #include #include @@ -28,7 +28,7 @@ #include #include -namespace control_performance_analysis +namespace autoware::control_performance_analysis { namespace utils { @@ -84,6 +84,6 @@ double curvatureFromThreePoints( std::array const & c); } // namespace utils -} // namespace control_performance_analysis +} // namespace autoware::control_performance_analysis -#endif // CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ +#endif // AUTOWARE__CONTROL_PERFORMANCE_ANALYSIS__CONTROL_PERFORMANCE_ANALYSIS_UTILS_HPP_ diff --git a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml b/control/autoware_control_performance_analysis/launch/control_performance_analysis.launch.xml similarity index 83% rename from control/control_performance_analysis/launch/control_performance_analysis.launch.xml rename to control/autoware_control_performance_analysis/launch/control_performance_analysis.launch.xml index f2997f0e10b83..eb479942e31a4 100644 --- a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml +++ b/control/autoware_control_performance_analysis/launch/control_performance_analysis.launch.xml @@ -1,5 +1,5 @@ - + @@ -10,7 +10,7 @@ - + diff --git a/control/autoware_control_performance_analysis/msg/DrivingMonitorStamped.msg b/control/autoware_control_performance_analysis/msg/DrivingMonitorStamped.msg new file mode 100644 index 0000000000000..408a5a283e7bc --- /dev/null +++ b/control/autoware_control_performance_analysis/msg/DrivingMonitorStamped.msg @@ -0,0 +1,6 @@ +autoware_control_performance_analysis/FloatStamped longitudinal_acceleration +autoware_control_performance_analysis/FloatStamped longitudinal_jerk +autoware_control_performance_analysis/FloatStamped lateral_acceleration +autoware_control_performance_analysis/FloatStamped lateral_jerk +autoware_control_performance_analysis/FloatStamped desired_steering_angle +autoware_control_performance_analysis/FloatStamped controller_processing_time diff --git a/control/control_performance_analysis/msg/Error.msg b/control/autoware_control_performance_analysis/msg/Error.msg similarity index 100% rename from control/control_performance_analysis/msg/Error.msg rename to control/autoware_control_performance_analysis/msg/Error.msg diff --git a/control/autoware_control_performance_analysis/msg/ErrorStamped.msg b/control/autoware_control_performance_analysis/msg/ErrorStamped.msg new file mode 100644 index 0000000000000..813c68d803e5b --- /dev/null +++ b/control/autoware_control_performance_analysis/msg/ErrorStamped.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +autoware_control_performance_analysis/Error error diff --git a/control/control_performance_analysis/msg/FloatStamped.msg b/control/autoware_control_performance_analysis/msg/FloatStamped.msg similarity index 100% rename from control/control_performance_analysis/msg/FloatStamped.msg rename to control/autoware_control_performance_analysis/msg/FloatStamped.msg diff --git a/control/control_performance_analysis/package.xml b/control/autoware_control_performance_analysis/package.xml similarity index 91% rename from control/control_performance_analysis/package.xml rename to control/autoware_control_performance_analysis/package.xml index 9c2ef69e0137b..a77a740130861 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/autoware_control_performance_analysis/package.xml @@ -1,8 +1,8 @@ - control_performance_analysis - 0.40.0 + autoware_control_performance_analysis + 0.41.0 Controller Performance Evaluation Berkay Karaman Taiki Tanaka @@ -12,6 +12,7 @@ Takamasa Horibe Satoshi Ota Takayuki Murooka + Junya Sasaki Apache License 2.0 @@ -25,6 +26,7 @@ builtin_interfaces autoware_control_msgs + autoware_internal_debug_msgs autoware_motion_utils autoware_planning_msgs autoware_signal_processing @@ -41,7 +43,6 @@ tf2 tf2_eigen tf2_ros - tier4_debug_msgs autoware_global_parameter_loader builtin_interfaces rosidl_default_runtime diff --git a/control/control_performance_analysis/schema/control_performance_analysis.schema.json b/control/autoware_control_performance_analysis/schema/control_performance_analysis.schema.json similarity index 100% rename from control/control_performance_analysis/schema/control_performance_analysis.schema.json rename to control/autoware_control_performance_analysis/schema/control_performance_analysis.schema.json diff --git a/control/control_performance_analysis/scripts/control_performance_plot.py b/control/autoware_control_performance_analysis/scripts/control_performance_plot.py similarity index 96% rename from control/control_performance_analysis/scripts/control_performance_plot.py rename to control/autoware_control_performance_analysis/scripts/control_performance_plot.py index c5e5cabd5b059..a30266a32809f 100644 --- a/control/control_performance_analysis/scripts/control_performance_plot.py +++ b/control/autoware_control_performance_analysis/scripts/control_performance_plot.py @@ -17,13 +17,13 @@ import argparse import math -from control_performance_analysis.msg import DrivingMonitorStamped -from control_performance_analysis.msg import ErrorStamped +from autoware_control_performance_analysis.msg import DrivingMonitorStamped +from autoware_control_performance_analysis.msg import ErrorStamped +from autoware_internal_debug_msgs.msg import BoolStamped import matplotlib.pyplot as plt from nav_msgs.msg import Odometry import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import BoolStamped parser = argparse.ArgumentParser() parser.add_argument("-i", "--interval", help="interval distance to plot") diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/autoware_control_performance_analysis/src/control_performance_analysis_core.cpp similarity index 95% rename from control/control_performance_analysis/src/control_performance_analysis_core.cpp rename to control/autoware_control_performance_analysis/src/control_performance_analysis_core.cpp index 08f736dd0b630..a5b818c64592e 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/autoware_control_performance_analysis/src/control_performance_analysis_core.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// Copyright 2021 - 2025 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "control_performance_analysis/control_performance_analysis_core.hpp" +#include "autoware/control_performance_analysis/control_performance_analysis_core.hpp" #include "autoware/motion_utils/trajectory/interpolation.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" @@ -25,14 +25,15 @@ #include #include -namespace control_performance_analysis +namespace autoware::control_performance_analysis { using geometry_msgs::msg::Quaternion; ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() { - prev_target_vars_ = std::make_unique(); - prev_driving_vars_ = std::make_unique(); + prev_target_vars_ = std::make_unique(); + prev_driving_vars_ = + std::make_unique(); odom_history_ptr_ = std::make_shared>(); p_.odom_interval_ = 0; p_.curvature_interval_length_ = 10.0; @@ -46,8 +47,9 @@ ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore() ControlPerformanceAnalysisCore::ControlPerformanceAnalysisCore(Params & p) : p_{p} { // prepare control performance struct - prev_target_vars_ = std::make_unique(); - prev_driving_vars_ = std::make_unique(); + prev_target_vars_ = std::make_unique(); + prev_driving_vars_ = + std::make_unique(); odom_history_ptr_ = std::make_shared>(); } @@ -251,7 +253,8 @@ bool ControlPerformanceAnalysisCore::calculateErrorVars() lpf(error.error_energy, prev_error.error_energy); } - prev_target_vars_ = std::make_unique(error_vars); + prev_target_vars_ = + std::make_unique(error_vars); return true; } @@ -336,7 +339,9 @@ bool ControlPerformanceAnalysisCore::calculateDrivingVars() lpf(curr.desired_steering_angle.data, prev->desired_steering_angle.data); } - prev_driving_vars_ = std::make_unique(driving_status_vars); + prev_driving_vars_ = + std::make_unique( + driving_status_vars); last_odom_header.stamp = odom_history_ptr_->at(odom_size - 1).header.stamp; last_steering_report.stamp = current_vec_steering_msg_ptr_->stamp; @@ -544,4 +549,4 @@ double ControlPerformanceAnalysisCore::estimatePurePursuitCurvature() return curvature_pure_pursuit; } -} // namespace control_performance_analysis +} // namespace autoware::control_performance_analysis diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/autoware_control_performance_analysis/src/control_performance_analysis_node.cpp similarity index 91% rename from control/control_performance_analysis/src/control_performance_analysis_node.cpp rename to control/autoware_control_performance_analysis/src/control_performance_analysis_node.cpp index 47ce3ce07e83c..1eaf8384614f8 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/autoware_control_performance_analysis/src/control_performance_analysis_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// Copyright 2021 - 2025 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. @@ -12,27 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "control_performance_analysis/control_performance_analysis_node.hpp" +#include "autoware/control_performance_analysis/control_performance_analysis_node.hpp" -#include "control_performance_analysis/msg/driving_monitor_stamped.hpp" -#include "control_performance_analysis/msg/error_stamped.hpp" +#include "autoware_control_performance_analysis/msg/driving_monitor_stamped.hpp" +#include "autoware_control_performance_analysis/msg/error_stamped.hpp" #include #include #include -namespace -{ -using autoware_control_msgs::msg::Control; -using control_performance_analysis::msg::DrivingMonitorStamped; -using control_performance_analysis::msg::ErrorStamped; - -} // namespace - -namespace control_performance_analysis +namespace autoware::control_performance_analysis { using autoware::vehicle_info_utils::VehicleInfoUtils; +using autoware_control_msgs::msg::Control; +using autoware_control_performance_analysis::msg::DrivingMonitorStamped; +using autoware_control_performance_analysis::msg::ErrorStamped; ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( const rclcpp::NodeOptions & node_options) @@ -195,7 +190,8 @@ bool ControlPerformanceAnalysisNode::isValidTrajectory(const Trajectory & traj) return check_condition; } -} // namespace control_performance_analysis +} // namespace autoware::control_performance_analysis #include -RCLCPP_COMPONENTS_REGISTER_NODE(control_performance_analysis::ControlPerformanceAnalysisNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::control_performance_analysis::ControlPerformanceAnalysisNode) diff --git a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp b/control/autoware_control_performance_analysis/src/control_performance_analysis_utils.cpp similarity index 74% rename from control/control_performance_analysis/src/control_performance_analysis_utils.cpp rename to control/autoware_control_performance_analysis/src/control_performance_analysis_utils.cpp index f96aba2b54547..721bcfd1ff83d 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_utils.cpp +++ b/control/autoware_control_performance_analysis/src/control_performance_analysis_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 - 2022 Tier IV, Inc., Leo Drive Teknoloji A.Ş. +// Copyright 2021 - 2025 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "control_performance_analysis/control_performance_analysis_utils.hpp" +#include "autoware/control_performance_analysis/control_performance_analysis_utils.hpp" #include -namespace control_performance_analysis +namespace autoware::control_performance_analysis { namespace utils { @@ -26,4 +26,4 @@ double determinant(std::array const & a, std::array const } } // namespace utils -} // namespace control_performance_analysis +} // namespace autoware::control_performance_analysis diff --git a/control/autoware_control_validator/CHANGELOG.rst b/control/autoware_control_validator/CHANGELOG.rst index c35ece2151697..994ab4f0e642f 100644 --- a/control/autoware_control_validator/CHANGELOG.rst +++ b/control/autoware_control_validator/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_control_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs to autoware_internal_debug_msgs in file contr… (`#9837 `_) + feat: tier4_debug_msgs to autoware_internal_debug_msgs in file control/autoware_control_validator +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 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 index 080e8f0e6abc3..b828c8d07ac49 100644 --- a/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp +++ b/control/autoware_control_validator/include/autoware/control_validator/control_validator.hpp @@ -25,10 +25,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -139,7 +139,8 @@ class ControlValidator : public rclcpp::Node universe_utils::InterProcessPollingSubscriber::SharedPtr sub_reference_traj_; rclcpp::Publisher::SharedPtr pub_status_; rclcpp::Publisher::SharedPtr pub_markers_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // system parameters int64_t diag_error_count_threshold_ = 0; diff --git a/control/autoware_control_validator/package.xml b/control/autoware_control_validator/package.xml index fbaa3f013b64e..fbd0d4d332fe8 100644 --- a/control/autoware_control_validator/package.xml +++ b/control/autoware_control_validator/package.xml @@ -2,7 +2,7 @@ autoware_control_validator - 0.40.0 + 0.41.0 ros node for autoware_control_validator Kyoichi Sugahara Takamasa Horibe diff --git a/control/autoware_control_validator/src/control_validator.cpp b/control/autoware_control_validator/src/control_validator.cpp index 0c16fae065939..e0b9c7135e5f0 100644 --- a/control/autoware_control_validator/src/control_validator.cpp +++ b/control/autoware_control_validator/src/control_validator.cpp @@ -48,8 +48,8 @@ ControlValidator::ControlValidator(const rclcpp::NodeOptions & options) pub_markers_ = create_publisher("~/output/markers", 1); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); debug_pose_publisher_ = std::make_shared(this); @@ -181,7 +181,7 @@ void ControlValidator::publish_debug_info() debug_pose_publisher_->publish(); // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); diff --git a/control/autoware_external_cmd_selector/CHANGELOG.rst b/control/autoware_external_cmd_selector/CHANGELOG.rst index 2e4ee4c7d2906..e8db21263e2d2 100644 --- a/control/autoware_external_cmd_selector/CHANGELOG.rst +++ b/control/autoware_external_cmd_selector/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_external_cmd_selector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/control/autoware_external_cmd_selector/package.xml b/control/autoware_external_cmd_selector/package.xml index 86cbdf9f00bfb..97b6670e409c9 100644 --- a/control/autoware_external_cmd_selector/package.xml +++ b/control/autoware_external_cmd_selector/package.xml @@ -2,7 +2,7 @@ autoware_external_cmd_selector - 0.40.0 + 0.41.0 The autoware_external_cmd_selector package Taiki Tanaka Tomoya Kimura diff --git a/control/autoware_joy_controller/CHANGELOG.rst b/control/autoware_joy_controller/CHANGELOG.rst index 8d25bab7fc6db..aaad4c6ded975 100644 --- a/control/autoware_joy_controller/CHANGELOG.rst +++ b/control/autoware_joy_controller/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_joy_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml index b178ec167306a..b0a128be10ddb 100644 --- a/control/autoware_joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -2,7 +2,7 @@ autoware_joy_controller - 0.40.0 + 0.41.0 The autoware_joy_controller package Taiki Tanaka Tomoya Kimura diff --git a/control/autoware_lane_departure_checker/CHANGELOG.rst b/control/autoware_lane_departure_checker/CHANGELOG.rst index 9b49f0c6c8fb1..2cdbcf9df15f8 100644 --- a/control/autoware_lane_departure_checker/CHANGELOG.rst +++ b/control/autoware_lane_departure_checker/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package autoware_lane_departure_checker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling (`#9791 `_) + * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling + --------- +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9846 `_) + * feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files ontrol/autoware_mpc_lateral_controller + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Kyoichi Sugahara, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_lane_departure_checker/CMakeLists.txt b/control/autoware_lane_departure_checker/CMakeLists.txt index 199195fc88b08..19046a09922f0 100644 --- a/control/autoware_lane_departure_checker/CMakeLists.txt +++ b/control/autoware_lane_departure_checker/CMakeLists.txt @@ -14,6 +14,7 @@ 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 src/lane_departure_checker_node/utils.cpp + src/lane_departure_checker_node/parameters.cpp ) rclcpp_components_register_node(${PROJECT_NAME} diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp index 1ac984394a25e..101338551cbf3 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker.hpp @@ -15,15 +15,12 @@ #ifndef AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#include -#include +#include "autoware/lane_departure_checker/parameters.hpp" + #include #include #include -#include -#include -#include #include #include #include @@ -48,81 +45,33 @@ namespace autoware::lane_departure_checker { -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::PoseDeviation; using autoware::universe_utils::Segment2d; -using autoware_planning_msgs::msg::LaneletRoute; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_planning_msgs::msg::PathWithLaneId; -using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; -struct Param -{ - double footprint_margin_scale{0.0}; - double footprint_extra_margin{0.0}; - double resample_interval{0.0}; - double max_deceleration{0.0}; - double delay_time{0.0}; - double max_lateral_deviation{0.0}; - double max_longitudinal_deviation{0.0}; - double max_yaw_deviation_deg{0.0}; - double min_braking_distance{0.0}; - // nearest search to ego - double ego_nearest_dist_threshold{0.0}; - double ego_nearest_yaw_threshold{0.0}; -}; - -struct Input -{ - nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; - lanelet::LaneletMapPtr lanelet_map{}; - LaneletRoute::ConstSharedPtr route{}; - lanelet::ConstLanelets route_lanelets{}; - lanelet::ConstLanelets shoulder_lanelets{}; - Trajectory::ConstSharedPtr reference_trajectory{}; - Trajectory::ConstSharedPtr predicted_trajectory{}; - std::vector boundary_types_to_detect{}; -}; - -struct Output -{ - std::map processing_time_map{}; - bool will_leave_lane{}; - bool is_out_of_lane{}; - bool will_cross_boundary{}; - PoseDeviation trajectory_deviation{}; - lanelet::ConstLanelets candidate_lanelets{}; - TrajectoryPoints resampled_trajectory{}; - std::vector vehicle_footprints{}; - std::vector vehicle_passing_areas{}; -}; - class LaneDepartureChecker { public: - LaneDepartureChecker( + explicit LaneDepartureChecker( std::shared_ptr time_keeper = std::make_shared()) : time_keeper_(time_keeper) { } - Output update(const Input & input); - void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) + LaneDepartureChecker( + const Param & param, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + std::shared_ptr time_keeper = + std::make_shared()) + : param_(param), + vehicle_info_ptr_(std::make_shared(vehicle_info)), + time_keeper_(time_keeper) { - param_ = param; - vehicle_info_ptr_ = std::make_shared(vehicle_info); } + Output update(const Input & input); void setParam(const Param & param) { param_ = param; } - void setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo vehicle_info) - { - vehicle_info_ptr_ = std::make_shared(vehicle_info); - } - bool checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp index 7e2229d7b5754..7aaf4816708bd 100644 --- a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/lane_departure_checker_node.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ #include "autoware/lane_departure_checker/lane_departure_checker.hpp" +#include "autoware/lane_departure_checker/parameters.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include @@ -24,6 +25,7 @@ #include #include +#include #include #include #include @@ -31,7 +33,6 @@ #include #include #include -#include #include #include @@ -47,21 +48,6 @@ namespace autoware::lane_departure_checker { using autoware_map_msgs::msg::LaneletMapBin; -struct NodeParam -{ - bool will_out_of_lane_checker; - bool out_of_lane_checker; - bool boundary_departure_checker; - - double update_rate; - bool visualize_lanelet; - bool include_right_lanes; - bool include_left_lanes; - bool include_opposite_lanes; - bool include_conflicting_lanes; - std::vector boundary_types_to_detect; -}; - class LaneDepartureCheckerNode : public rclcpp::Node { public: @@ -115,7 +101,8 @@ class LaneDepartureCheckerNode : public rclcpp::Node autoware::universe_utils::DebugPublisher debug_publisher_{this, "~/debug"}; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; - rclcpp::Publisher::SharedPtr processing_time_publisher_; + rclcpp::Publisher::SharedPtr + processing_time_publisher_; // Timer rclcpp::TimerBase::SharedPtr timer_; diff --git a/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.hpp new file mode 100644 index 0000000000000..425b032af425f --- /dev/null +++ b/control/autoware_lane_departure_checker/include/autoware/lane_departure_checker/parameters.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 AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ +#define AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ + +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::lane_departure_checker +{ +using autoware::universe_utils::PoseDeviation; +using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using autoware::universe_utils::LinearRing2d; + +struct Param +{ + static Param init(rclcpp::Node & node); + double footprint_margin_scale{}; + double footprint_extra_margin{}; + double resample_interval{}; + double max_deceleration{}; + double delay_time{}; + double max_lateral_deviation{}; + double max_longitudinal_deviation{}; + double max_yaw_deviation_deg{}; + double min_braking_distance{}; + // nearest search to ego + double ego_nearest_dist_threshold{}; + double ego_nearest_yaw_threshold{}; +}; + +struct NodeParam +{ + static NodeParam init(rclcpp::Node & node); + bool will_out_of_lane_checker{}; + bool out_of_lane_checker{}; + bool boundary_departure_checker{}; + + double update_rate{}; + bool visualize_lanelet{}; + bool include_right_lanes{}; + bool include_left_lanes{}; + bool include_opposite_lanes{}; + bool include_conflicting_lanes{}; + std::vector boundary_types_to_detect{}; +}; + +struct Input +{ + nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; + lanelet::LaneletMapPtr lanelet_map{}; + LaneletRoute::ConstSharedPtr route{}; + lanelet::ConstLanelets route_lanelets{}; + lanelet::ConstLanelets shoulder_lanelets{}; + Trajectory::ConstSharedPtr reference_trajectory{}; + Trajectory::ConstSharedPtr predicted_trajectory{}; + std::vector boundary_types_to_detect{}; +}; + +struct Output +{ + std::map processing_time_map{}; + bool will_leave_lane{}; + bool is_out_of_lane{}; + bool will_cross_boundary{}; + PoseDeviation trajectory_deviation{}; + lanelet::ConstLanelets candidate_lanelets{}; + TrajectoryPoints resampled_trajectory{}; + std::vector vehicle_footprints{}; + std::vector vehicle_passing_areas{}; +}; +} // namespace autoware::lane_departure_checker + +#endif // AUTOWARE__LANE_DEPARTURE_CHECKER__PARAMETERS_HPP_ diff --git a/control/autoware_lane_departure_checker/package.xml b/control/autoware_lane_departure_checker/package.xml index 2689b4a4dbcb7..5c06c43ffc0c8 100644 --- a/control/autoware_lane_departure_checker/package.xml +++ b/control/autoware_lane_departure_checker/package.xml @@ -2,7 +2,7 @@ autoware_lane_departure_checker - 0.40.0 + 0.41.0 The autoware_lane_departure_checker package Kyoichi Sugahara Makoto Kurihara @@ -14,6 +14,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -32,7 +33,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_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 index 366b84a1f6131..32384b7e6c7dd 100644 --- a/control/autoware_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 @@ -126,52 +126,24 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o : Node("lane_departure_checker_node", options) { using std::placeholders::_1; - - // Enable feature - node_param_.will_out_of_lane_checker = declare_parameter("will_out_of_lane_checker"); - node_param_.out_of_lane_checker = declare_parameter("out_of_lane_checker"); - node_param_.boundary_departure_checker = declare_parameter("boundary_departure_checker"); - - // Node Parameter - node_param_.update_rate = declare_parameter("update_rate"); - node_param_.visualize_lanelet = declare_parameter("visualize_lanelet"); - node_param_.include_right_lanes = declare_parameter("include_right_lanes"); - node_param_.include_left_lanes = declare_parameter("include_left_lanes"); - node_param_.include_opposite_lanes = declare_parameter("include_opposite_lanes"); - node_param_.include_conflicting_lanes = declare_parameter("include_conflicting_lanes"); - - // Boundary_departure_checker - node_param_.boundary_types_to_detect = - declare_parameter>("boundary_types_to_detect"); + node_param_ = NodeParam::init(*this); + param_ = Param::init(*this); // Vehicle Info const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_length_m_ = vehicle_info.vehicle_length_m; - // Core Parameter - param_.footprint_margin_scale = declare_parameter("footprint_margin_scale"); - param_.footprint_extra_margin = declare_parameter("footprint_extra_margin"); - param_.resample_interval = declare_parameter("resample_interval"); - param_.max_deceleration = declare_parameter("max_deceleration"); - param_.delay_time = declare_parameter("delay_time"); - param_.max_lateral_deviation = declare_parameter("max_lateral_deviation"); - param_.max_longitudinal_deviation = declare_parameter("max_longitudinal_deviation"); - param_.max_yaw_deviation_deg = declare_parameter("max_yaw_deviation_deg"); - param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); - param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - param_.min_braking_distance = declare_parameter("min_braking_distance"); - // Parameter Callback set_param_res_ = add_on_set_parameters_callback(std::bind(&LaneDepartureCheckerNode::onParameter, this, _1)); // Core - lane_departure_checker_ = std::make_unique(); - lane_departure_checker_->setParam(param_, vehicle_info); + lane_departure_checker_ = std::make_unique(param_, vehicle_info); // Publisher processing_time_publisher_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); // Nothing // Diagnostic Updater @@ -342,10 +314,11 @@ void LaneDepartureCheckerNode::onTimer() { const auto & deviation = output_.trajectory_deviation; - debug_publisher_.publish( + debug_publisher_.publish( "deviation/lateral", deviation.lateral); - debug_publisher_.publish("deviation/yaw", deviation.yaw); - debug_publisher_.publish( + debug_publisher_.publish( + "deviation/yaw", deviation.yaw); + debug_publisher_.publish( "deviation/yaw_deg", rad2deg(deviation.yaw)); } processing_time_map["Node: publishTrajectoryDeviation"] = stop_watch.toc(true); @@ -361,7 +334,7 @@ void LaneDepartureCheckerNode::onTimer() processing_time_map["Total"] = stop_watch.toc("Total"); processing_diag_publisher_.publish(processing_time_map); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = processing_time_map["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp new file mode 100644 index 0000000000000..f3aa23275e35c --- /dev/null +++ b/control/autoware_lane_departure_checker/src/lane_departure_checker_node/parameters.cpp @@ -0,0 +1,59 @@ +// 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/lane_departure_checker/parameters.hpp" + +#include +#include + +#include + +namespace autoware::lane_departure_checker +{ +using autoware::universe_utils::getOrDeclareParameter; + +Param Param::init(rclcpp::Node & node) +{ + Param p; + p.footprint_margin_scale = getOrDeclareParameter(node, "footprint_margin_scale"); + p.footprint_extra_margin = getOrDeclareParameter(node, "footprint_extra_margin"); + p.resample_interval = getOrDeclareParameter(node, "resample_interval"); + p.max_deceleration = getOrDeclareParameter(node, "max_deceleration"); + p.delay_time = getOrDeclareParameter(node, "delay_time"); + p.max_lateral_deviation = getOrDeclareParameter(node, "max_lateral_deviation"); + p.max_longitudinal_deviation = getOrDeclareParameter(node, "max_longitudinal_deviation"); + p.max_yaw_deviation_deg = getOrDeclareParameter(node, "max_yaw_deviation_deg"); + p.min_braking_distance = getOrDeclareParameter(node, "min_braking_distance"); + p.ego_nearest_dist_threshold = getOrDeclareParameter(node, "ego_nearest_dist_threshold"); + p.ego_nearest_yaw_threshold = getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); + return p; +} + +NodeParam NodeParam::init(rclcpp::Node & node) +{ + NodeParam p; + p.will_out_of_lane_checker = getOrDeclareParameter(node, "will_out_of_lane_checker"); + p.out_of_lane_checker = getOrDeclareParameter(node, "out_of_lane_checker"); + p.boundary_departure_checker = getOrDeclareParameter(node, "boundary_departure_checker"); + p.update_rate = getOrDeclareParameter(node, "update_rate"); + p.visualize_lanelet = getOrDeclareParameter(node, "visualize_lanelet"); + p.include_right_lanes = getOrDeclareParameter(node, "include_right_lanes"); + p.include_left_lanes = getOrDeclareParameter(node, "include_left_lanes"); + p.include_opposite_lanes = getOrDeclareParameter(node, "include_opposite_lanes"); + p.include_conflicting_lanes = getOrDeclareParameter(node, "include_conflicting_lanes"); + p.boundary_types_to_detect = + getOrDeclareParameter>(node, "boundary_types_to_detect"); + return p; +} +} // namespace autoware::lane_departure_checker diff --git a/control/autoware_mpc_lateral_controller/CHANGELOG.rst b/control/autoware_mpc_lateral_controller/CHANGELOG.rst index f149921648e8d..49aa808d65d4f 100644 --- a/control/autoware_mpc_lateral_controller/CHANGELOG.rst +++ b/control/autoware_mpc_lateral_controller/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package autoware_mpc_lateral_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9846 `_) + * feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files ontrol/autoware_mpc_lateral_controller + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* chore(autoware_mpc_lateral_controller): fix formula description in vehicle_model_bicycle_kinematics.hpp (`#8971 `_) + fix formula description in vehicle_model_bicycle_kinematics.hpp +* fix(mpc_lateral_controller): prevent unstable steering command while stopped (`#9690 `_) + * modify logic of function isStoppedState + * use a constant distance margin instead of wheelbase length + * add comment to implementation + --------- +* feat(mpc_lateral_controller): remove trans/rot deviation validation since the control_validator has the same feature (`#9684 `_) +* docs: modified minor sign error (`#8140 `_) +* Contributors: Autumn60, Fumiya Watanabe, Takayuki Murooka, Vishal Chauhan, Yuki Kimura, mkquda + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_mpc_lateral_controller/README.md b/control/autoware_mpc_lateral_controller/README.md index 7585e7db140b3..a4b2fa3045a74 100644 --- a/control/autoware_mpc_lateral_controller/README.md +++ b/control/autoware_mpc_lateral_controller/README.md @@ -95,13 +95,11 @@ 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 | -| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory | true | +| 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 | +| use_delayed_initial_state | boolean | flag to use x0_delayed as initial state for predicted trajectory | true | #### Path Smoothing diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp index 81c8b71683092..166dfa1814562 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc.hpp @@ -24,11 +24,11 @@ #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include #include @@ -41,11 +41,11 @@ namespace autoware::motion::control::mpc_lateral_controller using autoware::motion::control::trajectory_follower::LateralHorizon; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; using Eigen::MatrixXd; using Eigen::VectorXd; @@ -421,10 +421,8 @@ class MPC double m_raw_steer_cmd_prev = 0.0; // Previous MPC raw output. /* Parameters for control */ - double m_admissible_position_error; // Threshold for lateral error to trigger stop command [m]. - double m_admissible_yaw_error_rad; // Threshold for yaw error to trigger stop command [rad]. - double m_steer_lim; // Steering command limit [rad]. - double m_ctrl_period; // Control frequency [s]. + double m_steer_lim; // Steering command limit [rad]. + double m_ctrl_period; // Control frequency [s]. //!< @brief steering rate limit list depending on curvature [/m], [rad/s] std::vector> m_steer_rate_lim_map_by_curvature{}; diff --git a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp index d7442f64b028a..0f3004775e5bd 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -26,13 +26,13 @@ #include #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" #include #include @@ -45,11 +45,11 @@ namespace autoware::motion::control::mpc_lateral_controller namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_planning_msgs::msg::Trajectory; using autoware_vehicle_msgs::msg::SteeringReport; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; using trajectory_follower::LateralHorizon; class MpcLateralController : public trajectory_follower::LateralControllerBase diff --git a/control/autoware_mpc_lateral_controller/include/autoware/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 index d638142daa844..015b4e6fad4d6 100644 --- a/control/autoware_mpc_lateral_controller/include/autoware/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 @@ -14,6 +14,7 @@ /* * Representation + * k : reference curvature (input) * e : lateral error * th : heading angle error * steer : steering angle @@ -32,10 +33,12 @@ * dx3/dt = -(x3 - u) / tau * * Linearized model around reference point (v = v_r, th = th_r, steer = steer_r) - * [0, vr, 0] [ 0] [ 0] - * dx/dt = [0, 0, vr/W/cos(steer_r)^2] * x + [ 0] * u + [-vr*steer_r/W/cos(steer_r)^2] - * [0, 0, 1/tau] [1/tau] [ 0] + * [0, vr, 0] [ 0] [ 0] + * dx/dt = [0, 0, B] * x + [ 0] * u + [-vr*k + A - B*steer_r] + * [0, 0, -1/tau] [1/tau] [ 0] * + * where A = vr*tan(steer_r)/W + * B = vr/(W*cos(steer_r)^2) (partial derivative of A with respect to steer_r) */ #ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md b/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md index a1116d8a11c74..f28869bf950fb 100644 --- a/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md +++ b/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md @@ -365,10 +365,10 @@ and aligning the inequality signs $$ \begin{align} -u_{1} - u_{0} &< \dot u_{max}\text{d}t \\\ + -u_{1} + u_{0} &< -\dot u_{min}\text{d}t \\\ -u_{2} - u_{1} &< \dot u_{max}\text{d}t \\\ + -u_{2} + u_{1} &< - \dot u_{min}\text{d}t +u_{1} - u_{0} &< \dot u_{max}\text{d}t \\\ +- u_{1} + u_{0} &< -\dot u_{min}\text{d}t \\\ +u_{2} - u_{1} &< \dot u_{max}\text{d}t \\\ +- u_{2} + u_{1} &< - \dot u_{min}\text{d}t \end{align} $$ diff --git a/control/autoware_mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml index b450af05ea0e1..b818b24786a16 100644 --- a/control/autoware_mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -2,7 +2,7 @@ autoware_mpc_lateral_controller - 0.40.0 + 0.41.0 MPC-based lateral controller Takamasa Horibe @@ -19,6 +19,7 @@ autoware_cmake autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -36,7 +37,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index b358e95f86f99..1d30a28d05cb8 100644 --- a/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -3,8 +3,6 @@ # -- system -- traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory # -- path smoothing -- diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp index 4c3c8bec9b12e..49bf90b89bc90 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -307,17 +307,6 @@ std::pair MPC::getData( // get predicted steer data.predicted_steer = m_steering_predictor->calcSteerPrediction(); - // check error limit - const double dist_err = calcDistance2d(current_pose, data.nearest_pose); - if (dist_err > m_admissible_position_error) { - return {ResultWithReason{false, "too large position error"}, MPCData{}}; - } - - // check yaw error limit - if (std::fabs(data.yaw_err) > m_admissible_yaw_error_rad) { - return {ResultWithReason{false, "too large yaw error"}, MPCData{}}; - } - // check trajectory time length const double max_prediction_time = m_param.min_prediction_length / static_cast(m_param.prediction_horizon - 1); diff --git a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 76e5b4737e418..86d0c0143ed34 100644 --- a/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -57,8 +57,6 @@ MpcLateralController::MpcLateralController( p_filt.traj_resample_dist = dp_double("traj_resample_dist"); p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control"); - m_mpc->m_admissible_position_error = dp_double("admissible_position_error"); - m_mpc->m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad"); m_mpc->m_use_steer_prediction = dp_bool("use_steer_prediction"); m_mpc->m_param.steer_tau = dp_double("vehicle_model_steer_tau"); @@ -434,11 +432,18 @@ Lateral MpcLateralController::getInitialControlCommand() const bool MpcLateralController::isStoppedState() const { + const double current_vel = m_current_kinematic_state.twist.twist.linear.x; // If the nearest index is not found, return false - if (m_current_trajectory.points.empty()) { + if ( + m_current_trajectory.points.empty() || std::fabs(current_vel) > m_stop_state_entry_ego_speed) { return false; } + const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command + if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { + return false; // not stopState: keep control + } + // Note: This function used to take into account the distance to the stop line // for the stop state judgement. However, it has been removed since the steering // control was turned off when approaching/exceeding the stop line on a curve or @@ -447,21 +452,23 @@ bool MpcLateralController::isStoppedState() const m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); - const double current_vel = m_current_kinematic_state.twist.twist.linear.x; - const double target_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; - - const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command - if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { - return false; // not stopState: keep control - } + // It is possible that stop is executed earlier than stop point, and velocity controller + // will not start when the distance from ego to stop point is less than 0.5 meter. + // So we use a distance margin to ensure we can detect stopped state. + static constexpr double distance_margin = 1.0; + const double target_vel = std::invoke([&]() -> double { + auto min_vel = m_current_trajectory.points.at(nearest).longitudinal_velocity_mps; + auto covered_distance = 0.0; + for (auto i = nearest + 1; i < m_current_trajectory.points.size(); ++i) { + min_vel = std::min(min_vel, m_current_trajectory.points.at(i).longitudinal_velocity_mps); + covered_distance += autoware::universe_utils::calcDistance2d( + m_current_trajectory.points.at(i - 1).pose, m_current_trajectory.points.at(i).pose); + if (covered_distance > distance_margin) break; + } + return min_vel; + }); - if ( - std::fabs(current_vel) < m_stop_state_entry_ego_speed && - std::fabs(target_vel) < m_stop_state_entry_target_speed) { - return true; - } else { - return false; - } + return std::fabs(target_vel) < m_stop_state_entry_target_speed; } Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd) diff --git a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp index ccc91d0e7751b..0a34302f8f60c 100644 --- a/control/autoware_mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -24,11 +24,11 @@ #include #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #ifdef ROS_DISTRO_GALACTIC #include "tf2_geometry_msgs/tf2_geometry_msgs.h" @@ -45,12 +45,12 @@ namespace autoware::motion::control::mpc_lateral_controller using autoware::motion::control::trajectory_follower::LateralHorizon; using autoware_control_msgs::msg::Lateral; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; TrajectoryPoint makePoint(const double x, const double y, const float vx) { @@ -96,8 +96,6 @@ class MPCTest : public ::testing::Test double error_deriv_lpf_cutoff_hz = 5.0; // Test Parameters - double admissible_position_error = 5.0; - double admissible_yaw_error_rad = M_PI_2; double steer_lim = 0.610865; // 35 degrees double steer_rate_lim = 2.61799; // 150 degrees double ctrl_period = 0.03; @@ -162,8 +160,6 @@ class MPCTest : public ::testing::Test void initializeMPC(mpc_lateral_controller::MPC & mpc) { mpc.m_param = param; - mpc.m_admissible_position_error = admissible_position_error; - mpc.m_admissible_yaw_error_rad = admissible_yaw_error_rad; mpc.m_steer_lim = steer_lim; mpc.m_steer_rate_lim_map_by_curvature.emplace_back(0.0, steer_rate_lim); mpc.m_steer_rate_lim_map_by_velocity.emplace_back(0.0, steer_rate_lim); @@ -480,37 +476,4 @@ TEST_F(MPCTest, MultiSolveWithBuffer) EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_angle, 0.0f); EXPECT_EQ(ctrl_cmd_horizon.controls.front().steering_tire_rotation_rate, 0.0f); } - -TEST_F(MPCTest, FailureCases) -{ - auto node = rclcpp::Node("mpc_test_node", rclcpp::NodeOptions{}); - auto mpc = std::make_unique(node); - std::shared_ptr vehicle_model_ptr = - std::make_shared(wheelbase, steer_limit, steer_tau); - mpc->setVehicleModel(vehicle_model_ptr); - std::shared_ptr qpsolver_ptr = std::make_shared(); - mpc->setQPSolver(qpsolver_ptr); - - // Init parameters and reference trajectory - initializeMPC(*mpc); - - // Calculate MPC with a pose too far from the trajectory - Pose pose_far; - pose_far.position.x = pose_zero.position.x - admissible_position_error - 1.0; - pose_far.position.y = pose_zero.position.y - admissible_position_error - 1.0; - Lateral ctrl_cmd; - Trajectory pred_traj; - Float32MultiArrayStamped diag; - LateralHorizon ctrl_cmd_horizon; - const auto odom = makeOdometry(pose_far, default_velocity); - EXPECT_FALSE( - mpc->calculateMPC(neutral_steer, odom, ctrl_cmd, pred_traj, diag, ctrl_cmd_horizon).result); - - // Calculate MPC with a fast velocity to make the prediction go further than the reference path - EXPECT_FALSE(mpc - ->calculateMPC( - neutral_steer, makeOdometry(pose_far, default_velocity + 10.0), ctrl_cmd, - pred_traj, diag, ctrl_cmd_horizon) - .result); -} } // namespace autoware::motion::control::mpc_lateral_controller diff --git a/control/autoware_obstacle_collision_checker/CHANGELOG.rst b/control/autoware_obstacle_collision_checker/CHANGELOG.rst index 79a7b2d8819e4..0cf89271451dc 100644 --- a/control/autoware_obstacle_collision_checker/CHANGELOG.rst +++ b/control/autoware_obstacle_collision_checker/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_obstacle_collision_checker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_obstacle_collision_checker/package.xml b/control/autoware_obstacle_collision_checker/package.xml index 334c6d970fc36..3472882c92982 100644 --- a/control/autoware_obstacle_collision_checker/package.xml +++ b/control/autoware_obstacle_collision_checker/package.xml @@ -2,7 +2,7 @@ autoware_obstacle_collision_checker - 0.40.0 + 0.41.0 The obstacle_collision_checker package Taiki Tanaka Tomoya Kimura diff --git a/control/autoware_operation_mode_transition_manager/CHANGELOG.rst b/control/autoware_operation_mode_transition_manager/CHANGELOG.rst index 3f178c04f9d48..731ce29585e04 100644 --- a/control/autoware_operation_mode_transition_manager/CHANGELOG.rst +++ b/control/autoware_operation_mode_transition_manager/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_operation_mode_transition_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_operation_mode_transition_manager/package.xml b/control/autoware_operation_mode_transition_manager/package.xml index eb4d50b625f11..52e663a4a2103 100644 --- a/control/autoware_operation_mode_transition_manager/package.xml +++ b/control/autoware_operation_mode_transition_manager/package.xml @@ -1,6 +1,6 @@ autoware_operation_mode_transition_manager - 0.40.0 + 0.41.0 The operation_mode_transition_manager package Takamasa Horibe Tomoya Kimura @@ -12,7 +12,7 @@ autoware_cmake rosidl_default_generators - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_control_msgs autoware_motion_utils diff --git a/control/autoware_operation_mode_transition_manager/src/node.hpp b/control/autoware_operation_mode_transition_manager/src/node.hpp index a24a36407537f..ffef5f5fe999d 100644 --- a/control/autoware_operation_mode_transition_manager/src/node.hpp +++ b/control/autoware_operation_mode_transition_manager/src/node.hpp @@ -18,7 +18,7 @@ #include "compatibility.hpp" #include "state.hpp" -#include +#include #include #include #include @@ -36,9 +36,11 @@ class OperationModeTransitionManager : public rclcpp::Node private: using ChangeAutowareControlAPI = - autoware::component_interface_specs::system::ChangeAutowareControl; - using ChangeOperationModeAPI = autoware::component_interface_specs::system::ChangeOperationMode; - using OperationModeStateAPI = autoware::component_interface_specs::system::OperationModeState; + autoware::component_interface_specs_universe::system::ChangeAutowareControl; + using ChangeOperationModeAPI = + autoware::component_interface_specs_universe::system::ChangeOperationMode; + using OperationModeStateAPI = + autoware::component_interface_specs_universe::system::OperationModeState; autoware::component_interface_utils::Service::SharedPtr srv_autoware_control_; autoware::component_interface_utils::Service::SharedPtr diff --git a/control/autoware_pid_longitudinal_controller/CHANGELOG.rst b/control/autoware_pid_longitudinal_controller/CHANGELOG.rst index d63f075c26918..67c09b553339a 100644 --- a/control/autoware_pid_longitudinal_controller/CHANGELOG.rst +++ b/control/autoware_pid_longitudinal_controller/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package autoware_pid_longitudinal_controller ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix: remove unnecessary parameters (`#9935 `_) +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9848 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files control/autoware_pid_longitudinal_controller +* feat(pid_longitudinal_controller): add new slope compensation mode trajectory_goal_adaptive (`#9705 `_) +* feat(pid_longitudinal_controller): add virtual wall for dry steering and emergency (`#9685 `_) + * feat(pid_longitudinal_controller): add virtual wall for dry steering and emergency + * fix + --------- +* feat(pid_longitudinal_controller): remove trans/rot deviation validation since the control_validator has the same feature (`#9675 `_) + * feat(pid_longitudinal_controller): remove trans/rot deviation validation since the control_validator has the same feature + * fix test + --------- +* feat(pid_longitudinal_controller): add smooth_stop mode in debug_values (`#9681 `_) +* feat(pid_longitudinal_controller): update trajectory_adaptive; add debug_values, adopt rate limit fillter (`#9656 `_) +* fix(autoware_pid_longitudinal_controller): fix bugprone-branch-clone (`#9629 `_) + fix: bugprone-branch-clone +* Contributors: Fumiya Watanabe, Takayuki Murooka, Vishal Chauhan, Yuki TAKAGI, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md index b147b3b795391..957379866b62a 100644 --- a/control/autoware_pid_longitudinal_controller/README.md +++ b/control/autoware_pid_longitudinal_controller/README.md @@ -68,6 +68,8 @@ There are two sources of the slope information, which can be switched by a param - Cons: z-coordinates of high-precision map is needed. - Cons: Does not support free space planning (for now) +We also offer the options to switch between these, depending on driving conditions. + **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. @@ -193,8 +195,6 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | 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 diff --git a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml index 6d9b93ab98870..ec6a6f11b437d 100644 --- a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml +++ b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml @@ -16,8 +16,6 @@ stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 - emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7854 # drive state kp: 1.0 @@ -76,7 +74,7 @@ # slope compensation lpf_pitch_gain: 0.95 - slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + slope_source: "trajectory_goal_adaptive" # raw_pitch, trajectory_pitch, trajectory_adaptive or trajectory_goal_adaptive adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp index a0bceda625b21..e1cc4d2fd1690 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp @@ -34,9 +34,9 @@ class DebugValues NEAREST_VEL = 4, NEAREST_ACC = 5, SHIFT = 6, - PITCH_LPF_RAD = 7, + PITCH_USING_RAD = 7, PITCH_RAW_RAD = 8, - PITCH_LPF_DEG = 9, + PITCH_USING_DEG = 9, PITCH_RAW_DEG = 10, ERROR_VEL = 11, ERROR_VEL_FILTERED = 12, @@ -61,6 +61,9 @@ class DebugValues ERROR_ACC = 31, ERROR_ACC_FILTERED = 32, ACC_CMD_ACC_FB_APPLIED = 33, + PITCH_LPF_RAD = 34, + PITCH_LPF_DEG = 35, + SMOOTH_STOP_MODE = 36, SIZE // this is the number of enum elements }; @@ -76,6 +79,9 @@ class DebugValues * @return array of all debug values */ std::array(TYPE::SIZE)> getValues() const { return m_values; } + double getValue(const size_t index) const { return m_values.at(index); } + double getValue(const TYPE type) const { return m_values.at(static_cast(type)); } + /** * @brief set the given type to the given value * @param [in] type TYPE of the value diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 7daf3013bab4a..db5fa09b5eeee 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -34,11 +34,11 @@ #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" #include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "visualization_msgs/msg/marker.hpp" #include @@ -50,11 +50,8 @@ namespace autoware::motion::control::pid_longitudinal_controller { -using autoware::universe_utils::createDefaultMarker; -using autoware::universe_utils::createMarkerColor; -using autoware::universe_utils::createMarkerScale; using autoware_adapi_v1_msgs::msg::OperationModeState; -using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; @@ -87,7 +84,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro struct ControlData { - bool is_far_from_trajectory{false}; autoware_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx size_t target_idx{0}; @@ -102,9 +98,11 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; // ros variables - rclcpp::Publisher::SharedPtr m_pub_slope; - rclcpp::Publisher::SharedPtr m_pub_debug; - rclcpp::Publisher::SharedPtr m_pub_stop_reason_marker; + rclcpp::Publisher::SharedPtr + m_pub_slope; + rclcpp::Publisher::SharedPtr + m_pub_debug; + rclcpp::Publisher::SharedPtr m_pub_virtual_wall_marker; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; rcl_interfaces::msg::SetParametersResult paramCallback( @@ -162,8 +160,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double stopped_state_entry_acc; // emergency double emergency_state_overshoot_stop_dist; - double emergency_state_traj_trans_dev; - double emergency_state_traj_rot_dev; }; StateTransitionParams m_state_transition_params; @@ -210,12 +206,18 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_max_acc_cmd_diff; // slope compensation - enum class SlopeSource { RAW_PITCH = 0, TRAJECTORY_PITCH, TRAJECTORY_ADAPTIVE }; + enum class SlopeSource { + RAW_PITCH = 0, + TRAJECTORY_PITCH, + TRAJECTORY_ADAPTIVE, + TRAJECTORY_GOAL_ADAPTIVE + }; SlopeSource m_slope_source{SlopeSource::RAW_PITCH}; double m_adaptive_trajectory_velocity_th; std::shared_ptr m_lpf_pitch{nullptr}; double m_max_pitch_rad; double m_min_pitch_rad; + std::optional m_previous_slope_angle{std::nullopt}; // ego nearest index search double m_ego_nearest_dist_threshold; @@ -245,12 +247,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // Diagnostic std::shared_ptr diag_updater_{}; // Diagnostic updater for publishing diagnostic data. - struct DiagnosticData - { - double trans_deviation{0.0}; // translation deviation between nearest point and current_pose - double rot_deviation{0.0}; // rotation deviation between nearest point and current_pose - }; - DiagnosticData m_diagnostic_data; void setupDiagnosticUpdater(); void checkControlState(diagnostic_updater::DiagnosticStatusWrapper & stat); @@ -411,11 +407,14 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro /** * @brief update variables for debugging about pitch - * @param [in] pitch current pitch of the vehicle (filtered) - * @param [in] traj_pitch current trajectory pitch - * @param [in] raw_pitch current raw pitch of the vehicle (unfiltered) + * @param [in] pitch_using + * @param [in] traj_pitch + * @param [in] localization_pitch + * @param [in] localization_pitch_lpf */ - void updatePitchDebugValues(const double pitch, const double traj_pitch, const double raw_pitch); + void updatePitchDebugValues( + const double pitch_using, const double traj_pitch, const double localization_pitch, + const double localization_pitch_lpf); /** * @brief update variables for velocity and acceleration diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp index e84b44d95c886..8f8fc57e278ef 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/smooth_stop.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ #define AUTOWARE__PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#include "autoware/pid_longitudinal_controller/debug_values.hpp" #include "rclcpp/rclcpp.hpp" #include // NOLINT @@ -85,7 +86,8 @@ class SmoothStop */ double calculate( const double stop_dist, const double current_vel, const double current_acc, - const std::vector> & vel_hist, const double delay_time); + const std::vector> & vel_hist, const double delay_time, + DebugValues & debug_values); private: struct Params @@ -106,6 +108,8 @@ class SmoothStop }; Params m_params; + enum class Mode { STRONG = 0, WEAK, WEAK_STOP, STRONG_STOP }; + double m_strong_acc; rclcpp::Time m_weak_acc_time; bool m_is_set_params = false; diff --git a/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg index 88ac881f4f43c..f669b647dbf84 100644 --- a/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg +++ b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg @@ -175,7 +175,7 @@ : For 500 [ms], self velocity is smaller than C1, and self acceleration is smaller than C2.
emergency condition - : Speed command is zero velocity, and distance to stop is smaller than D1, or self pose is not within D2, D3 from trajectory. + : Speed command is zero velocity, and distance to stop is smaller than D1.

@@ -315,10 +315,6 @@ C2 : stopped_state_entry_acc
D1 : emergency_state_overshoot_stop_dist -
- D2 : emergency_state_traj_trans_dev -
- D3 : emergency_state_traj_rot_dev

diff --git a/control/autoware_pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml index 67e95a9d4c0ac..b685e424e8aa9 100644 --- a/control/autoware_pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -2,7 +2,7 @@ autoware_pid_longitudinal_controller - 0.40.0 + 0.41.0 PID-based longitudinal controller Takamasa Horibe @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_planning_msgs @@ -37,7 +38,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json index ef1272582e52b..14a34f04458eb 100644 --- a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json +++ b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json @@ -71,16 +71,6 @@ "description": "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]", "default": "1.5" }, - "emergency_state_traj_trans_dev": { - "type": "number", - "description": "If the ego's position is `emergency_state_traj_trans_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m]", - "default": "3.0" - }, - "emergency_state_traj_rot_dev": { - "type": "number", - "description": "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]", - "default": "0.7854" - }, "kp": { "type": "number", "description": "p gain for longitudinal control", @@ -326,8 +316,6 @@ "stopped_state_entry_vel", "stopped_state_entry_acc", "emergency_state_overshoot_stop_dist", - "emergency_state_traj_trans_dev", - "emergency_state_traj_rot_dev", "kp", "ki", "kd", diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 7f6da35527290..c87e936de3e40 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -14,6 +14,7 @@ #include "autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/normalization.hpp" @@ -79,10 +80,6 @@ PidLongitudinalController::PidLongitudinalController( // emergency p.emergency_state_overshoot_stop_dist = node.declare_parameter("emergency_state_overshoot_stop_dist"); // [m] - p.emergency_state_traj_trans_dev = - node.declare_parameter("emergency_state_traj_trans_dev"); // [m] - p.emergency_state_traj_rot_dev = - node.declare_parameter("emergency_state_traj_rot_dev"); // [m] } // parameters for drive state @@ -199,6 +196,8 @@ PidLongitudinalController::PidLongitudinalController( m_slope_source = SlopeSource::TRAJECTORY_PITCH; } else if (slope_source == "trajectory_adaptive") { m_slope_source = SlopeSource::TRAJECTORY_ADAPTIVE; + } else if (slope_source == "trajectory_goal_adaptive") { + m_slope_source = SlopeSource::TRAJECTORY_GOAL_ADAPTIVE; } else { RCLCPP_ERROR(logger_, "Slope source is not valid. Using raw_pitch option as default"); m_slope_source = SlopeSource::RAW_PITCH; @@ -215,11 +214,11 @@ PidLongitudinalController::PidLongitudinalController( : node.declare_parameter("ego_nearest_yaw_threshold"); // [rad] // subscriber, publisher - m_pub_slope = node.create_publisher( + m_pub_slope = node.create_publisher( "~/output/slope_angle", rclcpp::QoS{1}); - m_pub_debug = node.create_publisher( + m_pub_debug = node.create_publisher( "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); - m_pub_stop_reason_marker = node.create_publisher("~/output/stop_reason", rclcpp::QoS{1}); + m_pub_virtual_wall_marker = node.create_publisher("~/virtual_wall", 1); // set parameter callback m_set_param_res = node.add_on_set_parameters_callback( @@ -286,8 +285,6 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac update_param("stopped_state_entry_vel", p.stopped_state_entry_vel); update_param("stopped_state_entry_acc", p.stopped_state_entry_acc); update_param("emergency_state_overshoot_stop_dist", p.emergency_state_overshoot_stop_dist); - update_param("emergency_state_traj_trans_dev", p.emergency_state_traj_trans_dev); - update_param("emergency_state_traj_rot_dev", p.emergency_state_traj_rot_dev); } // drive state @@ -423,24 +420,6 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( const auto control_data = getControlData(current_pose); - // self pose is far from trajectory - if (control_data.is_far_from_trajectory) { - if (m_enable_large_tracking_error_emergency) { - // update control state - changeControlState(ControlState::EMERGENCY, "the tracking error is too large"); - } - const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command - m_prev_raw_ctrl_cmd = raw_ctrl_cmd; - const auto cmd_msg = - createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command - publishDebugData(raw_ctrl_cmd, control_data); // publish debug data - trajectory_follower::LongitudinalOutput output; - output.control_cmd = cmd_msg; - output.control_cmd_horizon.controls.push_back(cmd_msg); - output.control_cmd_horizon.time_step_ms = 0.0; - return output; - } - // update control state updateControlState(control_data); @@ -491,23 +470,6 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData const auto nearest_point = current_interpolated_pose.first; auto target_point = current_interpolated_pose.first; - // check if the deviation is worth emergency - m_diagnostic_data.trans_deviation = - autoware::universe_utils::calcDistance2d(current_interpolated_pose.first, current_pose); - const bool is_dist_deviation_large = - m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation; - m_diagnostic_data.rot_deviation = std::abs(autoware::universe_utils::normalizeRadian( - tf2::getYaw(current_interpolated_pose.first.pose.orientation) - - tf2::getYaw(current_pose.orientation))); - const bool is_yaw_deviation_large = - m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation; - - if (is_dist_deviation_large || is_yaw_deviation_large) { - // return here if nearest index is not found - control_data.is_far_from_trajectory = true; - return control_data; - } - // Delay compensation - Calculate the distance we got, predicted velocity and predicted // acceleration after delay control_data.state_after_delay = @@ -562,35 +524,52 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // distance to stopline control_data.stop_dist = longitudinal_utils::calcStopDistance( - control_data.interpolated_traj.points.at(control_data.nearest_idx).pose, - control_data.interpolated_traj, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + current_pose, control_data.interpolated_traj, m_ego_nearest_dist_threshold, + m_ego_nearest_yaw_threshold); // pitch // NOTE: getPitchByTraj() calculates the pitch angle as defined in // ../media/slope_definition.drawio.svg while getPitchByPose() is not, so `raw_pitch` is reversed const double raw_pitch = (-1.0) * longitudinal_utils::getPitchByPose(current_pose.orientation); + m_lpf_pitch->filter(raw_pitch); const double traj_pitch = longitudinal_utils::getPitchByTraj( control_data.interpolated_traj, control_data.target_idx, m_wheel_base); if (m_slope_source == SlopeSource::RAW_PITCH) { - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + control_data.slope_angle = m_lpf_pitch->getValue(); } else if (m_slope_source == SlopeSource::TRAJECTORY_PITCH) { control_data.slope_angle = traj_pitch; - } else if (m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE) { + } else if ( + m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE || + m_slope_source == SlopeSource::TRAJECTORY_GOAL_ADAPTIVE) { // if velocity is high, use target idx for slope, otherwise, use raw_pitch - if (control_data.current_motion.vel > m_adaptive_trajectory_velocity_th) { - control_data.slope_angle = traj_pitch; - m_lpf_pitch->filter(raw_pitch); - } else { - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + const bool is_vel_slow = control_data.current_motion.vel < m_adaptive_trajectory_velocity_th && + m_slope_source == SlopeSource::TRAJECTORY_ADAPTIVE; + + const double goal_dist = autoware::motion_utils::calcSignedArcLength( + control_data.interpolated_traj.points, current_pose.position, + control_data.interpolated_traj.points.size() - 1); + const bool is_close_to_trajectory_end = + goal_dist < m_wheel_base && m_slope_source == SlopeSource::TRAJECTORY_GOAL_ADAPTIVE; + + control_data.slope_angle = + (is_close_to_trajectory_end || is_vel_slow) ? m_lpf_pitch->getValue() : traj_pitch; + + if (m_previous_slope_angle.has_value()) { + constexpr double gravity_const = 9.8; + control_data.slope_angle = std::clamp( + control_data.slope_angle, + m_previous_slope_angle.value() + m_min_jerk * control_data.dt / gravity_const, + m_previous_slope_angle.value() + m_max_jerk * control_data.dt / gravity_const); } + m_previous_slope_angle = control_data.slope_angle; } else { RCLCPP_ERROR_THROTTLE( logger_, *clock_, 3000, "Slope source is not valid. Using raw_pitch option as default"); - control_data.slope_angle = m_lpf_pitch->filter(raw_pitch); + control_data.slope_angle = m_lpf_pitch->getValue(); } - updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); + updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch, m_lpf_pitch->getValue()); return control_data; } @@ -609,6 +588,11 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm longitudinal_utils::applyDiffLimitFilter(raw_ctrl_cmd.acc, m_prev_raw_ctrl_cmd.acc, dt, p.jerk); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_JERK_LIMITED, raw_ctrl_cmd.acc); + const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( + m_current_kinematic_state.pose.pose, "velocity control\n (emergency)", clock_->now(), 0, + m_wheel_base + m_front_overhang); + m_pub_virtual_wall_marker->publish(virtual_wall_marker); + return raw_ctrl_cmd; } @@ -773,14 +757,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d } // publish debug marker - auto marker = createDefaultMarker( - "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING, - createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999)); - marker.pose = autoware::universe_utils::calcOffsetPose( - m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang, - m_vehicle_width / 2 + 2.0, 1.5); - marker.text = "steering not\nconverged"; - m_pub_stop_reason_marker->publish(marker); + if (is_under_control) { + const auto virtual_wall_marker = autoware::motion_utils::createStopVirtualWallMarker( + m_current_kinematic_state.pose.pose, "velocity control\n(steering not converged)", + clock_->now(), 0, m_wheel_base + m_front_overhang); + m_pub_virtual_wall_marker->publish(virtual_wall_marker); + } // keep STOPPED return; @@ -863,7 +845,7 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( } else if (m_control_state == ControlState::STOPPING) { raw_ctrl_cmd.acc = m_smooth_stop.calculate( control_data.stop_dist, control_data.current_motion.vel, control_data.current_motion.acc, - m_vel_hist, m_delay_compensation_time); + m_vel_hist, m_delay_compensation_time, m_debug_values); raw_ctrl_cmd.vel = m_stopped_state_params.vel; RCLCPP_DEBUG( @@ -949,7 +931,7 @@ void PidLongitudinalController::publishDebugData( m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_PUBLISHED, ctrl_cmd.acc); // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = clock_->now(); for (const auto & v : m_debug_values.getValues()) { debug_msg.data.push_back(static_cast(v)); @@ -957,7 +939,7 @@ void PidLongitudinalController::publishDebugData( m_pub_debug->publish(debug_msg); // slope angle - tier4_debug_msgs::msg::Float32MultiArrayStamped slope_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped slope_msg{}; slope_msg.stamp = clock_->now(); slope_msg.data.push_back( static_cast(control_data.slope_angle)); @@ -1189,13 +1171,16 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont } void PidLongitudinalController::updatePitchDebugValues( - const double pitch, const double traj_pitch, const double raw_pitch) + const double pitch_using, const double traj_pitch, const double localization_pitch, + const double localization_pitch_lpf) { const double to_degrees = (180.0 / static_cast(M_PI)); - m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, pitch * to_degrees); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, raw_pitch); - m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, raw_pitch * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_RAD, pitch_using); + m_debug_values.setValues(DebugValues::TYPE::PITCH_USING_DEG, pitch_using * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_RAD, localization_pitch_lpf); + m_debug_values.setValues(DebugValues::TYPE::PITCH_LPF_DEG, localization_pitch_lpf * to_degrees); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_RAD, localization_pitch); + m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_DEG, localization_pitch * to_degrees); m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_RAD, traj_pitch); m_debug_values.setValues(DebugValues::TYPE::PITCH_RAW_TRAJ_DEG, traj_pitch * to_degrees); } @@ -1239,23 +1224,7 @@ void PidLongitudinalController::checkControlState( msg = "emergency occurred due to "; } - if ( - m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation) { - msg += "translation deviation"; - } - - if (m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation) { - msg += "rotation deviation"; - } - stat.add("control_state", static_cast(m_control_state)); - stat.addf( - "translation deviation threshold", "%lf", - m_state_transition_params.emergency_state_traj_trans_dev); - stat.addf("translation deviation", "%lf", m_diagnostic_data.trans_deviation); - stat.addf( - "rotation deviation threshold", "%lf", m_state_transition_params.emergency_state_traj_rot_dev); - stat.addf("rotation deviation", "%lf", m_diagnostic_data.rot_deviation); stat.summary(level, msg); } diff --git a/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp index cb87157fe1114..2dd2950d4356e 100644 --- a/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp @@ -116,7 +116,8 @@ std::experimental::optional SmoothStop::calcTimeToStop( double SmoothStop::calculate( const double stop_dist, const double current_vel, const double current_acc, - const std::vector> & vel_hist, const double delay_time) + const std::vector> & vel_hist, const double delay_time, + DebugValues & debug_values) { if (!m_is_set_params) { throw std::runtime_error("Trying to calculate uninitialized SmoothStop"); @@ -132,30 +133,37 @@ double SmoothStop::calculate( // when exceeding the stopline (stop_dist is negative in these cases.) if (stop_dist < m_params.strong_stop_dist) { // when exceeding the stopline much + debug_values.setValues( + DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG_STOP)); return m_params.strong_stop_acc; } else if (stop_dist < m_params.weak_stop_dist) { // when exceeding the stopline a bit + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK_STOP)); return m_params.weak_stop_acc; } // when the car is running if (is_running) { // when the car will not stop in a certain time - if (time_to_stop && *time_to_stop > m_params.weak_stop_time + delay_time) { - return m_strong_acc; - } else if (!time_to_stop && is_fast_vel) { + if ( + (time_to_stop && *time_to_stop > m_params.weak_stop_time + delay_time) || + (!time_to_stop && is_fast_vel)) { + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG)); return m_strong_acc; } m_weak_acc_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK)); return m_params.weak_acc; } // for 0.5 seconds after the car stopped if ((rclcpp::Clock{RCL_ROS_TIME}.now() - m_weak_acc_time).seconds() < 0.5) { + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::WEAK)); return m_params.weak_acc; } // when the car is not running + debug_values.setValues(DebugValues::TYPE::SMOOTH_STOP_MODE, static_cast(Mode::STRONG_STOP)); return m_params.strong_stop_acc; } } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp index a6d03b8032fe6..30d582bbbf1ef 100644 --- a/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -21,6 +21,7 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) { + using ::autoware::motion::control::pid_longitudinal_controller::DebugValues; using ::autoware::motion::control::pid_longitudinal_controller::SmoothStop; using rclcpp::Duration; using rclcpp::Time; @@ -40,9 +41,10 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) const double delay_time = 0.17; SmoothStop ss; + DebugValues debug_values; // Cannot calculate before setting parameters - EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time), std::runtime_error); + EXPECT_THROW(ss.calculate(0.0, 0.0, 0.0, {}, delay_time, debug_values), std::runtime_error); ss.setParams( max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel, @@ -62,34 +64,45 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) stop_dist = strong_stop_dist - 0.1; current_vel = 2.0; ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_EQ(accel, strong_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 3); // weak stop when the stop distance is below the threshold (but not bellow the strong_stop_dist) stop_dist = weak_stop_dist - 0.1; current_vel = 2.0; ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_EQ(accel, weak_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 2); // if not running, weak accel for 0.5 seconds after the previous init or previous weak_acc rclcpp::Rate rate_quart(1.0 / 0.25); rclcpp::Rate rate_half(1.0 / 0.5); stop_dist = 0.0; current_vel = 0.0; - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, weak_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); rate_quart.sleep(); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, weak_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); rate_half.sleep(); - EXPECT_NE( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), weak_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_NE(accel, weak_acc); + EXPECT_NE(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 1); // strong stop when the car is not running (and is at least 0.5seconds after initialization) - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - strong_stop_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, strong_stop_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 3); // accel between min/max_strong_acc when the car is running: // not predicted to exceed the stop line and is predicted to stop after weak_stop_time + delay @@ -97,19 +110,22 @@ TEST(TestSmoothStop, calculate_stopping_acceleration) current_vel = 1.0; vel_in_target = 1.0; ss.init(vel_in_target, stop_dist); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - max_strong_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, max_strong_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 0); vel_in_target = std::sqrt(2.0); ss.init(vel_in_target, stop_dist); - EXPECT_EQ( - ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time), - min_strong_acc); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); + EXPECT_EQ(accel, min_strong_acc); + EXPECT_EQ(debug_values.getValue(DebugValues::TYPE::SMOOTH_STOP_MODE), 0); for (double vel_in_target = 1.1; vel_in_target < std::sqrt(2.0); vel_in_target += 0.1) { ss.init(vel_in_target, stop_dist); - accel = ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time); + accel = + ss.calculate(stop_dist, current_vel, current_acc, velocity_history, delay_time, debug_values); EXPECT_GT(accel, min_strong_acc); EXPECT_LT(accel, max_strong_acc); } diff --git a/control/predicted_path_checker/CHANGELOG.rst b/control/autoware_predicted_path_checker/CHANGELOG.rst similarity index 83% rename from control/predicted_path_checker/CHANGELOG.rst rename to control/autoware_predicted_path_checker/CHANGELOG.rst index 2cfe80aeada7b..abd007f26638f 100644 --- a/control/predicted_path_checker/CHANGELOG.rst +++ b/control/autoware_predicted_path_checker/CHANGELOG.rst @@ -1,7 +1,32 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package predicted_path_checker +Changelog for package autoware_predicted_path_checker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `predicted_path_checker` (`#9985 `_) + * feat(predicted_path_checker): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(predicted_path_checker): move headers under `include/autoware` + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(predicted_path_checker): fix include header paths + * To follow the previous commit + * rename: `predicted_path_checker` => `autoware_predicted_path_checker` + * style(pre-commit): autofix + * bug(autoware_predicted_path_checker): fix inconsistent namespacings + * bug(autoware_predicted_path_checker): do not change node name + * This might contaminate topic name + * style(pre-commit): autofix + * bug(tier4_control_launch): fix wrong package/plugin names + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/predicted_path_checker/CMakeLists.txt b/control/autoware_predicted_path_checker/CMakeLists.txt similarity index 68% rename from control/predicted_path_checker/CMakeLists.txt rename to control/autoware_predicted_path_checker/CMakeLists.txt index cfbe95df99e74..d14798805b5e5 100644 --- a/control/predicted_path_checker/CMakeLists.txt +++ b/control/autoware_predicted_path_checker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(predicted_path_checker) +project(autoware_predicted_path_checker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,7 +12,7 @@ include_directories( ${Eigen3_INCLUDE_DIRS} ) -ament_auto_add_library(predicted_path_checker SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/predicted_path_checker_node/predicted_path_checker_node.cpp src/predicted_path_checker_node/collision_checker.cpp src/predicted_path_checker_node/utils.cpp @@ -20,9 +20,9 @@ ament_auto_add_library(predicted_path_checker SHARED ) -rclcpp_components_register_node(predicted_path_checker - PLUGIN "autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode" - EXECUTABLE predicted_path_checker_node +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::predicted_path_checker::PredictedPathCheckerNode" + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) diff --git a/control/predicted_path_checker/README.md b/control/autoware_predicted_path_checker/README.md similarity index 100% rename from control/predicted_path_checker/README.md rename to control/autoware_predicted_path_checker/README.md diff --git a/control/predicted_path_checker/config/predicted_path_checker.param.yaml b/control/autoware_predicted_path_checker/config/predicted_path_checker.param.yaml similarity index 100% rename from control/predicted_path_checker/config/predicted_path_checker.param.yaml rename to control/autoware_predicted_path_checker/config/predicted_path_checker.param.yaml diff --git a/control/predicted_path_checker/images/FlowChart.png b/control/autoware_predicted_path_checker/images/FlowChart.png similarity index 100% rename from control/predicted_path_checker/images/FlowChart.png rename to control/autoware_predicted_path_checker/images/FlowChart.png diff --git a/control/predicted_path_checker/images/Z_axis_filtering.png b/control/autoware_predicted_path_checker/images/Z_axis_filtering.png similarity index 100% rename from control/predicted_path_checker/images/Z_axis_filtering.png rename to control/autoware_predicted_path_checker/images/Z_axis_filtering.png diff --git a/control/predicted_path_checker/images/general-structure.png b/control/autoware_predicted_path_checker/images/general-structure.png similarity index 100% rename from control/predicted_path_checker/images/general-structure.png rename to control/autoware_predicted_path_checker/images/general-structure.png diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/collision_checker.hpp similarity index 90% rename from control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp rename to control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/collision_checker.hpp index e2c76bec24860..bed95f4d8987c 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/collision_checker.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ -#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#define AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #include #include +#include +#include #include #include #include #include -#include -#include #include #include @@ -40,7 +40,7 @@ #include #include -namespace autoware::motion::control::predicted_path_checker +namespace autoware::predicted_path_checker { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -123,6 +123,6 @@ class CollisionChecker autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::vector predicted_object_history_{}; }; -} // namespace autoware::motion::control::predicted_path_checker +} // namespace autoware::predicted_path_checker -#endif // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#endif // AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/debug_marker.hpp similarity index 91% rename from control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp rename to control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/debug_marker.hpp index 0f537d52cee06..f9dff3347272a 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp +++ b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/debug_marker.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 PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ -#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ +#define AUTOWARE__PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ #include @@ -35,7 +35,7 @@ #include #include -namespace autoware::motion::control::predicted_path_checker +namespace autoware::predicted_path_checker { enum class PolygonType : int8_t { Vehicle = 0, Collision }; @@ -87,6 +87,6 @@ class PredictedPathCheckerDebugNode std::vector> collision_polyhedrons_; }; -} // namespace autoware::motion::control::predicted_path_checker +} // namespace autoware::predicted_path_checker -#endif // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ +#endif // AUTOWARE__PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/predicted_path_checker_node.hpp similarity index 86% rename from control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp rename to control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/predicted_path_checker_node.hpp index af19f87fecc76..33e0cb13efdca 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/predicted_path_checker_node.hpp @@ -12,20 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ -#define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#define AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ -#include +#include #include #include #include +#include +#include #include #include #include #include #include -#include -#include #include #include @@ -45,7 +45,7 @@ #include #include -namespace autoware::motion::control::predicted_path_checker +namespace autoware::predicted_path_checker { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -97,11 +97,11 @@ class PredictedPathCheckerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_accel_; autoware::component_interface_utils::Subscription< - autoware::component_interface_specs::control::IsStopped>::SharedPtr sub_stop_state_; + autoware::component_interface_specs_universe::control::IsStopped>::SharedPtr sub_stop_state_; // Client autoware::component_interface_utils::Client< - autoware::component_interface_specs::control::SetStop>::SharedPtr cli_set_stop_; + autoware::component_interface_specs_universe::control::SetStop>::SharedPtr cli_set_stop_; // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; @@ -110,8 +110,8 @@ class PredictedPathCheckerNode : public rclcpp::Node PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; - autoware::component_interface_specs::control::IsStopped::Message::ConstSharedPtr is_stopped_ptr_{ - nullptr}; + autoware::component_interface_specs_universe::control::IsStopped::Message::ConstSharedPtr + is_stopped_ptr_{nullptr}; // Core std::unique_ptr collision_checker_; @@ -130,7 +130,8 @@ class PredictedPathCheckerNode : public rclcpp::Node void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); void onIsStopped( - const autoware::component_interface_specs::control::IsStopped::Message::ConstSharedPtr msg); + const autoware::component_interface_specs_universe::control::IsStopped::Message::ConstSharedPtr + msg); // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -177,6 +178,6 @@ class PredictedPathCheckerNode : public rclcpp::Node // Diagnostic Updater diagnostic_updater::Updater updater_; }; -} // namespace autoware::motion::control::predicted_path_checker +} // namespace autoware::predicted_path_checker -#endif // PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#endif // AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/utils.hpp similarity index 94% rename from control/predicted_path_checker/include/predicted_path_checker/utils.hpp rename to control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/utils.hpp index 984584d16aa8f..bf46025242401 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ -#define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_ +#define AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_ #include #include @@ -36,7 +36,7 @@ #include #include -namespace utils +namespace autoware::predicted_path_checker { using autoware::universe_utils::Point2d; @@ -99,6 +99,6 @@ void getCurrentObjectPose( const rclcpp::Time & current_time); bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); -} // namespace utils +} // namespace autoware::predicted_path_checker -#endif // PREDICTED_PATH_CHECKER__UTILS_HPP_ +#endif // AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_ diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/autoware_predicted_path_checker/launch/predicted_path_checker.launch.xml similarity index 79% rename from control/predicted_path_checker/launch/predicted_path_checker.launch.xml rename to control/autoware_predicted_path_checker/launch/predicted_path_checker.launch.xml index 6af1372a5bb4a..b0b8b385e7697 100755 --- a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml +++ b/control/autoware_predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -5,11 +5,11 @@ - + - + diff --git a/control/predicted_path_checker/package.xml b/control/autoware_predicted_path_checker/package.xml similarity index 88% rename from control/predicted_path_checker/package.xml rename to control/autoware_predicted_path_checker/package.xml index e2f9d17769e1a..521d34455f465 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/autoware_predicted_path_checker/package.xml @@ -1,18 +1,19 @@ - predicted_path_checker - 0.40.0 + autoware_predicted_path_checker + 0.41.0 The predicted_path_checker package Berkay Karaman + Junya Sasaki Apache 2.0 Berkay Karaman ament_cmake autoware_cmake - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_motion_utils autoware_perception_msgs diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp similarity index 93% rename from control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp rename to control/autoware_predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp index 2841888b61bcc..d21766f36e202 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp +++ b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "predicted_path_checker/collision_checker.hpp" +#include "autoware/predicted_path_checker/collision_checker.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace autoware::motion::control::predicted_path_checker +namespace autoware::predicted_path_checker { CollisionChecker::CollisionChecker( rclcpp::Node * node, std::shared_ptr debug_ptr) @@ -61,7 +61,7 @@ CollisionChecker::checkTrajectoryForCollision( // create one-step polygon for vehicle Polygon2d one_step_move_vehicle_polygon2d = - utils::createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin); + createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin); if (param_.enable_z_axis_obstacle_filtering) { debug_ptr_->pushPolyhedron( one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Vehicle); @@ -126,7 +126,7 @@ CollisionChecker::checkObstacleHistory( std::vector> collision_points_in_history; for (const auto & obj_history : predicted_object_history_) { if (param_.enable_z_axis_obstacle_filtering) { - if (!utils::intersectsInZAxis(obj_history.object, z_min, z_max)) { + if (!intersectsInZAxis(obj_history.object, z_min, z_max)) { continue; } } @@ -169,11 +169,11 @@ CollisionChecker::checkDynamicObjects( for (size_t i = 0; i < dynamic_objects->objects.size(); ++i) { const auto & obj = dynamic_objects->objects.at(i); if (param_.enable_z_axis_obstacle_filtering) { - if (!utils::intersectsInZAxis(obj, z_min, z_max)) { + if (!intersectsInZAxis(obj, z_min, z_max)) { continue; } } - const auto object_polygon = utils::convertObjToPolygon(obj); + const auto object_polygon = convertObjToPolygon(obj); if (object_polygon.outer().empty()) { // unsupported type continue; @@ -205,7 +205,7 @@ CollisionChecker::checkDynamicObjects( } geometry_msgs::msg::Point nearest_collision_point_tmp; - double norm = utils::getNearestPointAndDistanceForPredictedObject( + double norm = getNearestPointAndDistanceForPredictedObject( collision_point_array, base_pose, &nearest_collision_point_tmp); if (norm < min_norm_collision_norm || !is_init) { min_norm_collision_norm = norm; @@ -217,7 +217,7 @@ CollisionChecker::checkDynamicObjects( } if (is_init) { const auto & obj = dynamic_objects->objects.at(nearest_collision_object_index); - const auto obstacle_polygon = utils::convertObjToPolygon(obj); + const auto obstacle_polygon = convertObjToPolygon(obj); if (param_.enable_z_axis_obstacle_filtering) { debug_ptr_->pushPolyhedron(obstacle_polygon, z_min, z_max, PolygonType::Collision); } else { @@ -229,4 +229,4 @@ CollisionChecker::checkDynamicObjects( } return boost::none; } -} // namespace autoware::motion::control::predicted_path_checker +} // namespace autoware::predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp similarity index 98% rename from control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp rename to control/autoware_predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp index 3fae5e38e7ede..61125b88484f0 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp +++ b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "predicted_path_checker/debug_marker.hpp" +#include "autoware/predicted_path_checker/debug_marker.hpp" #include #include @@ -38,7 +38,7 @@ using autoware::universe_utils::createMarkerOrientation; using autoware::universe_utils::createMarkerScale; using autoware::universe_utils::createPoint; -namespace autoware::motion::control::predicted_path_checker +namespace autoware::predicted_path_checker { PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode( rclcpp::Node * node, const double base_link2front) @@ -326,4 +326,4 @@ visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualiz return msg; } -} // namespace autoware::motion::control::predicted_path_checker +} // namespace autoware::predicted_path_checker diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp similarity index 94% rename from control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp rename to control/autoware_predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index a743b8cead5e7..e849508f32e50 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "predicted_path_checker/predicted_path_checker_node.hpp" +#include "autoware/predicted_path_checker/predicted_path_checker_node.hpp" #include #include @@ -26,7 +26,7 @@ #include #include -namespace autoware::motion::control::predicted_path_checker +namespace autoware::predicted_path_checker { PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options) @@ -132,7 +132,8 @@ void PredictedPathCheckerNode::onAccel( } void PredictedPathCheckerNode::onIsStopped( - const autoware::component_interface_specs::control::IsStopped::Message::ConstSharedPtr msg) + const autoware::component_interface_specs_universe::control::IsStopped::Message::ConstSharedPtr + msg) { is_stopped_ptr_ = msg; @@ -300,7 +301,7 @@ void PredictedPathCheckerNode::onTimer() // Check if the vehicle is in the brake distance - const bool is_in_brake_distance = utils::isInBrakeDistance( + const bool is_in_brake_distance = isInBrakeDistance( predicted_trajectory_array, stop_idx, relative_velocity, relative_acceleration, node_param_.max_deceleration, node_param_.delay_time); @@ -415,8 +416,8 @@ void PredictedPathCheckerNode::checkVehicleState(diagnostic_updater::DiagnosticS void PredictedPathCheckerNode::sendRequest(bool make_stop_vehicle) { if (!is_calling_set_stop_ && cli_set_stop_->service_is_ready()) { - const auto req = - std::make_shared(); + const auto req = std::make_shared< + autoware::component_interface_specs_universe::control::SetStop::Service::Request>(); req->stop = make_stop_vehicle; req->request_source = "predicted_path_checker"; is_calling_set_stop_ = true; @@ -430,7 +431,7 @@ bool PredictedPathCheckerNode::isItDiscretePoint( const auto nearest_segment = autoware::motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose); - const auto nearest_point = utils::calcInterpolatedPoint( + const auto nearest_point = calcInterpolatedPoint( reference_trajectory, collision_point.pose.position, *nearest_segment, false); const auto distance = autoware::universe_utils::calcDistance2d( @@ -494,7 +495,7 @@ void PredictedPathCheckerNode::extendTrajectoryPointsArray(TrajectoryPoints & tr // collision_point. const double extend_distance = vehicle_info_.max_longitudinal_offset_m + node_param_.stop_margin; const auto & goal_point = trajectory.back(); - const auto trajectory_point_extend = utils::getExtendTrajectoryPoint(extend_distance, goal_point); + const auto trajectory_point_extend = getExtendTrajectoryPoint(extend_distance, goal_point); trajectory.push_back(trajectory_point_extend); } @@ -505,14 +506,14 @@ size_t PredictedPathCheckerNode::insertStopPoint( autoware::motion_utils::findNearestSegmentIndex(trajectory, collision_point); const auto nearest_collision_point = - utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true); + calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true); const size_t collision_idx = nearest_collision_segment + 1; trajectory.insert(trajectory.begin() + static_cast(collision_idx), nearest_collision_point); const auto stop_point = - utils::findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_); + findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_); const size_t stop_idx = stop_point.first + 1; trajectory.insert(trajectory.begin() + static_cast(stop_idx), stop_point.second); @@ -549,13 +550,12 @@ void PredictedPathCheckerNode::filterObstacles( for (auto & object : object_ptr_.get()->objects) { // Check is it in front of ego vehicle - if (!utils::isFrontObstacle( - ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) { + if (!isFrontObstacle(ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) { continue; } // Check is it near to trajectory - const double max_length = utils::calcObstacleMaxLength(object.shape); + const double max_length = calcObstacleMaxLength(object.shape); const size_t seg_idx = autoware::motion_utils::findNearestSegmentIndex( traj, object.kinematics.initial_pose_with_covariance.pose.position); const auto p_front = autoware::universe_utils::getPoint(traj.at(seg_idx)); @@ -579,14 +579,13 @@ void PredictedPathCheckerNode::filterObstacles( } PredictedObject filtered_object = object; if (use_prediction) { - utils::getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now()); + getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now()); } filtered_objects.objects.push_back(filtered_object); } } -} // namespace autoware::motion::control::predicted_path_checker +} // namespace autoware::predicted_path_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::predicted_path_checker::PredictedPathCheckerNode) diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/utils.cpp similarity index 98% rename from control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp rename to control/autoware_predicted_path_checker/src/predicted_path_checker_node/utils.cpp index 12e38fd9968ab..c3d4a8b3c7594 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/autoware_predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "predicted_path_checker/utils.hpp" +#include "autoware/predicted_path_checker/utils.hpp" #include #include @@ -20,7 +20,7 @@ #include -namespace utils +namespace autoware::predicted_path_checker { using autoware::motion_utils::findFirstNearestIndexWithSoftConstraints; @@ -355,15 +355,15 @@ Polygon2d convertObjToPolygon(const PredictedObject & obj) { Polygon2d object_polygon{}; if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - object_polygon = utils::convertCylindricalObjectToGeometryPolygon( + object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; - object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( + object_polygon = convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { - object_polygon = utils::convertPolygonObjectToGeometryPolygon( + object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); } else { throw std::runtime_error("Unsupported shape type"); @@ -430,4 +430,4 @@ void getCurrentObjectPose( 0.0, 0.0, autoware::universe_utils::normalizeRadian(yaw + delta_yaw)); } -} // namespace utils +} // namespace autoware::predicted_path_checker diff --git a/control/autoware_pure_pursuit/CHANGELOG.rst b/control/autoware_pure_pursuit/CHANGELOG.rst index e0afa320a49a5..a36d12c8c6b7b 100644 --- a/control/autoware_pure_pursuit/CHANGELOG.rst +++ b/control/autoware_pure_pursuit/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_pure_pursuit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_pure_pursuit)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_pure_pursuit (`#9849 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files control/autoware_pure_pursuit +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 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 index 16cb6471deb2c..f1ad63738df7c 100644 --- 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 @@ -42,11 +42,11 @@ #include #include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.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 @@ -115,7 +115,8 @@ class PurePursuitLateralController : public LateralControllerBase // Debug Publisher rclcpp::Publisher::SharedPtr pub_debug_marker_; - rclcpp::Publisher::SharedPtr pub_debug_values_; + rclcpp::Publisher::SharedPtr + pub_debug_values_; // Predicted Trajectory publish rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; diff --git a/control/autoware_pure_pursuit/package.xml b/control/autoware_pure_pursuit/package.xml index 06500ddb32af0..98f1e71f26cf6 100644 --- a/control/autoware_pure_pursuit/package.xml +++ b/control/autoware_pure_pursuit/package.xml @@ -2,7 +2,7 @@ autoware_pure_pursuit - 0.40.0 + 0.41.0 The pure_pursuit package Takamasa Horibe Takayuki Murooka @@ -15,6 +15,7 @@ autoware_cmake autoware_control_msgs + autoware_internal_debug_msgs autoware_motion_utils autoware_planning_msgs autoware_trajectory_follower_base @@ -32,7 +33,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs visualization_msgs ament_lint_auto 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 index f3a7bc3c1333d..d173cfe64d3e8 100644 --- 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 @@ -92,8 +92,9 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) // Debug Publishers pub_debug_marker_ = node.create_publisher("~/debug/markers", 0); - pub_debug_values_ = node.create_publisher( - "~/debug/ld_outputs", rclcpp::QoS{1}); + pub_debug_values_ = + node.create_publisher( + "~/debug/ld_outputs", rclcpp::QoS{1}); // Publish predicted trajectory pub_predicted_trajectory_ = node.create_publisher( @@ -118,7 +119,7 @@ double PurePursuitLateralController::calcLookaheadDistance( std::clamp(vel_ld + curvature_ld + lateral_error_ld, min_ld, param_.max_lookahead_distance); auto pubDebugValues = [&]() { - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_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); diff --git a/control/autoware_shift_decider/CHANGELOG.rst b/control/autoware_shift_decider/CHANGELOG.rst index 2b5eda421e452..73bbcb95fbdd5 100644 --- a/control/autoware_shift_decider/CHANGELOG.rst +++ b/control/autoware_shift_decider/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_shift_decider ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/control/autoware_shift_decider/package.xml b/control/autoware_shift_decider/package.xml index fb7811e094dc2..6ae72a6ade360 100644 --- a/control/autoware_shift_decider/package.xml +++ b/control/autoware_shift_decider/package.xml @@ -2,7 +2,7 @@ autoware_shift_decider - 0.40.0 + 0.41.0 The autoware_shift_decider package Takamasa Horibe Takayuki Murooka diff --git a/control/autoware_smart_mpc_trajectory_follower/CHANGELOG.rst b/control/autoware_smart_mpc_trajectory_follower/CHANGELOG.rst index d3dc4d5eb1aa8..a18c00c93f1e0 100644 --- a/control/autoware_smart_mpc_trajectory_follower/CHANGELOG.rst +++ b/control/autoware_smart_mpc_trajectory_follower/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_smart_mpc_trajectory_follower ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9851 `_) +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_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 index d2687b352a17d..7a50a7e118298 100644 --- a/control/autoware_smart_mpc_trajectory_follower/autoware_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 @@ -7,27 +7,27 @@ gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autow gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral autoware_internal_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw autoware_internal_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState --csv --qos-history keep_all --qos-reliability reliable > system_operation_mode_state.csv' gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' gnome-terminal -- bash -c 'ros2 topic echo /sensing/imu/imu_data sensor_msgs/msg/Imu --csv --qos-history keep_all --qos-reliability reliable > imu.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_y_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_y_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_v_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_v_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_yaw_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_yaw_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_acc_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_acc_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_steer_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_steer_des.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_X_des_converted tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_X_des_converted.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_current tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_current.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_error_prediction tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_error_prediction.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_max_trajectory_err tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_max_trajectory_err.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_emergency_stop_mode tier4_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_emergency_stop_mode.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_goal_stop_mode tier4_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_goal_stop_mode.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_total_ctrl_time tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_total_ctrl_time.csv' -gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_calc_u_opt_time tier4_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_calc_u_opt_time.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_y_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_y_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_v_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_v_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_yaw_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_yaw_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_acc_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_acc_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_steer_des autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_steer_des.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_X_des_converted autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_X_des_converted.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_current autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_current.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_error_prediction autoware_internal_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_error_prediction.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_max_trajectory_err autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_max_trajectory_err.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_emergency_stop_mode autoware_internal_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_emergency_stop_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_goal_stop_mode autoware_internal_debug_msgs/msg/BoolStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_goal_stop_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_total_ctrl_time autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_total_ctrl_time.csv' +gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_calc_u_opt_time autoware_internal_debug_msgs/msg/Float32Stamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_calc_u_opt_time.csv' # wait a moment to open new terminals converting rosbag2 to csv sleep 8 diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml index 7260dee488c52..a5dff3d73da63 100644 --- a/control/autoware_smart_mpc_trajectory_follower/package.xml +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -2,7 +2,7 @@ autoware_smart_mpc_trajectory_follower - 0.40.0 + 0.41.0 Nodes to follow a trajectory by generating control commands using smart mpc diff --git a/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index f767dad223f0d..104261866cf98 100755 --- a/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -21,6 +21,9 @@ from autoware_adapi_v1_msgs.msg import OperationModeState from autoware_control_msgs.msg import Control +from autoware_internal_debug_msgs.msg import BoolStamped +from autoware_internal_debug_msgs.msg import Float32MultiArrayStamped +from autoware_internal_debug_msgs.msg import Float32Stamped from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint from autoware_smart_mpc_trajectory_follower.scripts import drive_controller @@ -43,9 +46,6 @@ from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp from std_msgs.msg import String -from tier4_debug_msgs.msg import BoolStamped -from tier4_debug_msgs.msg import Float32MultiArrayStamped -from tier4_debug_msgs.msg import Float32Stamped from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray diff --git a/control/autoware_trajectory_follower_base/CHANGELOG.rst b/control/autoware_trajectory_follower_base/CHANGELOG.rst index 88ccd57905c89..a4b5f68e14311 100644 --- a/control/autoware_trajectory_follower_base/CHANGELOG.rst +++ b/control/autoware_trajectory_follower_base/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_trajectory_follower_base ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_msgs in files con… (`#9852 `_) + feat: tier4_debug_msgs changed to autoware_internal_msgs in files control/autoware_trajectory_follower_base +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_trajectory_follower_base/package.xml b/control/autoware_trajectory_follower_base/package.xml index e7b00e4b19cdc..e7d4bd9599625 100644 --- a/control/autoware_trajectory_follower_base/package.xml +++ b/control/autoware_trajectory_follower_base/package.xml @@ -2,7 +2,7 @@ autoware_trajectory_follower_base - 0.40.0 + 0.41.0 Library for generating lateral and longitudinal controls following a trajectory @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -37,7 +38,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/control/autoware_trajectory_follower_node/CHANGELOG.rst b/control/autoware_trajectory_follower_node/CHANGELOG.rst index c77e252230bfb..f9554faa7a19f 100644 --- a/control/autoware_trajectory_follower_node/CHANGELOG.rst +++ b/control/autoware_trajectory_follower_node/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package autoware_trajectory_follower_node ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9853 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files control/autoware_trajectory_follower_node +* fix: remove unnecessary parameters (`#9935 `_) +* chore(trajectory_follower_node): fix typos (`#9707 `_) +* feat(pid_longitudinal_controller): update plotjuggler settings (`#9703 `_) +* feat(pid_longitudinal_controller): remove trans/rot deviation validation since the control_validator has the same feature (`#9675 `_) + * feat(pid_longitudinal_controller): remove trans/rot deviation validation since the control_validator has the same feature + * fix test + --------- +* Contributors: Fumiya Watanabe, Kosuke Takeuchi, Takayuki Murooka, Vishal Chauhan, Yuki TAKAGI + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_trajectory_follower_node/README.md b/control/autoware_trajectory_follower_node/README.md index ac05dd67f18e6..c56154f909d1a 100644 --- a/control/autoware_trajectory_follower_node/README.md +++ b/control/autoware_trajectory_follower_node/README.md @@ -151,7 +151,7 @@ Giving the longitudinal controller information about steer convergence allows it ## Debugging -Debug information are published by the lateral and longitudinal controller using `tier4_debug_msgs/Float32MultiArrayStamped` messages. +Debug information are published by the lateral and longitudinal controller using `autoware_internal_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. diff --git a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml index 0de08076b8c06..fda7e1f3906e0 100644 --- a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml +++ b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml @@ -1,130 +1,130 @@ - - + + - - + + - - + + - + - + - + - + - + - + - + - - + + - + - + - - + + - + - + - + - - + + - + - + - + - + - - + + - + - + - - + + @@ -133,114 +133,150 @@ - + - + - - + + - + - + - + - + - - + + - - - - - - + - + + + + + + + + + + + - + - + - + - + - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + - + - + - + - + - - + + - + + + + + + @@ -255,7 +291,7 @@ - + @@ -270,11 +306,11 @@ - + - - - + + + 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 index 1733b4bcbbb7d..c84355cc202fa 100644 --- 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 @@ -44,7 +44,7 @@ #include "tf2_msgs/msg/tf_message.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include -#include +#include #include #include @@ -63,7 +63,7 @@ namespace trajectory_follower_node using autoware::universe_utils::StopWatch; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_control_msgs::msg::ControlHorizon; -using tier4_debug_msgs::msg::Float64Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; diff --git a/control/autoware_trajectory_follower_node/package.xml b/control/autoware_trajectory_follower_node/package.xml index aa65e340d20fe..e12cda7d0de1b 100644 --- a/control/autoware_trajectory_follower_node/package.xml +++ b/control/autoware_trajectory_follower_node/package.xml @@ -2,7 +2,7 @@ autoware_trajectory_follower_node - 0.40.0 + 0.41.0 Nodes to follow a trajectory by generating control commands separated into lateral and longitudinal commands diff --git a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml index b358e95f86f99..1d30a28d05cb8 100644 --- a/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml +++ b/control/autoware_trajectory_follower_node/param/lateral/mpc.param.yaml @@ -3,8 +3,6 @@ # -- system -- traj_resample_dist: 0.1 # path resampling interval [m] use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value use_delayed_initial_state: true # flag to use x0_delayed as initial state for predicted trajectory # -- path smoothing -- diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index 5c5d65a13b2d5..f3139308db37c 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -16,8 +16,6 @@ stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 - emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7854 # drive state kp: 1.0 diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp index 0b1dac644a8ab..982af104538b5 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -510,34 +510,6 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } -TEST_F(FakeNodeFixture, longitudinal_emergency) -{ - const auto node_options = makeNodeOptions(); - ControllerTester tester(this, node_options); - - tester.send_default_transform(); - tester.publish_default_odom(); - tester.publish_autonomous_operation_mode(); - tester.publish_default_steer(); - tester.publish_default_acc(); - - // Publish trajectory starting away from the current ego pose - Trajectory traj; - traj.header.stamp = tester.node->now(); - traj.header.frame_id = "map"; - traj.points.push_back(make_traj_point(10.0, 0.0, 1.0f)); - traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); - traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); - tester.traj_pub->publish(traj); - - test_utils::waitForMessage(tester.node, this, tester.received_control_command); - - ASSERT_TRUE(tester.received_control_command); - // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); - EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); -} - TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) { const auto node_options = makeNodeOptions(); diff --git a/control/autoware_vehicle_cmd_gate/CHANGELOG.rst b/control/autoware_vehicle_cmd_gate/CHANGELOG.rst index 38dae3279b1c8..aad435d519c55 100644 --- a/control/autoware_vehicle_cmd_gate/CHANGELOG.rst +++ b/control/autoware_vehicle_cmd_gate/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_vehicle_cmd_gate ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_vehicle_cmd_gate)!: tier4-debug_msgs changed to autoware_internal_debug_msgs in autoware_vehicle_cmd_gate (`#9856 `_) +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/control/autoware_vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml index c3697337e2256..ba1f31c194946 100644 --- a/control/autoware_vehicle_cmd_gate/package.xml +++ b/control/autoware_vehicle_cmd_gate/package.xml @@ -2,7 +2,7 @@ autoware_vehicle_cmd_gate - 0.40.0 + 0.41.0 The vehicle_cmd_gate package Takamasa Horibe Tomoya Kimura @@ -16,9 +16,10 @@ rosidl_default_generators autoware_adapi_v1_msgs - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_control_msgs + autoware_internal_debug_msgs autoware_motion_utils autoware_universe_utils autoware_vehicle_info_utils @@ -30,7 +31,6 @@ std_srvs tier4_api_utils tier4_control_msgs - tier4_debug_msgs tier4_external_api_msgs tier4_system_msgs tier4_vehicle_msgs diff --git a/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp index 3ddbff82e3102..5466497b7ee27 100644 --- a/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp +++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp @@ -15,7 +15,7 @@ #ifndef ADAPI_PAUSE_INTERFACE_HPP_ #define ADAPI_PAUSE_INTERFACE_HPP_ -#include +#include #include #include @@ -29,9 +29,9 @@ class AdapiPauseInterface private: static constexpr double eps = 1e-3; using Control = autoware_control_msgs::msg::Control; - using SetPause = autoware::component_interface_specs::control::SetPause; - using IsPaused = autoware::component_interface_specs::control::IsPaused; - using IsStartRequested = autoware::component_interface_specs::control::IsStartRequested; + using SetPause = autoware::component_interface_specs_universe::control::SetPause; + using IsPaused = autoware::component_interface_specs_universe::control::IsPaused; + using IsStartRequested = autoware::component_interface_specs_universe::control::IsStartRequested; public: explicit AdapiPauseInterface(rclcpp::Node * node); diff --git a/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp index 2cef0071b0dd9..bebdcbfa0e208 100644 --- a/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp +++ b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -15,7 +15,7 @@ #ifndef MODERATE_STOP_INTERFACE_HPP_ #define MODERATE_STOP_INTERFACE_HPP_ -#include +#include #include #include @@ -28,9 +28,9 @@ namespace autoware::vehicle_cmd_gate class ModerateStopInterface { private: - using SetStop = autoware::component_interface_specs::control::SetStop; - using IsStopped = autoware::component_interface_specs::control::IsStopped; - using IsStartRequested = autoware::component_interface_specs::control::IsStartRequested; + using SetStop = autoware::component_interface_specs_universe::control::SetStop; + using IsStopped = autoware::component_interface_specs_universe::control::IsStopped; + using IsStartRequested = autoware::component_interface_specs_universe::control::IsStartRequested; public: explicit ModerateStopInterface(rclcpp::Node * node); diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index b67b213b426dd..a4838ddcdc1f5 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -77,8 +77,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) engage_pub_ = create_publisher("output/engage", durable_qos); pub_external_emergency_ = create_publisher("output/external_emergency", durable_qos); operation_mode_pub_ = create_publisher("output/operation_mode", durable_qos); - processing_time_pub_ = - this->create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = this->create_publisher( + "~/debug/processing_time_ms", 1); is_filter_activated_pub_ = create_publisher("~/is_filter_activated", durable_qos); @@ -520,7 +520,7 @@ void VehicleCmdGate::onTimer() } // ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index c44e96631998f..094f114abb0a5 100644 --- a/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -32,6 +32,8 @@ #include #include #include +#include +#include #include #include #include @@ -41,8 +43,6 @@ #include #include #include -#include -#include #include #include #include @@ -62,6 +62,7 @@ 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_internal_debug_msgs::msg::BoolStamped; using autoware_vehicle_cmd_gate::msg::IsFilterActivated; using autoware_vehicle_msgs::msg::GearCommand; using autoware_vehicle_msgs::msg::HazardLightsCommand; @@ -70,7 +71,6 @@ using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using std_srvs::srv::Trigger; using tier4_control_msgs::msg::GateMode; -using tier4_debug_msgs::msg::BoolStamped; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; @@ -116,7 +116,8 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr filter_activated_marker_pub_; rclcpp::Publisher::SharedPtr filter_activated_marker_raw_pub_; rclcpp::Publisher::SharedPtr filter_activated_flag_pub_; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; // Parameter callback OnSetParametersCallbackHandle::SharedPtr set_param_res_; rcl_interfaces::msg::SetParametersResult onParameter( diff --git a/control/control_performance_analysis/msg/DrivingMonitorStamped.msg b/control/control_performance_analysis/msg/DrivingMonitorStamped.msg deleted file mode 100644 index 2dea0b5e36b8f..0000000000000 --- a/control/control_performance_analysis/msg/DrivingMonitorStamped.msg +++ /dev/null @@ -1,6 +0,0 @@ -control_performance_analysis/FloatStamped longitudinal_acceleration -control_performance_analysis/FloatStamped longitudinal_jerk -control_performance_analysis/FloatStamped lateral_acceleration -control_performance_analysis/FloatStamped lateral_jerk -control_performance_analysis/FloatStamped desired_steering_angle -control_performance_analysis/FloatStamped controller_processing_time diff --git a/control/control_performance_analysis/msg/ErrorStamped.msg b/control/control_performance_analysis/msg/ErrorStamped.msg deleted file mode 100644 index 8b0b37f653b40..0000000000000 --- a/control/control_performance_analysis/msg/ErrorStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -std_msgs/Header header -control_performance_analysis/Error error diff --git a/evaluator/autoware_control_evaluator/CHANGELOG.rst b/evaluator/autoware_control_evaluator/CHANGELOG.rst index 88afa49d97cc3..ec41a5cc77c8c 100644 --- a/evaluator/autoware_control_evaluator/CHANGELOG.rst +++ b/evaluator/autoware_control_evaluator/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package autoware_control_evaluator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_control_evaluator): add new steering metrics (`#10012 `_) +* feat(autoware_control_evaluator): add new boundary_distance metrics (`#9984 `_) + * add boundary_distance metric + * pre-commit + * use path topic instead of lanenet. + * remove unused import + * apply is_point_left_of_line + * fix typo + * fix test bug + * manual pre-commit + --------- + Co-authored-by: t4-adc +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9858 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files evaluator/autoware_control_evaluator +* fix(autoware_control_evaluator): fix bugprone-exception-escape (`#9630 `_) + * fix: bugprone-exception-escape + * fix: cpplint + --------- +* Contributors: Fumiya Watanabe, Kem (TiankuiXian), Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/evaluator/autoware_control_evaluator/CMakeLists.txt b/evaluator/autoware_control_evaluator/CMakeLists.txt index 5d6474de88015..e01f1d2bba1f8 100644 --- a/evaluator/autoware_control_evaluator/CMakeLists.txt +++ b/evaluator/autoware_control_evaluator/CMakeLists.txt @@ -7,8 +7,7 @@ autoware_package() find_package(pluginlib REQUIRED) ament_auto_add_library(control_evaluator_node SHARED - src/control_evaluator_node.cpp - src/metrics/deviation_metrics.cpp + DIRECTORY src ) rclcpp_components_register_node(control_evaluator_node diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index c510b2ea46779..06f188a1fb63d 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,14 +22,17 @@ #include #include #include +#include #include +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include #include #include -#include #include #include +#include #include #include @@ -39,7 +42,11 @@ namespace control_diagnostics { using autoware::universe_utils::Accumulator; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; @@ -48,6 +55,7 @@ using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; using MetricMsg = tier4_metric_msgs::msg::Metric; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; +using tier4_planning_msgs::msg::PathWithLaneId; /** * @brief Node for control evaluation @@ -64,10 +72,12 @@ class ControlEvaluatorNode : public rclcpp::Node void AddGoalLongitudinalDeviationMetricMsg(const Pose & ego_pose); void AddGoalLateralDeviationMetricMsg(const Pose & ego_pose); void AddGoalYawDeviationMetricMsg(const Pose & ego_pose); + void AddBoundaryDistanceMetricMsg(const PathWithLaneId & behavior_path, const Pose & ego_pose); - void AddLaneletMetricMsg(const Pose & ego_pose); + void AddLaneletInfoMsg(const Pose & ego_pose); void AddKinematicStateMetricMsg( const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); + void AddSteeringMetricMsg(const SteeringReport & steering_report); void onTimer(); @@ -84,8 +94,13 @@ class ControlEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber< LaneletMapBin, autoware::universe_utils::polling_policy::Newest> vector_map_subscriber_{this, "~/input/vector_map", rclcpp::QoS{1}.transient_local()}; + autoware::universe_utils::InterProcessPollingSubscriber behavior_path_subscriber_{ + this, "~/input/behavior_path"}; + autoware::universe_utils::InterProcessPollingSubscriber steering_sub_{ + this, "~/input/steering_status"}; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; // update Route Handler @@ -97,17 +112,29 @@ class ControlEvaluatorNode : public rclcpp::Node // Metric const std::vector metrics_ = { // collect all metrics - Metric::lateral_deviation, Metric::yaw_deviation, Metric::goal_longitudinal_deviation, - Metric::goal_lateral_deviation, Metric::goal_yaw_deviation, + Metric::lateral_deviation, + Metric::yaw_deviation, + Metric::goal_longitudinal_deviation, + Metric::goal_lateral_deviation, + Metric::goal_yaw_deviation, + Metric::left_boundary_distance, + Metric::right_boundary_distance, + Metric::steering_angle, + Metric::steering_rate, + Metric::steering_acceleration, }; std::array, static_cast(Metric::SIZE)> metric_accumulators_; // 3(min, max, mean) * metric_size MetricArrayMsg metrics_msg_; + VehicleInfo vehicle_info_; autoware::route_handler::RouteHandler route_handler_; rclcpp::TimerBase::SharedPtr timer_; std::optional prev_acc_stamped_{std::nullopt}; + std::optional prev_steering_angle_{std::nullopt}; + std::optional prev_steering_rate_{std::nullopt}; + std::optional prev_steering_angle_timestamp_{std::nullopt}; }; } // namespace control_diagnostics diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp index e0e04b0a65070..514dfcc69ae35 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/deviation_metrics.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp index be2f3135d7f7e..39ef6fa26dfa8 100644 --- a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metric.hpp @@ -31,6 +31,11 @@ enum class Metric { goal_longitudinal_deviation, goal_lateral_deviation, goal_yaw_deviation, + left_boundary_distance, + right_boundary_distance, + steering_angle, + steering_rate, + steering_acceleration, SIZE, }; @@ -40,6 +45,11 @@ static const std::unordered_map str_to_metric = { {"goal_longitudinal_deviation", Metric::goal_longitudinal_deviation}, {"goal_lateral_deviation", Metric::goal_lateral_deviation}, {"goal_yaw_deviation", Metric::goal_yaw_deviation}, + {"left_boundary_distance", Metric::left_boundary_distance}, + {"right_boundary_distance", Metric::right_boundary_distance}, + {"steering_angle", Metric::steering_angle}, + {"steering_rate", Metric::steering_rate}, + {"steering_acceleration", Metric::steering_acceleration}, }; static const std::unordered_map metric_to_str = { @@ -48,6 +58,11 @@ static const std::unordered_map metric_to_str = { {Metric::goal_longitudinal_deviation, "goal_longitudinal_deviation"}, {Metric::goal_lateral_deviation, "goal_lateral_deviation"}, {Metric::goal_yaw_deviation, "goal_yaw_deviation"}, + {Metric::left_boundary_distance, "left_boundary_distance"}, + {Metric::right_boundary_distance, "right_boundary_distance"}, + {Metric::steering_angle, "steering_angle"}, + {Metric::steering_rate, "steering_rate"}, + {Metric::steering_acceleration, "steering_acceleration"}, }; // Metrics descriptions @@ -56,7 +71,13 @@ static const std::unordered_map metric_descriptions = { {Metric::yaw_deviation, "Yaw deviation from the reference trajectory[rad]"}, {Metric::goal_longitudinal_deviation, "Longitudinal deviation from the goal point[m]"}, {Metric::goal_lateral_deviation, "Lateral deviation from the goal point[m]"}, - {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"}}; + {Metric::goal_yaw_deviation, "Yaw deviation from the goal point[rad]"}, + {Metric::left_boundary_distance, "Signed distance to the left boundary[m]"}, + {Metric::right_boundary_distance, "Signed distance to the right boundary[m]"}, + {Metric::steering_angle, "Steering angle[rad]"}, + {Metric::steering_rate, "Steering angle rate[rad/s]"}, + {Metric::steering_acceleration, "Steering angle acceleration[rad/s^2]"}, +}; namespace details { diff --git a/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp new file mode 100644 index 0000000000000..9527b00aa3b9d --- /dev/null +++ b/evaluator/autoware_control_evaluator/include/autoware/control_evaluator/metrics/metrics_utils.hpp @@ -0,0 +1,62 @@ +// Copyright 2025 Tier 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_EVALUATOR__METRICS__METRICS_UTILS_HPP_ +#define AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_ + +#include +#include + +#include +namespace control_diagnostics +{ +namespace metrics +{ +namespace utils +{ +using autoware::route_handler::RouteHandler; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; + +/** + * @brief Get the closest lanelets to the ego vehicle, considering shoulder lanelets. + * @param [in] route_handler route handler + * @param [in] ego_pose ego vehicle pose + * @return closest lanelets to the ego vehicle + **/ +lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, const Pose & ego_pose); + +/** + * @brief Calculate the Euler distance between the vehicle and the lanelet. + * @param [in] vehicle_footprint vehicle footprint + * @param [in] line lanelet line + * @return distance between the vehicle footprint and the lanelet, 0.0 if the vehicle intersects + *with the line + **/ +double calc_distance_to_line( + const autoware::universe_utils::LinearRing2d & vehicle_footprint, + const autoware::universe_utils::LineString2d & line); + +/** + * @brief Check if the point is on the left side of the line. + * @param [in] point point + * @param [in] line line + * @return true if the ego vehicle is on the left side of the lanelet line, false otherwise + **/ +bool is_point_left_of_line(const Point & point, const std::vector & line); + +} // namespace utils +} // namespace metrics +} // namespace control_diagnostics +#endif // AUTOWARE__CONTROL_EVALUATOR__METRICS__METRICS_UTILS_HPP_ diff --git a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml index 4cf795afb5a7d..f1415410c13e4 100644 --- a/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml +++ b/evaluator/autoware_control_evaluator/launch/control_evaluator.launch.xml @@ -5,6 +5,8 @@ + + @@ -15,6 +17,8 @@ + + diff --git a/evaluator/autoware_control_evaluator/package.xml b/evaluator/autoware_control_evaluator/package.xml index a636a55bc5efd..8d32c2cf76711 100644 --- a/evaluator/autoware_control_evaluator/package.xml +++ b/evaluator/autoware_control_evaluator/package.xml @@ -2,7 +2,7 @@ autoware_control_evaluator - 0.40.0 + 0.41.0 ROS 2 node for evaluating control Daniel SANCHEZ takayuki MUROOKA @@ -21,6 +21,8 @@ autoware_route_handler autoware_test_utils autoware_universe_utils + autoware_vehicle_info_utils + autoware_vehicle_msgs nav_msgs nlohmann-json-dev pluginlib @@ -29,6 +31,7 @@ tf2 tf2_ros tier4_metric_msgs + tier4_planning_msgs ament_cmake_ros ament_lint_auto diff --git a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp index 9c8579469f878..435d1bb8c4dd3 100644 --- a/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,6 +14,9 @@ #include "autoware/control_evaluator/control_evaluator_node.hpp" +#include "autoware/control_evaluator/metrics/metrics_utils.hpp" + +#include #include #include #include @@ -21,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -28,13 +32,14 @@ namespace control_diagnostics { ControlEvaluatorNode::ControlEvaluatorNode(const rclcpp::NodeOptions & node_options) -: Node("control_evaluator", node_options) +: Node("control_evaluator", node_options), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()) { using std::placeholders::_1; // Publisher - processing_time_pub_ = - this->create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = this->create_publisher( + "~/debug/processing_time_ms", 1); metrics_pub_ = create_publisher("~/metrics", 1); // Parameters @@ -52,45 +57,51 @@ ControlEvaluatorNode::~ControlEvaluatorNode() return; } - // generate json data - using json = nlohmann::json; - json j; - for (Metric metric : metrics_) { - const std::string base_name = metric_to_str.at(metric) + "/"; - j[base_name + "min"] = metric_accumulators_[static_cast(metric)].min(); - j[base_name + "max"] = metric_accumulators_[static_cast(metric)].max(); - j[base_name + "mean"] = metric_accumulators_[static_cast(metric)].mean(); - j[base_name + "count"] = metric_accumulators_[static_cast(metric)].count(); - j[base_name + "description"] = metric_descriptions.at(metric); - } + try { + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + j[base_name + "min"] = metric_accumulators_[static_cast(metric)].min(); + j[base_name + "max"] = metric_accumulators_[static_cast(metric)].max(); + j[base_name + "mean"] = metric_accumulators_[static_cast(metric)].mean(); + j[base_name + "count"] = metric_accumulators_[static_cast(metric)].count(); + j[base_name + "description"] = metric_descriptions.at(metric); + } - // get output folder - const std::string output_folder_str = - rclcpp::get_logging_directory().string() + "/autoware_metrics"; - if (!std::filesystem::exists(output_folder_str)) { - if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); - return; + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } } - } - // get time stamp - std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm * local_time = std::localtime(&now_time_t); - std::ostringstream oss; - oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); - std::string cur_time_str = oss.str(); - - // Write metrics .json to file - const std::string output_file_str = - output_folder_str + "/autoware_control_evaluator-" + cur_time_str + ".json"; - std::ofstream f(output_file_str); - if (f.is_open()) { - f << j.dump(4); - f.close(); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_control_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } + } catch (const std::exception & e) { + std::cerr << "Exception in ControlEvaluatorNode destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in ControlEvaluatorNode destructor" << std::endl; } } @@ -129,17 +140,9 @@ void ControlEvaluatorNode::AddMetricMsg(const Metric & metric, const double & me } } -void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) +void ControlEvaluatorNode::AddLaneletInfoMsg(const Pose & ego_pose) { - const auto current_lanelets = [&]() { - lanelet::ConstLanelet closest_route_lanelet; - route_handler_.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); - const auto shoulder_lanelets = route_handler_.getShoulderLaneletsAtPose(ego_pose); - lanelet::ConstLanelets closest_lanelets{closest_route_lanelet}; - closest_lanelets.insert( - closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end()); - return closest_lanelets; - }(); + const auto current_lanelets = metrics::utils::get_current_lanes(route_handler_, ego_pose); const auto arc_coordinates = lanelet::utils::getArcCoordinates(current_lanelets, ego_pose); lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); @@ -166,6 +169,43 @@ void ControlEvaluatorNode::AddLaneletMetricMsg(const Pose & ego_pose) } } +void ControlEvaluatorNode::AddBoundaryDistanceMetricMsg( + const PathWithLaneId & behavior_path, const Pose & ego_pose) +{ + const auto current_lanelets = metrics::utils::get_current_lanes(route_handler_, ego_pose); + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet(current_lanelets, ego_pose, ¤t_lane); + const auto local_vehicle_footprint = vehicle_info_.createFootprint(); + const auto current_vehicle_footprint = + transformVector(local_vehicle_footprint, autoware::universe_utils::pose2transform(ego_pose)); + + if (behavior_path.left_bound.size() >= 1) { + LineString2d left_boundary; + for (const auto & p : behavior_path.left_bound) left_boundary.push_back(Point2d(p.x, p.y)); + double distance_to_left_boundary = + metrics::utils::calc_distance_to_line(current_vehicle_footprint, left_boundary); + + if (metrics::utils::is_point_left_of_line(ego_pose.position, behavior_path.left_bound)) { + distance_to_left_boundary *= -1.0; + } + const Metric metric_left = Metric::left_boundary_distance; + AddMetricMsg(metric_left, distance_to_left_boundary); + } + + if (behavior_path.right_bound.size() >= 1) { + LineString2d right_boundary; + for (const auto & p : behavior_path.right_bound) right_boundary.push_back(Point2d(p.x, p.y)); + double distance_to_right_boundary = + metrics::utils::calc_distance_to_line(current_vehicle_footprint, right_boundary); + + if (!metrics::utils::is_point_left_of_line(ego_pose.position, behavior_path.right_bound)) { + distance_to_right_boundary *= -1.0; + } + const Metric metric_right = Metric::right_boundary_distance; + AddMetricMsg(metric_right, distance_to_right_boundary); + } +} + void ControlEvaluatorNode::AddKinematicStateMetricMsg( const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped) { @@ -204,6 +244,47 @@ void ControlEvaluatorNode::AddKinematicStateMetricMsg( return; } +void ControlEvaluatorNode::AddSteeringMetricMsg(const SteeringReport & steering_status) +{ + // steering angle + double cur_steering_angle = steering_status.steering_tire_angle; + const double cur_t = static_cast(steering_status.stamp.sec) + + static_cast(steering_status.stamp.nanosec) * 1e-9; + AddMetricMsg(Metric::steering_angle, cur_steering_angle); + + if (!prev_steering_angle_timestamp_.has_value()) { + prev_steering_angle_timestamp_ = cur_t; + prev_steering_angle_ = cur_steering_angle; + return; + } + + // d_t + const double dt = cur_t - prev_steering_angle_timestamp_.value(); + if (dt < std::numeric_limits::epsilon()) { + prev_steering_angle_timestamp_ = cur_t; + prev_steering_angle_ = cur_steering_angle; + return; + } + + // steering rate + const double steering_rate = (cur_steering_angle - prev_steering_angle_.value()) / dt; + AddMetricMsg(Metric::steering_rate, steering_rate); + + // steering acceleration + if (!prev_steering_rate_.has_value()) { + prev_steering_angle_timestamp_ = cur_t; + prev_steering_angle_ = cur_steering_angle; + prev_steering_rate_ = steering_rate; + return; + } + const double steering_acceleration = (steering_rate - prev_steering_rate_.value()) / dt; + AddMetricMsg(Metric::steering_acceleration, steering_acceleration); + + prev_steering_angle_timestamp_ = cur_t; + prev_steering_angle_ = cur_steering_angle; + prev_steering_rate_ = steering_rate; +} + void ControlEvaluatorNode::AddLateralDeviationMetricMsg( const Trajectory & traj, const Point & ego_point) { @@ -253,6 +334,8 @@ void ControlEvaluatorNode::onTimer() const auto traj = traj_sub_.takeData(); const auto odom = odometry_sub_.takeData(); const auto acc = accel_sub_.takeData(); + const auto behavior_path = behavior_path_subscriber_.takeData(); + const auto steering_status = steering_sub_.takeData(); // calculate deviation metrics if (odom && traj && !traj->points.empty()) { @@ -264,8 +347,7 @@ void ControlEvaluatorNode::onTimer() getRouteData(); if (odom && route_handler_.isHandlerReady()) { const Pose ego_pose = odom->pose.pose; - AddLaneletMetricMsg(ego_pose); - + AddLaneletInfoMsg(ego_pose); AddGoalLongitudinalDeviationMetricMsg(ego_pose); AddGoalLateralDeviationMetricMsg(ego_pose); AddGoalYawDeviationMetricMsg(ego_pose); @@ -275,13 +357,22 @@ void ControlEvaluatorNode::onTimer() AddKinematicStateMetricMsg(*odom, *acc); } + if (odom && behavior_path) { + const Pose ego_pose = odom->pose.pose; + AddBoundaryDistanceMetricMsg(*behavior_path, ego_pose); + } + + if (steering_status) { + AddSteeringMetricMsg(*steering_status); + } + // Publish metrics metrics_msg_.stamp = now(); metrics_pub_->publish(metrics_msg_); metrics_msg_ = MetricArrayMsg{}; // Publish processing time - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp index a851eeb410620..aa5b47ad31a4b 100644 --- a/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_control_evaluator/src/metrics/deviation_metrics.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp new file mode 100644 index 0000000000000..3fe95d4decb8d --- /dev/null +++ b/evaluator/autoware_control_evaluator/src/metrics/metrics_utils.cpp @@ -0,0 +1,69 @@ +// Copyright 2025 TIER 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_evaluator/metrics/metrics_utils.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include +#include + +#include +// #include + +#include + +#include +#include +namespace control_diagnostics +{ +namespace metrics +{ +namespace utils +{ +using autoware::route_handler::RouteHandler; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; + +lanelet::ConstLanelets get_current_lanes(const RouteHandler & route_handler, const Pose & ego_pose) +{ + lanelet::ConstLanelet closest_route_lanelet; + route_handler.getClosestLaneletWithinRoute(ego_pose, &closest_route_lanelet); + const auto shoulder_lanelets = route_handler.getShoulderLaneletsAtPose(ego_pose); + lanelet::ConstLanelets closest_lanelets{closest_route_lanelet}; + closest_lanelets.insert( + closest_lanelets.end(), shoulder_lanelets.begin(), shoulder_lanelets.end()); + return closest_lanelets; +} + +double calc_distance_to_line( + const autoware::universe_utils::LinearRing2d & vehicle_footprint, + const autoware::universe_utils::LineString2d & line) +{ + return boost::geometry::distance(vehicle_footprint, line); +} + +bool is_point_left_of_line(const Point & point, const std::vector & line) +{ + const size_t closest_idx = autoware::motion_utils::findNearestSegmentIndex(line, point); + const auto & p1 = line[closest_idx]; + const auto & p2 = line[closest_idx + 1]; + return ((p2.x - p1.x) * (point.y - p1.y) - (p2.y - p1.y) * (point.x - p1.x)) > 0; +} + +} // namespace utils +} // namespace metrics +} // namespace control_diagnostics diff --git a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp index 9098ce5667424..b2dcc93855c0f 100644 --- a/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp +++ b/evaluator/autoware_control_evaluator/test/test_control_evaluator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -51,11 +51,13 @@ class EvalTest : public ::testing::Test rclcpp::init(0, nullptr); rclcpp::NodeOptions options; - const auto share_dir = - ament_index_cpp::get_package_share_directory("autoware_control_evaluator"); - options.arguments({"--ros-args", "-p", "output_metrics:=false"}); + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + options.arguments( + {"--ros-args", "-p", "output_metrics:=false", "--params-file", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml"}); - dummy_node = std::make_shared("control_evaluator_test_node"); + dummy_node = std::make_shared("control_evaluator_test_node", options); eval_node = std::make_shared(options); // Enable all logging in the node auto ret = rcutils_logging_set_logger_level( diff --git a/evaluator/kinematic_evaluator/CHANGELOG.rst b/evaluator/autoware_kinematic_evaluator/CHANGELOG.rst similarity index 93% rename from evaluator/kinematic_evaluator/CHANGELOG.rst rename to evaluator/autoware_kinematic_evaluator/CHANGELOG.rst index 2283ea3b36a95..d14c5972e9e07 100644 --- a/evaluator/kinematic_evaluator/CHANGELOG.rst +++ b/evaluator/autoware_kinematic_evaluator/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package kinematic_evaluator -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_kinematic_evaluator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `evaluator/kinematic_evaluator` (`#9936 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/evaluator/kinematic_evaluator/CMakeLists.txt b/evaluator/autoware_kinematic_evaluator/CMakeLists.txt similarity index 67% rename from evaluator/kinematic_evaluator/CMakeLists.txt rename to evaluator/autoware_kinematic_evaluator/CMakeLists.txt index 6c1b6abc3dace..721f164016c6c 100644 --- a/evaluator/kinematic_evaluator/CMakeLists.txt +++ b/evaluator/autoware_kinematic_evaluator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(kinematic_evaluator) +project(autoware_kinematic_evaluator) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -15,26 +15,25 @@ autoware_package() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(${PROJECT_NAME}_node SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/metrics_calculator.cpp - src/${PROJECT_NAME}_node.cpp src/kinematic_evaluator_node.cpp src/metrics/kinematic_metrics.cpp ) -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "kinematic_diagnostics::KinematicEvaluatorNode" - EXECUTABLE ${PROJECT_NAME} +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::kinematic_diagnostics::KinematicEvaluatorNode" + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - ament_add_gtest(test_${PROJECT_NAME} + ament_add_gtest(test_kinematic_evaluator test/test_kinematic_evaluator_node.cpp ) - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME}_node + target_link_libraries(test_kinematic_evaluator + ${PROJECT_NAME} ) endif() diff --git a/evaluator/autoware_kinematic_evaluator/README.md b/evaluator/autoware_kinematic_evaluator/README.md new file mode 100644 index 0000000000000..341229bd0aac6 --- /dev/null +++ b/evaluator/autoware_kinematic_evaluator/README.md @@ -0,0 +1,7 @@ +# autoware_kinematic_evaluator + +TBD + +## Parameters + +{{json_to_markdown("evaluator/autoware_kinematic_evaluator/schema/kinematic_evaluator.schema.json")}} diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/kinematic_evaluator_node.hpp similarity index 86% rename from evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp rename to evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/kinematic_evaluator_node.hpp index d401f63649468..053fbfc11f192 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/kinematic_evaluator_node.hpp +++ b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/kinematic_evaluator_node.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ -#define KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ +#ifndef AUTOWARE__KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ +#define AUTOWARE__KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ +#include "autoware/kinematic_evaluator/metrics_calculator.hpp" #include "autoware/universe_utils/math/accumulator.hpp" -#include "kinematic_evaluator/metrics_calculator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -31,7 +31,7 @@ #include #include -namespace kinematic_diagnostics +namespace autoware::kinematic_diagnostics { using autoware::universe_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; @@ -78,6 +78,6 @@ class KinematicEvaluatorNode : public rclcpp::Node std::array>, static_cast(Metric::SIZE)> metric_stats_; std::unordered_map> metrics_dict_; }; -} // namespace kinematic_diagnostics +} // namespace autoware::kinematic_diagnostics -#endif // KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ +#endif // AUTOWARE__KINEMATIC_EVALUATOR__KINEMATIC_EVALUATOR_NODE_HPP_ diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp similarity index 78% rename from evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp rename to evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp index e7d18910e7c39..2ae1f60902de7 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/kinematic_metrics.hpp +++ b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ -#define KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ +#ifndef AUTOWARE__KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ +#define AUTOWARE__KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ #include "autoware/universe_utils/math/accumulator.hpp" #include -namespace kinematic_diagnostics +namespace autoware::kinematic_diagnostics { namespace metrics { @@ -35,6 +35,6 @@ using nav_msgs::msg::Odometry; Accumulator updateVelocityStats(const double & value, const Accumulator stat_prev); } // namespace metrics -} // namespace kinematic_diagnostics +} // namespace autoware::kinematic_diagnostics -#endif // KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ +#endif // AUTOWARE__KINEMATIC_EVALUATOR__METRICS__KINEMATIC_METRICS_HPP_ diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/metric.hpp b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/metric.hpp similarity index 86% rename from evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/metric.hpp rename to evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/metric.hpp index 2e46d655312d8..01d645355b4f5 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics/metric.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_ -#define KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_ +#ifndef AUTOWARE__KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_ +#define AUTOWARE__KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_ #include #include #include #include -namespace kinematic_diagnostics +namespace autoware::kinematic_diagnostics { /** * @brief Enumeration of velocity metrics @@ -57,6 +57,6 @@ static struct CheckCorrectMaps } check; } // namespace details -} // namespace kinematic_diagnostics +} // namespace autoware::kinematic_diagnostics -#endif // KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_ +#endif // AUTOWARE__KINEMATIC_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics_calculator.hpp similarity index 78% rename from evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp rename to evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics_calculator.hpp index 372d4a34af62c..0a70d6477de1a 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/metrics_calculator.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ -#define KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ +#ifndef AUTOWARE__KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define AUTOWARE__KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ +#include "autoware/kinematic_evaluator/metrics/metric.hpp" +#include "autoware/kinematic_evaluator/parameters.hpp" #include "autoware/universe_utils/math/accumulator.hpp" -#include "kinematic_evaluator/metrics/metric.hpp" -#include "kinematic_evaluator/parameters.hpp" #include "geometry_msgs/msg/pose.hpp" #include -namespace kinematic_diagnostics +namespace autoware::kinematic_diagnostics { using autoware::universe_utils::Accumulator; using nav_msgs::msg::Odometry; @@ -52,6 +52,6 @@ class MetricsCalculator const Metric metric, const Odometry & odom, const Accumulator stat_prev) const; }; // class MetricsCalculator -} // namespace kinematic_diagnostics +} // namespace autoware::kinematic_diagnostics -#endif // KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ +#endif // AUTOWARE__KINEMATIC_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/parameters.hpp similarity index 71% rename from evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp rename to evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/parameters.hpp index d8328fe62b3ed..22e6fc91720c2 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/parameters.hpp +++ b/evaluator/autoware_kinematic_evaluator/include/autoware/kinematic_evaluator/parameters.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_EVALUATOR__PARAMETERS_HPP_ -#define LOCALIZATION_EVALUATOR__PARAMETERS_HPP_ +#ifndef AUTOWARE__KINEMATIC_EVALUATOR__PARAMETERS_HPP_ +#define AUTOWARE__KINEMATIC_EVALUATOR__PARAMETERS_HPP_ -#include "localization_evaluator/metrics/metric.hpp" +#include "autoware/kinematic_evaluator/metrics/metric.hpp" #include -namespace localization_diagnostics +namespace autoware::kinematic_diagnostics { /** * @brief Enumeration of trajectory metrics @@ -29,6 +29,6 @@ struct Parameters std::array(Metric::SIZE)> metrics{}; // default values to false }; // struct Parameters -} // namespace localization_diagnostics +} // namespace autoware::kinematic_diagnostics -#endif // LOCALIZATION_EVALUATOR__PARAMETERS_HPP_ +#endif // AUTOWARE__KINEMATIC_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/autoware_kinematic_evaluator/launch/kinematic_evaluator.launch.xml b/evaluator/autoware_kinematic_evaluator/launch/kinematic_evaluator.launch.xml new file mode 100644 index 0000000000000..60818f875b9d0 --- /dev/null +++ b/evaluator/autoware_kinematic_evaluator/launch/kinematic_evaluator.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/autoware_kinematic_evaluator/package.xml similarity index 90% rename from evaluator/kinematic_evaluator/package.xml rename to evaluator/autoware_kinematic_evaluator/package.xml index fbd5df3f27765..7118702290f27 100644 --- a/evaluator/kinematic_evaluator/package.xml +++ b/evaluator/autoware_kinematic_evaluator/package.xml @@ -1,7 +1,7 @@ - kinematic_evaluator - 0.40.0 + autoware_kinematic_evaluator + 0.41.0 kinematic evaluator ROS 2 node Dominik Jargot @@ -11,6 +11,7 @@ Takayuki Murooka Fumiya Watanabe Satoshi Ota + Junya Sasaki Apache License 2.0 diff --git a/evaluator/kinematic_evaluator/param/kinematic_evaluator.defaults.yaml b/evaluator/autoware_kinematic_evaluator/param/kinematic_evaluator.defaults.yaml similarity index 100% rename from evaluator/kinematic_evaluator/param/kinematic_evaluator.defaults.yaml rename to evaluator/autoware_kinematic_evaluator/param/kinematic_evaluator.defaults.yaml diff --git a/evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json b/evaluator/autoware_kinematic_evaluator/schema/kinematic_evaluator.schema.json similarity index 100% rename from evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json rename to evaluator/autoware_kinematic_evaluator/schema/kinematic_evaluator.schema.json diff --git a/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp b/evaluator/autoware_kinematic_evaluator/src/kinematic_evaluator_node.cpp similarity index 95% rename from evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp rename to evaluator/autoware_kinematic_evaluator/src/kinematic_evaluator_node.cpp index d34f9deb7f1ba..93a6bf9f046b1 100644 --- a/evaluator/kinematic_evaluator/src/kinematic_evaluator_node.cpp +++ b/evaluator/autoware_kinematic_evaluator/src/kinematic_evaluator_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kinematic_evaluator/kinematic_evaluator_node.hpp" +#include "autoware/kinematic_evaluator/kinematic_evaluator_node.hpp" #include "boost/lexical_cast.hpp" @@ -22,7 +22,7 @@ #include #include -namespace kinematic_diagnostics +namespace autoware::kinematic_diagnostics { KinematicEvaluatorNode::KinematicEvaluatorNode(const rclcpp::NodeOptions & node_options) : Node("kinematic_evaluator", node_options) @@ -139,7 +139,7 @@ geometry_msgs::msg::Pose KinematicEvaluatorNode::getCurrentEgoPose() const return p; } -} // namespace kinematic_diagnostics +} // namespace autoware::kinematic_diagnostics #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(kinematic_diagnostics::KinematicEvaluatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::kinematic_diagnostics::KinematicEvaluatorNode) diff --git a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp b/evaluator/autoware_kinematic_evaluator/src/metrics/kinematic_metrics.cpp similarity index 64% rename from evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp rename to evaluator/autoware_kinematic_evaluator/src/metrics/kinematic_metrics.cpp index d410863a05010..c686a60436803 100644 --- a/evaluator/kinematic_evaluator/src/metrics/kinematic_metrics.cpp +++ b/evaluator/autoware_kinematic_evaluator/src/metrics/kinematic_metrics.cpp @@ -12,19 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kinematic_evaluator/metrics/kinematic_metrics.hpp" +#include "autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp" -namespace kinematic_diagnostics +namespace autoware::kinematic_diagnostics { namespace metrics { -Accumulator updateVelocityStats(const double & value, const Accumulator stat_prev) +autoware::universe_utils::Accumulator updateVelocityStats( + const double & value, const autoware::universe_utils::Accumulator stat_prev) { - Accumulator stat(stat_prev); + autoware::universe_utils::Accumulator stat(stat_prev); stat.add(value); return stat; } } // namespace metrics -} // namespace kinematic_diagnostics +} // namespace autoware::kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_kinematic_evaluator/src/metrics_calculator.cpp similarity index 82% rename from evaluator/kinematic_evaluator/src/metrics_calculator.cpp rename to evaluator/autoware_kinematic_evaluator/src/metrics_calculator.cpp index 8b0f581817b38..85b3711ce7c8f 100644 --- a/evaluator/kinematic_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_kinematic_evaluator/src/metrics_calculator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "kinematic_evaluator/metrics_calculator.hpp" +#include "autoware/kinematic_evaluator/metrics_calculator.hpp" -#include "kinematic_evaluator/metrics/kinematic_metrics.hpp" +#include "autoware/kinematic_evaluator/metrics/kinematic_metrics.hpp" -namespace kinematic_diagnostics +namespace autoware::kinematic_diagnostics { Accumulator MetricsCalculator::updateStat( const Metric metric, const Odometry & odom, const Accumulator stat_prev) const @@ -31,4 +31,4 @@ Accumulator MetricsCalculator::updateStat( } } -} // namespace kinematic_diagnostics +} // namespace autoware::kinematic_diagnostics diff --git a/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp b/evaluator/autoware_kinematic_evaluator/test/test_kinematic_evaluator_node.cpp similarity index 89% rename from evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp rename to evaluator/autoware_kinematic_evaluator/test/test_kinematic_evaluator_node.cpp index 769f98416848f..9e8288a3a7f91 100644 --- a/evaluator/kinematic_evaluator/test/test_kinematic_evaluator_node.cpp +++ b/evaluator/autoware_kinematic_evaluator/test/test_kinematic_evaluator_node.cpp @@ -18,7 +18,7 @@ #include "rclcpp/time.hpp" #include "tf2_ros/transform_broadcaster.h" -#include +#include #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -32,7 +32,7 @@ #include #include -using EvalNode = kinematic_diagnostics::KinematicEvaluatorNode; +using EvalNode = autoware::kinematic_diagnostics::KinematicEvaluatorNode; using diagnostic_msgs::msg::DiagnosticArray; using nav_msgs::msg::Odometry; @@ -44,7 +44,8 @@ class EvalTest : public ::testing::Test rclcpp::init(0, nullptr); rclcpp::NodeOptions options; - const auto share_dir = ament_index_cpp::get_package_share_directory("kinematic_evaluator"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_kinematic_evaluator"); options.arguments( {"--ros-args", "--params-file", share_dir + "/param/kinematic_evaluator.defaults.yaml"}); @@ -70,9 +71,9 @@ class EvalTest : public ::testing::Test ~EvalTest() override { /*rclcpp::shutdown();*/ } - void setTargetMetric(kinematic_diagnostics::Metric metric) + void setTargetMetric(autoware::kinematic_diagnostics::Metric metric) { - const auto metric_str = kinematic_diagnostics::metric_to_str.at(metric); + const auto metric_str = autoware::kinematic_diagnostics::metric_to_str.at(metric); const auto is_target_metric = [metric_str](const auto & status) { return status.name == metric_str; }; @@ -130,7 +131,7 @@ class EvalTest : public ::testing::Test TEST_F(EvalTest, TestVelocityStats) { - setTargetMetric(kinematic_diagnostics::Metric::velocity_stats); + setTargetMetric(autoware::kinematic_diagnostics::Metric::velocity_stats); Odometry odom = makeOdometry(0.0); EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom), 0.0); Odometry odom2 = makeOdometry(1.0); diff --git a/evaluator/localization_evaluator/CHANGELOG.rst b/evaluator/autoware_localization_evaluator/CHANGELOG.rst similarity index 84% rename from evaluator/localization_evaluator/CHANGELOG.rst rename to evaluator/autoware_localization_evaluator/CHANGELOG.rst index 0c24579d3004d..b21f49b633da2 100644 --- a/evaluator/localization_evaluator/CHANGELOG.rst +++ b/evaluator/autoware_localization_evaluator/CHANGELOG.rst @@ -1,7 +1,26 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package localization_evaluator +Changelog for package autoware_localization_evaluator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `evaluator/localization_evaluator` (`#9954 `_) + * feat(localization_evaluator): apply `autoware\_` prefix (see below): + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(localization_evaluator): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(localization_evaluator): fix include paths + * To follow the previous commit + * rename: `localization_evaluator` => `autoware_localization_evaluator` + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/evaluator/localization_evaluator/CMakeLists.txt b/evaluator/autoware_localization_evaluator/CMakeLists.txt similarity index 67% rename from evaluator/localization_evaluator/CMakeLists.txt rename to evaluator/autoware_localization_evaluator/CMakeLists.txt index 0cdcc420eb797..f926353708e90 100644 --- a/evaluator/localization_evaluator/CMakeLists.txt +++ b/evaluator/autoware_localization_evaluator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(localization_evaluator) +project(autoware_localization_evaluator) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -15,26 +15,25 @@ autoware_package() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(${PROJECT_NAME}_node SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/metrics_calculator.cpp - src/${PROJECT_NAME}_node.cpp src/localization_evaluator_node.cpp src/metrics/localization_metrics.cpp ) -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "localization_diagnostics::LocalizationEvaluatorNode" - EXECUTABLE ${PROJECT_NAME} +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::localization_diagnostics::LocalizationEvaluatorNode" + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - ament_add_gtest(test_${PROJECT_NAME} + ament_add_gtest(test_localization_evaluator test/test_localization_evaluator_node.cpp ) - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME}_node + target_link_libraries(test_localization_evaluator + ${PROJECT_NAME} ) endif() diff --git a/evaluator/localization_evaluator/README.md b/evaluator/autoware_localization_evaluator/README.md similarity index 100% rename from evaluator/localization_evaluator/README.md rename to evaluator/autoware_localization_evaluator/README.md diff --git a/evaluator/localization_evaluator/config/localization_evaluator.param.yaml b/evaluator/autoware_localization_evaluator/config/localization_evaluator.param.yaml similarity index 100% rename from evaluator/localization_evaluator/config/localization_evaluator.param.yaml rename to evaluator/autoware_localization_evaluator/config/localization_evaluator.param.yaml diff --git a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/localization_evaluator_node.hpp similarity index 87% rename from evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp rename to evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/localization_evaluator_node.hpp index feb61dd3cacbe..99f38df4d35e1 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/localization_evaluator_node.hpp +++ b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/localization_evaluator_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ -#define LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ +#ifndef AUTOWARE__LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ +#define AUTOWARE__LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ +#include "autoware/localization_evaluator/metrics_calculator.hpp" #include "autoware/universe_utils/math/accumulator.hpp" -#include "localization_evaluator/metrics_calculator.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -36,7 +36,7 @@ #include #include -namespace localization_diagnostics +namespace autoware::localization_diagnostics { using autoware::universe_utils::Accumulator; using diagnostic_msgs::msg::DiagnosticArray; @@ -97,6 +97,6 @@ class LocalizationEvaluatorNode : public rclcpp::Node std::array>, static_cast(Metric::SIZE)> metric_stats_; std::unordered_map> metrics_dict_; }; -} // namespace localization_diagnostics +} // namespace autoware::localization_diagnostics -#endif // LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ +#endif // AUTOWARE__LOCALIZATION_EVALUATOR__LOCALIZATION_EVALUATOR_NODE_HPP_ diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/localization_metrics.hpp similarity index 80% rename from evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp rename to evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/localization_metrics.hpp index 82119efca6082..78b9e446f7cce 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/metrics/localization_metrics.hpp +++ b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/localization_metrics.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ -#define LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ +#ifndef AUTOWARE__LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ +#define AUTOWARE__LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ #include "autoware/universe_utils/math/accumulator.hpp" #include -namespace localization_diagnostics +namespace autoware::localization_diagnostics { using autoware::universe_utils::Accumulator; namespace metrics @@ -46,6 +46,6 @@ Accumulator updateAbsoluteStats( const geometry_msgs::msg::Point & pos_ref); } // namespace metrics -} // namespace localization_diagnostics +} // namespace autoware::localization_diagnostics -#endif // LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ +#endif // AUTOWARE__LOCALIZATION_EVALUATOR__METRICS__LOCALIZATION_METRICS_HPP_ diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics/metric.hpp b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/metric.hpp similarity index 85% rename from evaluator/localization_evaluator/include/localization_evaluator/metrics/metric.hpp rename to evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/metric.hpp index dfe1a538a3488..0d791d6e2365a 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics/metric.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_EVALUATOR__METRICS__METRIC_HPP_ -#define LOCALIZATION_EVALUATOR__METRICS__METRIC_HPP_ +#ifndef AUTOWARE__LOCALIZATION_EVALUATOR__METRICS__METRIC_HPP_ +#define AUTOWARE__LOCALIZATION_EVALUATOR__METRICS__METRIC_HPP_ #include #include #include #include -namespace localization_diagnostics +namespace autoware::localization_diagnostics { /** * @brief Enumeration of localization metrics @@ -58,6 +58,6 @@ static struct CheckCorrectMaps } check; } // namespace details -} // namespace localization_diagnostics +} // namespace autoware::localization_diagnostics -#endif // LOCALIZATION_EVALUATOR__METRICS__METRIC_HPP_ +#endif // AUTOWARE__LOCALIZATION_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics_calculator.hpp similarity index 74% rename from evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp rename to evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics_calculator.hpp index 310d1b25e92f1..6fb86dc9e8d7b 100644 --- a/evaluator/localization_evaluator/include/localization_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/metrics_calculator.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ -#define LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#ifndef AUTOWARE__LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define AUTOWARE__LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#include "autoware/localization_evaluator/metrics/metric.hpp" +#include "autoware/localization_evaluator/parameters.hpp" #include "autoware/universe_utils/math/accumulator.hpp" -#include "localization_evaluator/metrics/metric.hpp" -#include "localization_evaluator/parameters.hpp" #include "geometry_msgs/msg/pose.hpp" #include -namespace localization_diagnostics +namespace autoware::localization_diagnostics { using autoware::universe_utils::Accumulator; class MetricsCalculator @@ -44,6 +44,6 @@ class MetricsCalculator const geometry_msgs::msg::Point & pos_ref) const; }; // class MetricsCalculator -} // namespace localization_diagnostics +} // namespace autoware::localization_diagnostics -#endif // LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ +#endif // AUTOWARE__LOCALIZATION_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/parameters.hpp similarity index 67% rename from evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp rename to evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/parameters.hpp index dada46f44292f..20b530ea272e6 100644 --- a/evaluator/kinematic_evaluator/include/kinematic_evaluator/parameters.hpp +++ b/evaluator/autoware_localization_evaluator/include/autoware/localization_evaluator/parameters.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef KINEMATIC_EVALUATOR__PARAMETERS_HPP_ -#define KINEMATIC_EVALUATOR__PARAMETERS_HPP_ +#ifndef AUTOWARE__LOCALIZATION_EVALUATOR__PARAMETERS_HPP_ +#define AUTOWARE__LOCALIZATION_EVALUATOR__PARAMETERS_HPP_ -#include "kinematic_evaluator/metrics/metric.hpp" +#include "autoware/localization_evaluator/metrics/metric.hpp" #include -namespace kinematic_diagnostics +namespace autoware::localization_diagnostics { /** * @brief Enumeration of trajectory metrics @@ -29,6 +29,6 @@ struct Parameters std::array(Metric::SIZE)> metrics{}; // default values to false }; // struct Parameters -} // namespace kinematic_diagnostics +} // namespace autoware::localization_diagnostics -#endif // KINEMATIC_EVALUATOR__PARAMETERS_HPP_ +#endif // AUTOWARE__LOCALIZATION_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/localization_evaluator/launch/localization_evaluator.launch.xml b/evaluator/autoware_localization_evaluator/launch/localization_evaluator.launch.xml similarity index 61% rename from evaluator/localization_evaluator/launch/localization_evaluator.launch.xml rename to evaluator/autoware_localization_evaluator/launch/localization_evaluator.launch.xml index f8595f9d23c37..6a20c6ddc8583 100644 --- a/evaluator/localization_evaluator/launch/localization_evaluator.launch.xml +++ b/evaluator/autoware_localization_evaluator/launch/localization_evaluator.launch.xml @@ -2,8 +2,8 @@ - - + + diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/autoware_localization_evaluator/package.xml similarity index 90% rename from evaluator/localization_evaluator/package.xml rename to evaluator/autoware_localization_evaluator/package.xml index 7058064f17b72..69f4b4eeb818c 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/autoware_localization_evaluator/package.xml @@ -1,7 +1,7 @@ - localization_evaluator - 0.40.0 + autoware_localization_evaluator + 0.41.0 localization evaluator ROS 2 node Dominik Jargot @@ -11,6 +11,7 @@ Shintaro Sakoda Taiki Yamada Yamato Ando + Junya Sasaki Apache License 2.0 diff --git a/evaluator/localization_evaluator/schema/localization_evaluator.schema.json b/evaluator/autoware_localization_evaluator/schema/localization_evaluator.schema.json similarity index 100% rename from evaluator/localization_evaluator/schema/localization_evaluator.schema.json rename to evaluator/autoware_localization_evaluator/schema/localization_evaluator.schema.json diff --git a/evaluator/localization_evaluator/src/localization_evaluator_node.cpp b/evaluator/autoware_localization_evaluator/src/localization_evaluator_node.cpp similarity index 94% rename from evaluator/localization_evaluator/src/localization_evaluator_node.cpp rename to evaluator/autoware_localization_evaluator/src/localization_evaluator_node.cpp index 8a9352d2a73e8..eedfd6f955b88 100644 --- a/evaluator/localization_evaluator/src/localization_evaluator_node.cpp +++ b/evaluator/autoware_localization_evaluator/src/localization_evaluator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_evaluator/localization_evaluator_node.hpp" +#include "autoware/localization_evaluator/localization_evaluator_node.hpp" #include "boost/lexical_cast.hpp" @@ -23,7 +23,7 @@ #include #include -namespace localization_diagnostics +namespace autoware::localization_diagnostics { LocalizationEvaluatorNode::LocalizationEvaluatorNode(const rclcpp::NodeOptions & node_options) : Node("localization_evaluator", node_options), @@ -134,7 +134,7 @@ void LocalizationEvaluatorNode::syncCallback( metrics_pub_->publish(metrics_msg); } } -} // namespace localization_diagnostics +} // namespace autoware::localization_diagnostics #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(localization_diagnostics::LocalizationEvaluatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::localization_diagnostics::LocalizationEvaluatorNode) diff --git a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp b/evaluator/autoware_localization_evaluator/src/metrics/localization_metrics.cpp similarity index 86% rename from evaluator/localization_evaluator/src/metrics/localization_metrics.cpp rename to evaluator/autoware_localization_evaluator/src/metrics/localization_metrics.cpp index 97fd8e326cca7..00673f5883069 100644 --- a/evaluator/localization_evaluator/src/metrics/localization_metrics.cpp +++ b/evaluator/autoware_localization_evaluator/src/metrics/localization_metrics.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_evaluator/metrics/localization_metrics.hpp" +#include "autoware/localization_evaluator/metrics/localization_metrics.hpp" #include -namespace localization_diagnostics +namespace autoware::localization_diagnostics { namespace metrics { @@ -43,4 +43,4 @@ Accumulator updateAbsoluteStats( } } // namespace metrics -} // namespace localization_diagnostics +} // namespace autoware::localization_diagnostics diff --git a/evaluator/localization_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_localization_evaluator/src/metrics_calculator.cpp similarity index 82% rename from evaluator/localization_evaluator/src/metrics_calculator.cpp rename to evaluator/autoware_localization_evaluator/src/metrics_calculator.cpp index 864fb4a5ddbd2..887a042a22403 100644 --- a/evaluator/localization_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_localization_evaluator/src/metrics_calculator.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_evaluator/metrics_calculator.hpp" +#include "autoware/localization_evaluator/metrics_calculator.hpp" -#include "localization_evaluator/metrics/localization_metrics.hpp" +#include "autoware/localization_evaluator/metrics/localization_metrics.hpp" -namespace localization_diagnostics +namespace autoware::localization_diagnostics { Accumulator MetricsCalculator::updateStat( const Accumulator stat_prev, const Metric metric, const geometry_msgs::msg::Point & pos, @@ -39,4 +39,4 @@ Accumulator MetricsCalculator::updateStat( } } -} // namespace localization_diagnostics +} // namespace autoware::localization_diagnostics diff --git a/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp b/evaluator/autoware_localization_evaluator/test/test_localization_evaluator_node.cpp similarity index 89% rename from evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp rename to evaluator/autoware_localization_evaluator/test/test_localization_evaluator_node.cpp index 6ab428c6fb578..43367ff4ee6f1 100644 --- a/evaluator/localization_evaluator/test/test_localization_evaluator_node.cpp +++ b/evaluator/autoware_localization_evaluator/test/test_localization_evaluator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,7 +18,7 @@ #include "rclcpp/time.hpp" #include "tf2_ros/transform_broadcaster.h" -#include +#include #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" @@ -33,7 +33,7 @@ #include #include -using EvalNode = localization_diagnostics::LocalizationEvaluatorNode; +using EvalNode = autoware::localization_diagnostics::LocalizationEvaluatorNode; using diagnostic_msgs::msg::DiagnosticArray; using geometry_msgs::msg::PoseWithCovarianceStamped; using nav_msgs::msg::Odometry; @@ -46,7 +46,8 @@ class EvalTest : public ::testing::Test rclcpp::init(0, nullptr); rclcpp::NodeOptions options; - const auto share_dir = ament_index_cpp::get_package_share_directory("localization_evaluator"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_localization_evaluator"); options.arguments( {"--ros-args", "--params-file", share_dir + "/config/localization_evaluator.param.yaml"}); @@ -74,9 +75,9 @@ class EvalTest : public ::testing::Test ~EvalTest() override { rclcpp::shutdown(); } - void setTargetMetric(localization_diagnostics::Metric metric) + void setTargetMetric(autoware::localization_diagnostics::Metric metric) { - const auto metric_str = localization_diagnostics::metric_to_str.at(metric); + const auto metric_str = autoware::localization_diagnostics::metric_to_str.at(metric); const auto is_target_metric = [metric_str](const auto & status) { return status.name == metric_str; }; @@ -141,7 +142,7 @@ class EvalTest : public ::testing::Test TEST_F(EvalTest, TestLateralErrorStats) { - setTargetMetric(localization_diagnostics::Metric::lateral_error); + setTargetMetric(autoware::localization_diagnostics::Metric::lateral_error); Odometry odom = makeOdometry(1.0, 1.0, 0.0); PoseWithCovarianceStamped pos_ref = makePos(4.0, 5.0, 0.0); EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom, pos_ref), 3.0); @@ -152,7 +153,7 @@ TEST_F(EvalTest, TestLateralErrorStats) TEST_F(EvalTest, TestAbsoluteErrorStats) { - setTargetMetric(localization_diagnostics::Metric::absolute_error); + setTargetMetric(autoware::localization_diagnostics::Metric::absolute_error); Odometry odom = makeOdometry(1.0, 1.0, 0.0); PoseWithCovarianceStamped pos_ref = makePos(4.0, 5.0, 0.0); EXPECT_DOUBLE_EQ(publishOdometryAndGetMetric(odom, pos_ref), 5.0); diff --git a/evaluator/perception_online_evaluator/CHANGELOG.rst b/evaluator/autoware_perception_online_evaluator/CHANGELOG.rst similarity index 87% rename from evaluator/perception_online_evaluator/CHANGELOG.rst rename to evaluator/autoware_perception_online_evaluator/CHANGELOG.rst index c3ffd03cce3b1..551ae877439f8 100644 --- a/evaluator/perception_online_evaluator/CHANGELOG.rst +++ b/evaluator/autoware_perception_online_evaluator/CHANGELOG.rst @@ -1,7 +1,32 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package perception_online_evaluator +Changelog for package autoware_perception_online_evaluator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `perception_online_evaluator` (`#9956 `_) + * feat(perception_online_evaluator): apply `autoware\_` prefix (see below): + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * bug(perception_online_evaluator): remove duplicated properties + * It seems the `motion_evaluator` is defined and used in the `autoware_planning_evaluator` + * rename(perception_online_evaluator): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(perception_online_evaluator): fix include paths + * To follow the previous commit + * rename: `perception_online_evaluator` => `autoware_perception_online_evaluator` + * style(pre-commit): autofix + * bug(autoware_perception_online_evaluator): revert wrongly updated copyright + * bug(autoware_perception_online_evaluator): `autoware\_` prefix is not needed here + * update: `CODEOWNERS` + * bug(autoware_perception_online_evaluator): fix a wrong package name + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/evaluator/autoware_perception_online_evaluator/CMakeLists.txt b/evaluator/autoware_perception_online_evaluator/CMakeLists.txt new file mode 100644 index 0000000000000..cddc59e1a91c6 --- /dev/null +++ b/evaluator/autoware_perception_online_evaluator/CMakeLists.txt @@ -0,0 +1,39 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_perception_online_evaluator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(pluginlib REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/metrics_calculator.cpp + src/perception_online_evaluator_node.cpp + src/metrics/deviation_metrics.cpp + src/metrics/detection_count.cpp + src/utils/marker_utils.cpp + src/utils/objects_filtering.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::perception_diagnostics::PerceptionOnlineEvaluatorNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +target_link_libraries(${PROJECT_NAME} glog::glog) + +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_perception_online_evaluator_node + test/test_perception_online_evaluator_node.cpp + TIMEOUT 300 + ) + target_link_libraries(test_perception_online_evaluator_node + ${PROJECT_NAME} + ) +endif() + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/autoware_perception_online_evaluator/README.md similarity index 100% rename from evaluator/perception_online_evaluator/README.md rename to evaluator/autoware_perception_online_evaluator/README.md diff --git a/evaluator/perception_online_evaluator/images/detection_counts.drawio.svg b/evaluator/autoware_perception_online_evaluator/images/detection_counts.drawio.svg similarity index 100% rename from evaluator/perception_online_evaluator/images/detection_counts.drawio.svg rename to evaluator/autoware_perception_online_evaluator/images/detection_counts.drawio.svg diff --git a/evaluator/perception_online_evaluator/images/lateral_deviation.drawio.svg b/evaluator/autoware_perception_online_evaluator/images/lateral_deviation.drawio.svg similarity index 100% rename from evaluator/perception_online_evaluator/images/lateral_deviation.drawio.svg rename to evaluator/autoware_perception_online_evaluator/images/lateral_deviation.drawio.svg diff --git a/evaluator/perception_online_evaluator/images/path_deviation.drawio.svg b/evaluator/autoware_perception_online_evaluator/images/path_deviation.drawio.svg similarity index 100% rename from evaluator/perception_online_evaluator/images/path_deviation.drawio.svg rename to evaluator/autoware_perception_online_evaluator/images/path_deviation.drawio.svg diff --git a/evaluator/perception_online_evaluator/images/path_deviation_each_object.drawio.svg b/evaluator/autoware_perception_online_evaluator/images/path_deviation_each_object.drawio.svg similarity index 100% rename from evaluator/perception_online_evaluator/images/path_deviation_each_object.drawio.svg rename to evaluator/autoware_perception_online_evaluator/images/path_deviation_each_object.drawio.svg diff --git a/evaluator/perception_online_evaluator/images/yaw_deviation.drawio.svg b/evaluator/autoware_perception_online_evaluator/images/yaw_deviation.drawio.svg similarity index 100% rename from evaluator/perception_online_evaluator/images/yaw_deviation.drawio.svg rename to evaluator/autoware_perception_online_evaluator/images/yaw_deviation.drawio.svg diff --git a/evaluator/perception_online_evaluator/images/yaw_rate.drawio.svg b/evaluator/autoware_perception_online_evaluator/images/yaw_rate.drawio.svg similarity index 100% rename from evaluator/perception_online_evaluator/images/yaw_rate.drawio.svg rename to evaluator/autoware_perception_online_evaluator/images/yaw_rate.drawio.svg diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/detection_count.hpp similarity index 92% rename from evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp rename to evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/detection_count.hpp index c111a725a2ea0..33a4353fc5c4a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/detection_count.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_ +#ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_ +#define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_ -#include "perception_online_evaluator/parameters.hpp" +#include "autoware/perception_online_evaluator/parameters.hpp" #include "tf2_ros/buffer.h" #include @@ -32,7 +32,7 @@ #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { namespace metrics { @@ -143,6 +143,6 @@ class DetectionCounter seen_uuids_; }; } // namespace metrics -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics -#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_ +#endif // AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__DETECTION_COUNT_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/deviation_metrics.hpp similarity index 81% rename from evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp rename to evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/deviation_metrics.hpp index 0f3379b3f055e..90256ce18110f 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/deviation_metrics.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ #include #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { namespace metrics { @@ -44,6 +44,6 @@ double calcLateralDeviation(const std::vector & ref_path, const Pose & tar double calcYawDeviation(const std::vector & ref_path, const Pose & target_pose); } // namespace metrics -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics -#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ +#endif // AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__DEVIATION_METRICS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/metric.hpp similarity index 90% rename from evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp rename to evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/metric.hpp index 4caf932919e61..c190af8162ebf 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics/metric.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ +#ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ +#define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ #include "autoware/universe_utils/math/accumulator.hpp" @@ -23,7 +23,7 @@ #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { /** * @brief Enumeration of trajectory metrics @@ -90,6 +90,6 @@ static struct CheckCorrectMaps } check; } // namespace details -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics -#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ +#endif // AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS__METRIC_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics_calculator.hpp similarity index 89% rename from evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp rename to evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics_calculator.hpp index c17051d2a30a7..b59f0a6c32b3d 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/metrics_calculator.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ - -#include "perception_online_evaluator/metrics/detection_count.hpp" -#include "perception_online_evaluator/metrics/deviation_metrics.hpp" -#include "perception_online_evaluator/metrics/metric.hpp" -#include "perception_online_evaluator/parameters.hpp" -#include "perception_online_evaluator/utils/objects_filtering.hpp" +#ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ +#define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ + +#include "autoware/perception_online_evaluator/metrics/detection_count.hpp" +#include "autoware/perception_online_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/perception_online_evaluator/metrics/metric.hpp" +#include "autoware/perception_online_evaluator/parameters.hpp" +#include "autoware/perception_online_evaluator/utils/objects_filtering.hpp" #include "tf2_ros/buffer.h" #include @@ -38,7 +38,7 @@ #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -160,6 +160,6 @@ class MetricsCalculator }; // class MetricsCalculator -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics -#endif // PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ +#endif // AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__METRICS_CALCULATOR_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/parameters.hpp similarity index 84% rename from evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp rename to evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/parameters.hpp index b4cf23b85effb..5c53332b0a003 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/parameters.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/parameters.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ +#ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ +#define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ -#include "perception_online_evaluator/metrics/metric.hpp" +#include "autoware/perception_online_evaluator/metrics/metric.hpp" #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { /** * @brief Enumeration of perception metrics @@ -64,6 +64,6 @@ struct Parameters std::unordered_map object_parameters; }; -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics -#endif // PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ +#endif // AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__PARAMETERS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/perception_online_evaluator_node.hpp similarity index 87% rename from evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp rename to evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/perception_online_evaluator_node.hpp index 61c1ba40134df..62e4d14af63e0 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#include "autoware/perception_online_evaluator/metrics_calculator.hpp" +#include "autoware/perception_online_evaluator/parameters.hpp" #include "autoware/universe_utils/math/accumulator.hpp" -#include "perception_online_evaluator/metrics_calculator.hpp" -#include "perception_online_evaluator/parameters.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -34,7 +34,7 @@ #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { using autoware::universe_utils::Accumulator; using autoware_perception_msgs::msg::ObjectClassification; @@ -108,6 +108,6 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node // Debug void publishDebugMarker(); }; -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics -#endif // PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ +#endif // AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__PERCEPTION_ONLINE_EVALUATOR_NODE_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/marker_utils.hpp similarity index 89% rename from evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp rename to evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/marker_utils.hpp index 0ad0872723d55..ad849acd23956 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/marker_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ #include @@ -27,7 +27,7 @@ #include #include -namespace marker_utils +namespace autoware::perception_diagnostics::marker_utils { using autoware::universe_utils::Polygon2d; @@ -77,6 +77,6 @@ MarkerArray createDeviationLines( const std::vector & poses1, const std::vector & poses2, const std::string & ns, const int32_t & first_id, const float r, const float g, const float b); -} // namespace marker_utils +} // namespace autoware::perception_diagnostics::marker_utils -#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ +#endif // AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/objects_filtering.hpp similarity index 94% rename from evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp rename to evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/objects_filtering.hpp index 7adab46f42d2f..4220284b243f9 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp +++ b/evaluator/autoware_perception_online_evaluator/include/autoware/perception_online_evaluator/utils/objects_filtering.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ -#define PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#ifndef AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#define AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ -#include "perception_online_evaluator/parameters.hpp" +#include "autoware/perception_online_evaluator/parameters.hpp" #include #include @@ -31,7 +31,7 @@ * most of this file is copied from objects_filtering.hpp in safety_check of behavior_path_planner */ -namespace perception_diagnostics +namespace autoware::perception_diagnostics { using autoware_perception_msgs::msg::ObjectClassification; @@ -170,6 +170,6 @@ PredictedObjects filterObjectsByVelocity( PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, double velocity_threshold, double max_velocity); -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics -#endif // PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ +#endif // AUTOWARE__PERCEPTION_ONLINE_EVALUATOR__UTILS__OBJECTS_FILTERING_HPP_ diff --git a/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml b/evaluator/autoware_perception_online_evaluator/launch/perception_online_evaluator.launch.xml similarity index 63% rename from evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml rename to evaluator/autoware_perception_online_evaluator/launch/perception_online_evaluator.launch.xml index fc045da0302b7..5fedb030c62eb 100644 --- a/evaluator/perception_online_evaluator/launch/perception_online_evaluator.launch.xml +++ b/evaluator/autoware_perception_online_evaluator/launch/perception_online_evaluator.launch.xml @@ -3,8 +3,8 @@ - - + + diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/autoware_perception_online_evaluator/package.xml similarity index 91% rename from evaluator/perception_online_evaluator/package.xml rename to evaluator/autoware_perception_online_evaluator/package.xml index 94575a0adbb4c..b8e50f6bf593c 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/autoware_perception_online_evaluator/package.xml @@ -1,8 +1,8 @@ - perception_online_evaluator - 0.40.0 + autoware_perception_online_evaluator + 0.41.0 ROS 2 node for evaluating perception Fumiya Watanabe Kosuke Takeuchi @@ -10,6 +10,7 @@ Kyoichi Sugahara Shunsuke Miura Yoshi Ri + Junya Sasaki Apache License 2.0 Kosuke Takeuchi diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/autoware_perception_online_evaluator/param/perception_online_evaluator.defaults.yaml similarity index 100% rename from evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml rename to evaluator/autoware_perception_online_evaluator/param/perception_online_evaluator.defaults.yaml diff --git a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp b/evaluator/autoware_perception_online_evaluator/src/metrics/detection_count.cpp similarity index 94% rename from evaluator/perception_online_evaluator/src/metrics/detection_count.cpp rename to evaluator/autoware_perception_online_evaluator/src/metrics/detection_count.cpp index 4a1d97aeeb90b..3245a80dd7ff5 100644 --- a/evaluator/perception_online_evaluator/src/metrics/detection_count.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/metrics/detection_count.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_online_evaluator/metrics/detection_count.hpp" +#include "autoware/perception_online_evaluator/metrics/detection_count.hpp" #include "autoware/object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/perception_online_evaluator/utils/objects_filtering.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" -#include "perception_online_evaluator/utils/objects_filtering.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { namespace metrics { @@ -110,10 +110,9 @@ void DetectionCounter::initializeDetectionMap() ++i) { for (const auto & range : getRanges()) { std::string range_str = range.toString(); - if (time_series_counts_.find(i) == time_series_counts_.end()) { - time_series_counts_[i][range_str] = std::vector(); - seen_uuids_[i][range_str] = std::set(); - } else if (time_series_counts_[i].find(range_str) == time_series_counts_[i].end()) { + if ( + time_series_counts_[i].find(range_str) == time_series_counts_[i].end() || + time_series_counts_.find(i) == time_series_counts_.end()) { time_series_counts_[i][range_str] = std::vector(); seen_uuids_[i][range_str] = std::set(); } @@ -237,4 +236,4 @@ void DetectionCounter::purgeOldRecords(rclcpp::Time current_time) } } } // namespace metrics -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics diff --git a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_perception_online_evaluator/src/metrics/deviation_metrics.cpp similarity index 90% rename from evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp rename to evaluator/autoware_perception_online_evaluator/src/metrics/deviation_metrics.cpp index 292fc9cd65a17..9aa8632ec7f51 100644 --- a/evaluator/perception_online_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/metrics/deviation_metrics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_online_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/perception_online_evaluator/metrics/deviation_metrics.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/geometry/pose_deviation.hpp" @@ -21,7 +21,7 @@ #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { namespace metrics { @@ -50,4 +50,4 @@ double calcYawDeviation(const std::vector & ref_path, const Pose & target_ } } // namespace metrics -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_perception_online_evaluator/src/metrics_calculator.cpp similarity index 99% rename from evaluator/perception_online_evaluator/src/metrics_calculator.cpp rename to evaluator/autoware_perception_online_evaluator/src/metrics_calculator.cpp index 0ced0b9d6c8a8..0243e0c7edbbb 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/metrics_calculator.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_online_evaluator/metrics_calculator.hpp" +#include "autoware/perception_online_evaluator/metrics_calculator.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/object_recognition_utils/object_classification.hpp" @@ -21,7 +21,7 @@ #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { using autoware::object_recognition_utils::convertLabelToString; using autoware::universe_utils::inverseTransformPoint; @@ -686,4 +686,4 @@ std::vector MetricsCalculator::averageFilterPath( return filtered_path; } -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/autoware_perception_online_evaluator/src/perception_online_evaluator_node.cpp similarity index 97% rename from evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp rename to evaluator/autoware_perception_online_evaluator/src/perception_online_evaluator_node.cpp index 7d8344c24819c..4a02745b6ad99 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_online_evaluator/perception_online_evaluator_node.hpp" +#include "autoware/perception_online_evaluator/perception_online_evaluator_node.hpp" +#include "autoware/perception_online_evaluator/utils/marker_utils.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" #include "autoware/universe_utils/ros/parameter.hpp" #include "autoware/universe_utils/ros/update_param.hpp" -#include "perception_online_evaluator/utils/marker_utils.hpp" #include @@ -33,7 +33,7 @@ #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { PerceptionOnlineEvaluatorNode::PerceptionOnlineEvaluatorNode( const rclcpp::NodeOptions & node_options) @@ -382,7 +382,7 @@ void PerceptionOnlineEvaluatorNode::initParameter() getOrDeclareParameter(*this, ns + "object_polygon"); } } -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(perception_diagnostics::PerceptionOnlineEvaluatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::perception_diagnostics::PerceptionOnlineEvaluatorNode) diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/autoware_perception_online_evaluator/src/utils/marker_utils.cpp similarity index 97% rename from evaluator/perception_online_evaluator/src/utils/marker_utils.cpp rename to evaluator/autoware_perception_online_evaluator/src/utils/marker_utils.cpp index 7489447ccb47e..154ca23e9c0cf 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/utils/marker_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_online_evaluator/utils/marker_utils.hpp" +#include "autoware/perception_online_evaluator/utils/marker_utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" @@ -29,7 +29,7 @@ #include #include -namespace marker_utils +namespace autoware::perception_diagnostics::marker_utils { using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::createDefaultMarker; @@ -197,4 +197,4 @@ MarkerArray createObjectPolygonMarkerArray( return msg; } -} // namespace marker_utils +} // namespace autoware::perception_diagnostics::marker_utils diff --git a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp b/evaluator/autoware_perception_online_evaluator/src/utils/objects_filtering.cpp similarity index 95% rename from evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp rename to evaluator/autoware_perception_online_evaluator/src/utils/objects_filtering.cpp index 13bd820f18cec..10ad8915da061 100644 --- a/evaluator/perception_online_evaluator/src/utils/objects_filtering.cpp +++ b/evaluator/autoware_perception_online_evaluator/src/utils/objects_filtering.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "perception_online_evaluator/utils/objects_filtering.hpp" +#include "autoware/perception_online_evaluator/utils/objects_filtering.hpp" #include #include -namespace perception_diagnostics +namespace autoware::perception_diagnostics { namespace filter { @@ -102,4 +102,4 @@ PredictedObjects filterObjectsByVelocity( filterObjects(filtered, filter); return filtered; } -} // namespace perception_diagnostics +} // namespace autoware::perception_diagnostics diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/autoware_perception_online_evaluator/test/test_perception_online_evaluator_node.cpp similarity index 99% rename from evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp rename to evaluator/autoware_perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index ffefee835f933..9faf680767db8 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/autoware_perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -16,8 +16,8 @@ #include "rclcpp/time.hpp" #include +#include #include -#include #include #include @@ -36,7 +36,7 @@ #include #include -using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode; +using EvalNode = autoware::perception_diagnostics::PerceptionOnlineEvaluatorNode; using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; using PredictedObject = autoware_perception_msgs::msg::PredictedObject; using MarkerArray = visualization_msgs::msg::MarkerArray; @@ -57,7 +57,7 @@ class EvalTest : public ::testing::Test rclcpp::NodeOptions options; const auto share_dir = - ament_index_cpp::get_package_share_directory("perception_online_evaluator"); + ament_index_cpp::get_package_share_directory("autoware_perception_online_evaluator"); options.arguments( {"--ros-args", "--params-file", share_dir + "/param/perception_online_evaluator.defaults.yaml"}); @@ -126,9 +126,9 @@ class EvalTest : public ::testing::Test tf_pub_->publish(tf_msg); } - void setTargetMetric(const perception_diagnostics::Metric & metric) + void setTargetMetric(const autoware::perception_diagnostics::Metric & metric) { - const auto metric_str = perception_diagnostics::metric_to_str.at(metric); + const auto metric_str = autoware::perception_diagnostics::metric_to_str.at(metric); setTargetMetric(metric_str); } diff --git a/evaluator/autoware_planning_evaluator/CHANGELOG.rst b/evaluator/autoware_planning_evaluator/CHANGELOG.rst index 77f544ae078fa..198c6216a9416 100644 --- a/evaluator/autoware_planning_evaluator/CHANGELOG.rst +++ b/evaluator/autoware_planning_evaluator/CHANGELOG.rst @@ -2,6 +2,38 @@ Changelog for package autoware_planning_evaluator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9859 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files evaluator/autoware_planning_evaluator +* fix(planning_evaluator): update lateral_trajectory_displacement to absolute value (`#9696 `_) + * fix(planning_evaluator): update lateral_trajectory_displacement to absolute value + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* fix(autoware_planning_evaluator): rename lateral deviation metrics (`#9801 `_) + * refactor(planning_evaluator): rename and add lateral trajectory displacement metrics + * fix typo + --------- +* feat(planning_evaluator): add evaluation feature of trajectory lateral displacement (`#9718 `_) + * feat(planning_evaluator): add evaluation feature of trajectory lateral displacement + * feat(metrics_calculator): implement lookahead trajectory calculation and remove deprecated method + * fix(planning_evaluator): rename lateral_trajectory_displacement to trajectory_lateral_displacement for consistency + --------- +* fix(autoware_planning_evaluator): fix bugprone-exception-escape (`#9730 `_) + fix: bugprone-exception-escape +* feat(planning_evaluator): add lateral trajectory displacement metrics (`#9674 `_) + * feat(planning_evaluator): add nearest pose deviation msg + * update comment contents + * update variable name + * Revert "update variable name" + This reverts commit ee427222fcbd2a18ffbc20fecca3ad557f527e37. + * move lateral_trajectory_displacement position + * prev.dist -> prev_lateral_deviation + --------- +* Contributors: Fumiya Watanabe, Kazunori-Nakajima, Kyoichi Sugahara, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/evaluator/autoware_planning_evaluator/README.md b/evaluator/autoware_planning_evaluator/README.md index 4fcdf4d7e55fd..b9dd3201a2541 100644 --- a/evaluator/autoware_planning_evaluator/README.md +++ b/evaluator/autoware_planning_evaluator/README.md @@ -13,6 +13,7 @@ Metrics are calculated using the following information: - the previous trajectory `T(-1)`. - the _reference_ trajectory assumed to be used as the reference to plan `T(0)`. - the current ego pose. +- the current ego odometry. - the set of objects in the environment. These information are maintained by an instance of class `MetricsCalculator` diff --git a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml index 14c1bcc6beea4..7605ed2a5e859 100644 --- a/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml +++ b/evaluator/autoware_planning_evaluator/config/planning_evaluator.param.yaml @@ -14,6 +14,8 @@ - lateral_deviation - yaw_deviation - velocity_deviation + - lateral_trajectory_displacement_local + - lateral_trajectory_displacement_lookahead - stability - stability_frechet - obstacle_distance @@ -24,6 +26,7 @@ trajectory: min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation + evaluation_time_s: 5.0 # [s] time duration for trajectory evaluation in seconds lookahead: max_dist_m: 5.0 # [m] maximum distance from ego along the trajectory to use for calculation max_time_s: 3.0 # [s] maximum time ahead of ego along the trajectory to use for calculation diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp index 59866c96ad702..2341ad2bb6ba3 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/deviation_metrics.hpp @@ -38,6 +38,16 @@ using geometry_msgs::msg::Pose; */ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajectory & traj); +/** + * @brief calculate lateral trajectory displacement from the previous trajectory and the trajectory + * @param [in] prev previous trajectory + * @param [in] traj input trajectory + * @param [in] base_pose base pose + * @return calculated statistics + */ +Accumulator calcLocalLateralTrajectoryDisplacement( + const Trajectory & prev, const Trajectory & traj, const Pose & base_pose); + /** * @brief calculate yaw deviation of the given trajectory from the reference trajectory * @param [in] ref reference trajectory diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp index c0313ed0727dd..7c207bf6c8f57 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metric.hpp @@ -37,6 +37,8 @@ enum class Metric { lateral_deviation, yaw_deviation, velocity_deviation, + lateral_trajectory_displacement_local, + lateral_trajectory_displacement_lookahead, stability, stability_frechet, obstacle_distance, @@ -62,6 +64,8 @@ static const std::unordered_map str_to_metric = { {"lateral_deviation", Metric::lateral_deviation}, {"yaw_deviation", Metric::yaw_deviation}, {"velocity_deviation", Metric::velocity_deviation}, + {"lateral_trajectory_displacement_local", Metric::lateral_trajectory_displacement_local}, + {"lateral_trajectory_displacement_lookahead", Metric::lateral_trajectory_displacement_lookahead}, {"stability", Metric::stability}, {"stability_frechet", Metric::stability_frechet}, {"obstacle_distance", Metric::obstacle_distance}, @@ -82,6 +86,8 @@ static const std::unordered_map metric_to_str = { {Metric::lateral_deviation, "lateral_deviation"}, {Metric::yaw_deviation, "yaw_deviation"}, {Metric::velocity_deviation, "velocity_deviation"}, + {Metric::lateral_trajectory_displacement_local, "lateral_trajectory_displacement_local"}, + {Metric::lateral_trajectory_displacement_lookahead, "lateral_trajectory_displacement_lookahead"}, {Metric::stability, "stability"}, {Metric::stability_frechet, "stability_frechet"}, {Metric::obstacle_distance, "obstacle_distance"}, @@ -103,6 +109,8 @@ static const std::unordered_map metric_descriptions = { {Metric::lateral_deviation, "Lateral_deviation[m]"}, {Metric::yaw_deviation, "Yaw_deviation[rad]"}, {Metric::velocity_deviation, "Velocity_deviation[m/s]"}, + {Metric::lateral_trajectory_displacement_local, "Nearest Pose Lateral Deviation[m]"}, + {Metric::lateral_trajectory_displacement_lookahead, "Lateral_Offset_Over_Distance_Ahead[m]"}, {Metric::stability, "Stability[m]"}, {Metric::stability_frechet, "StabilityFrechet[m]"}, {Metric::obstacle_distance, "Obstacle_distance[m]"}, diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp index 0006e49c3338a..9f2b99007af7d 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/metrics_utils.hpp @@ -25,6 +25,7 @@ namespace metrics namespace utils { using autoware_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Pose; /** * @brief find the index in the trajectory at the given distance of the given index @@ -35,6 +36,29 @@ using autoware_planning_msgs::msg::Trajectory; */ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance); +/** + * @brief trim a trajectory from the current ego pose to some fixed time or distance + * @param [in] traj input trajectory to trim + * @param [in] max_dist_m [m] maximum distance ahead of the ego pose + * @param [in] max_time_s [s] maximum time ahead of the ego pose + * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum + * duration max_time_s + */ +Trajectory get_lookahead_trajectory( + const Trajectory & traj, const Pose & ego_pose, const double max_dist_m, const double max_time_s); + +/** + * @brief calculate the total distance from ego position to the end of trajectory + * @details finds the nearest point to ego position on the trajectory and calculates + * the cumulative distance by summing up the distances between consecutive points + * from that position to the end of the trajectory. + * + * @param [in] traj input trajectory to calculate distance along + * @param [in] ego_pose current ego vehicle pose + * @return total distance from ego position to trajectory end in meters + */ +double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose); + } // namespace utils } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp index 38f388feeadda..1b46fbddfb297 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics/stability_metrics.hpp @@ -18,6 +18,7 @@ #include "autoware/universe_utils/math/accumulator.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" +#include namespace planning_diagnostics { @@ -42,6 +43,23 @@ Accumulator calcFrechetDistance(const Trajectory & traj1, const Trajecto */ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajectory & traj2); +/** + * @brief calculate the total lateral displacement between two trajectories + * @details Evaluates the cumulative absolute lateral displacement by sampling points + * along the first trajectory and measuring their offset from the second trajectory. + * The evaluation section length is determined by the ego vehicle's velocity and + * the specified evaluation time. + * + * @param traj1 first trajectory to compare + * @param traj2 second trajectory to compare against + * @param [in] ego_odom current ego vehicle odometry containing pose and velocity + * @param [in] trajectory_eval_time_s time duration for trajectory evaluation in seconds + * @return statistical accumulator containing the total lateral displacement + */ +Accumulator calcLookaheadLateralTrajectoryDisplacement( + const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, + const double trajectory_eval_time_s); + } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp index 97d23cad10af2..fe9d9eaf4278b 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/metrics_calculator.hpp @@ -23,6 +23,7 @@ #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" +#include #include @@ -74,7 +75,7 @@ class MetricsCalculator * @brief set the ego pose * @param [in] traj input previous trajectory */ - void setEgoPose(const geometry_msgs::msg::Pose & pose); + void setEgoPose(const nav_msgs::msg::Odometry & ego_odometry); /** * @brief get the ego pose @@ -83,23 +84,13 @@ class MetricsCalculator Pose getEgoPose(); private: - /** - * @brief trim a trajectory from the current ego pose to some fixed time or distance - * @param [in] traj input trajectory to trim - * @param [in] max_dist_m [m] maximum distance ahead of the ego pose - * @param [in] max_time_s [s] maximum time ahead of the ego pose - * @return sub-trajectory starting from the ego pose and of maximum length max_dist_m, maximum - * duration max_time_s - */ - Trajectory getLookaheadTrajectory( - const Trajectory & traj, const double max_dist_m, const double max_time_s) const; - Trajectory reference_trajectory_; Trajectory reference_trajectory_lookahead_; Trajectory previous_trajectory_; Trajectory previous_trajectory_lookahead_; PredictedObjects dynamic_objects_; geometry_msgs::msg::Pose ego_pose_; + nav_msgs::msg::Odometry ego_odometry_; PoseWithUuidStamped modified_goal_; }; // class MetricsCalculator diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp index cd894fecc2331..94920195ee89c 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/parameters.hpp @@ -31,6 +31,7 @@ struct Parameters struct { double min_point_dist_m = 0.1; + double evaluation_time_s = 5.0; struct { double max_dist_m = 5.0; diff --git a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp index 9c268206846d9..8c0b49ce2dd26 100644 --- a/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/autoware_planning_evaluator/include/autoware/planning_evaluator/planning_evaluator_node.hpp @@ -31,8 +31,8 @@ #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include #include -#include #include #include @@ -145,7 +145,8 @@ class PlanningEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ this, "~/input/acceleration"}; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; diff --git a/evaluator/autoware_planning_evaluator/package.xml b/evaluator/autoware_planning_evaluator/package.xml index 4389b2e52f44d..22c3b93035e9d 100644 --- a/evaluator/autoware_planning_evaluator/package.xml +++ b/evaluator/autoware_planning_evaluator/package.xml @@ -2,7 +2,7 @@ autoware_planning_evaluator - 0.40.0 + 0.41.0 ROS 2 node for evaluating planners Maxime CLEMENT Kyoichi Sugahara diff --git a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json index 1ef3874a0dcbc..263bd374a7e45 100644 --- a/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json +++ b/evaluator/autoware_planning_evaluator/schema/autoware_planning_evaluator.schema.json @@ -34,6 +34,7 @@ "lateral_deviation", "yaw_deviation", "velocity_deviation", + "lateral_trajectory_displacement", "stability", "stability_frechet", "obstacle_distance", diff --git a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp index 7e2217a49ef87..9b5959948f8cb 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/deviation_metrics.cpp @@ -45,6 +45,25 @@ Accumulator calcLateralDeviation(const Trajectory & ref, const Trajector return stat; } +Accumulator calcLocalLateralTrajectoryDisplacement( + const Trajectory & prev, const Trajectory & traj, const Pose & ego_pose) +{ + Accumulator stat; + + if (prev.points.empty() || traj.points.empty()) { + return stat; + } + + const auto prev_lateral_deviation = + autoware::motion_utils::calcLateralOffset(prev.points, ego_pose.position); + const auto traj_lateral_deviation = + autoware::motion_utils::calcLateralOffset(traj.points, ego_pose.position); + const auto lateral_trajectory_displacement = + std::abs(traj_lateral_deviation - prev_lateral_deviation); + stat.add(lateral_trajectory_displacement); + return stat; +} + Accumulator calcYawDeviation(const Trajectory & ref, const Trajectory & traj) { Accumulator stat; diff --git a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp index 669afdd7229b0..7451264f037a6 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/metrics_utils.cpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" + namespace planning_diagnostics { namespace metrics @@ -23,6 +25,7 @@ namespace utils using autoware::universe_utils::calcDistance2d; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Pose; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) { @@ -41,6 +44,52 @@ size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, cons return target_id; } +Trajectory get_lookahead_trajectory( + const Trajectory & traj, const Pose & ego_pose, const double max_dist_m, const double max_time_s) +{ + if (traj.points.empty()) { + return traj; + } + + const auto ego_index = + autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose.position); + Trajectory lookahead_traj; + lookahead_traj.header = traj.header; + double dist = 0.0; + double time = 0.0; + auto curr_point_it = std::next(traj.points.begin(), ego_index); + auto prev_point_it = curr_point_it; + while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { + lookahead_traj.points.push_back(*curr_point_it); + const auto d = autoware::universe_utils::calcDistance2d( + prev_point_it->pose.position, curr_point_it->pose.position); + dist += d; + if (prev_point_it->longitudinal_velocity_mps != 0.0) { + time += d / std::abs(prev_point_it->longitudinal_velocity_mps); + } + prev_point_it = curr_point_it; + ++curr_point_it; + } + return lookahead_traj; +} + +double calc_lookahead_trajectory_distance(const Trajectory & traj, const Pose & ego_pose) +{ + const auto ego_index = + autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose.position); + double dist = 0.0; + auto curr_point_it = std::next(traj.points.begin(), ego_index); + auto prev_point_it = curr_point_it; + for (size_t i = 0; i < traj.points.size(); ++i) { + const auto d = autoware::universe_utils::calcDistance2d( + prev_point_it->pose.position, curr_point_it->pose.position); + dist += d; + prev_point_it = curr_point_it; + ++curr_point_it; + } + + return dist; +} } // namespace utils } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp index e6ede0d07b9b3..61e18a6ad0b63 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics/stability_metrics.cpp @@ -14,7 +14,9 @@ #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" +#include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/planning_evaluator/metrics/metrics_utils.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -96,5 +98,47 @@ Accumulator calcLateralDistance(const Trajectory & traj1, const Trajecto return stat; } +Accumulator calcLookaheadLateralTrajectoryDisplacement( + const Trajectory traj1, const Trajectory traj2, const nav_msgs::msg::Odometry & ego_odom, + const double trajectory_eval_time_s) +{ + Accumulator stat; + + if (traj1.points.empty() || traj2.points.empty()) { + return stat; + } + + const double ego_velocity = + std::hypot(ego_odom.twist.twist.linear.x, ego_odom.twist.twist.linear.y); + + const double evaluation_section_length = trajectory_eval_time_s * std::abs(ego_velocity); + + const double traj1_lookahead_distance = + utils::calc_lookahead_trajectory_distance(traj1, ego_odom.pose.pose); + const double traj2_lookahead_distance = + utils::calc_lookahead_trajectory_distance(traj2, ego_odom.pose.pose); + + if ( + traj1_lookahead_distance < evaluation_section_length || + traj2_lookahead_distance < evaluation_section_length) { + return stat; + } + + constexpr double num_evaluation_points = 10.0; + const double interval = evaluation_section_length / num_evaluation_points; + + const auto resampled_traj1 = autoware::motion_utils::resampleTrajectory( + utils::get_lookahead_trajectory( + traj1, ego_odom.pose.pose, evaluation_section_length, trajectory_eval_time_s), + interval); + + for (const auto & point : resampled_traj1.points) { + const auto p0 = autoware::universe_utils::getPoint(point); + const double dist = autoware::motion_utils::calcLateralOffset(traj2.points, p0); + stat.add(std::abs(dist)); + } + return stat; +} + } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp index 6e057bcc9fc3d..c30420a5632fa 100644 --- a/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/autoware_planning_evaluator/src/metrics_calculator.cpp @@ -16,6 +16,7 @@ #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/planning_evaluator/metrics/deviation_metrics.hpp" +#include "autoware/planning_evaluator/metrics/metrics_utils.hpp" #include "autoware/planning_evaluator/metrics/obstacle_metrics.hpp" #include "autoware/planning_evaluator/metrics/stability_metrics.hpp" #include "autoware/planning_evaluator/metrics/trajectory_metrics.hpp" @@ -49,21 +50,26 @@ std::optional> MetricsCalculator::calculate( return metrics::calcYawDeviation(reference_trajectory_, traj); case Metric::velocity_deviation: return metrics::calcVelocityDeviation(reference_trajectory_, traj); + case Metric::lateral_trajectory_displacement_local: + return metrics::calcLocalLateralTrajectoryDisplacement(previous_trajectory_, traj, ego_pose_); + case Metric::lateral_trajectory_displacement_lookahead: + return metrics::calcLookaheadLateralTrajectoryDisplacement( + previous_trajectory_, traj, ego_odometry_, parameters.trajectory.evaluation_time_s); case Metric::stability_frechet: return metrics::calcFrechetDistance( - getLookaheadTrajectory( - previous_trajectory_, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + previous_trajectory_, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s), - getLookaheadTrajectory( - traj, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); case Metric::stability: return metrics::calcLateralDistance( - getLookaheadTrajectory( - previous_trajectory_, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + previous_trajectory_, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s), - getLookaheadTrajectory( - traj, parameters.trajectory.lookahead.max_dist_m, + metrics::utils::get_lookahead_trajectory( + traj, ego_pose_, parameters.trajectory.lookahead.max_dist_m, parameters.trajectory.lookahead.max_time_s)); case Metric::obstacle_distance: return metrics::calcDistanceToObstacle(dynamic_objects_, traj); @@ -105,9 +111,10 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) dynamic_objects_ = objects; } -void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) +void MetricsCalculator::setEgoPose(const nav_msgs::msg::Odometry & ego_odometry) { - ego_pose_ = pose; + ego_pose_ = ego_odometry.pose.pose; + ego_odometry_ = ego_odometry; } Pose MetricsCalculator::getEgoPose() @@ -115,33 +122,4 @@ Pose MetricsCalculator::getEgoPose() return ego_pose_; } -Trajectory MetricsCalculator::getLookaheadTrajectory( - const Trajectory & traj, const double max_dist_m, const double max_time_s) const -{ - if (traj.points.empty()) { - return traj; - } - - const auto ego_index = - autoware::motion_utils::findNearestSegmentIndex(traj.points, ego_pose_.position); - Trajectory lookahead_traj; - lookahead_traj.header = traj.header; - double dist = 0.0; - double time = 0.0; - auto curr_point_it = std::next(traj.points.begin(), ego_index); - auto prev_point_it = curr_point_it; - while (curr_point_it != traj.points.end() && dist <= max_dist_m && time <= max_time_s) { - lookahead_traj.points.push_back(*curr_point_it); - const auto d = autoware::universe_utils::calcDistance2d( - prev_point_it->pose.position, curr_point_it->pose.position); - dist += d; - if (prev_point_it->longitudinal_velocity_mps != 0.0) { - time += d / std::abs(prev_point_it->longitudinal_velocity_mps); - } - prev_point_it = curr_point_it; - ++curr_point_it; - } - return lookahead_traj; -} - } // namespace planning_diagnostics diff --git a/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp index ac6f35179f771..d43aed1ec5687 100644 --- a/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/motion_evaluator_node.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -50,47 +51,52 @@ MotionEvaluatorNode::~MotionEvaluatorNode() if (!output_metrics_) { return; } - - // generate json data - using json = nlohmann::json; - json j; - for (Metric metric : metrics_) { - const std::string base_name = metric_to_str.at(metric) + "/"; - const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); - if (stat) { - j[base_name + "min"] = stat->min(); - j[base_name + "max"] = stat->max(); - j[base_name + "mean"] = stat->mean(); + try { + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); + if (stat) { + j[base_name + "min"] = stat->min(); + j[base_name + "max"] = stat->max(); + j[base_name + "mean"] = stat->mean(); + } } - } - // get output folder - const std::string output_folder_str = - rclcpp::get_logging_directory().string() + "/autoware_metrics"; - if (!std::filesystem::exists(output_folder_str)) { - if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); - return; + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } } - } - // get time stamp - std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm * local_time = std::localtime(&now_time_t); - std::ostringstream oss; - oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); - std::string cur_time_str = oss.str(); - - // Write metrics .json to file - const std::string output_file_str = - output_folder_str + "/autoware_motion_evaluator-" + cur_time_str + ".json"; - std::ofstream f(output_file_str); - if (f.is_open()) { - f << j.dump(4); - f.close(); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_motion_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } + } catch (const std::exception & e) { + std::cerr << "Exception in MotionEvaluatorNode destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in MotionEvaluatorNode destructor" << std::endl; } } diff --git a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp index a61e56cd532ad..53319ffa4c1fd 100644 --- a/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/autoware_planning_evaluator/src/planning_evaluator_node.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -50,14 +51,16 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op declare_parameter("trajectory.lookahead.max_dist_m"); metrics_calculator_.parameters.trajectory.lookahead.max_time_s = declare_parameter("trajectory.lookahead.max_time_s"); + metrics_calculator_.parameters.trajectory.evaluation_time_s = + declare_parameter("trajectory.evaluation_time_s"); metrics_calculator_.parameters.obstacle.dist_thr_m = declare_parameter("obstacle.dist_thr_m"); output_metrics_ = declare_parameter("output_metrics"); ego_frame_str_ = declare_parameter("ego_frame"); - processing_time_pub_ = - this->create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = this->create_publisher( + "~/debug/processing_time_ms", 1); // List of metrics to calculate and publish metrics_pub_ = create_publisher("~/metrics", 1); @@ -74,45 +77,51 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode() return; } - // generate json data - using json = nlohmann::json; - json j; - for (Metric metric : metrics_) { - const std::string base_name = metric_to_str.at(metric) + "/"; - j[base_name + "min"] = metric_accumulators_[static_cast(metric)][0].min(); - j[base_name + "max"] = metric_accumulators_[static_cast(metric)][1].max(); - j[base_name + "mean"] = metric_accumulators_[static_cast(metric)][2].mean(); - j[base_name + "count"] = metric_accumulators_[static_cast(metric)][2].count(); - j[base_name + "description"] = metric_descriptions.at(metric); - } + try { + // generate json data + using json = nlohmann::json; + json j; + for (Metric metric : metrics_) { + const std::string base_name = metric_to_str.at(metric) + "/"; + j[base_name + "min"] = metric_accumulators_[static_cast(metric)][0].min(); + j[base_name + "max"] = metric_accumulators_[static_cast(metric)][1].max(); + j[base_name + "mean"] = metric_accumulators_[static_cast(metric)][2].mean(); + j[base_name + "count"] = metric_accumulators_[static_cast(metric)][2].count(); + j[base_name + "description"] = metric_descriptions.at(metric); + } - // get output folder - const std::string output_folder_str = - rclcpp::get_logging_directory().string() + "/autoware_metrics"; - if (!std::filesystem::exists(output_folder_str)) { - if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); - return; + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } } - } - // get time stamp - std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm * local_time = std::localtime(&now_time_t); - std::ostringstream oss; - oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); - std::string cur_time_str = oss.str(); - - // Write metrics .json to file - const std::string output_file_str = - output_folder_str + "/autoware_planning_evaluator-" + cur_time_str + ".json"; - std::ofstream f(output_file_str); - if (f.is_open()) { - f << j.dump(4); - f.close(); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_planning_evaluator-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } + } catch (const std::exception & e) { + std::cerr << "Exception in MotionEvaluatorNode destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in MotionEvaluatorNode destructor" << std::endl; } } @@ -273,7 +282,7 @@ void PlanningEvaluatorNode::onTimer() metrics_msg_ = MetricArrayMsg{}; // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); @@ -343,7 +352,7 @@ void PlanningEvaluatorNode::onModifiedGoal( void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) { if (!odometry_msg) return; - metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + metrics_calculator_.setEgoPose(*odometry_msg); { getRouteData(); if (route_handler_.isHandlerReady() && odometry_msg) { diff --git a/evaluator/scenario_simulator_v2_adapter/CHANGELOG.rst b/evaluator/autoware_scenario_simulator_v2_adapter/CHANGELOG.rst similarity index 95% rename from evaluator/scenario_simulator_v2_adapter/CHANGELOG.rst rename to evaluator/autoware_scenario_simulator_v2_adapter/CHANGELOG.rst index 7b583a3da7959..5bf589f24fb9a 100644 --- a/evaluator/scenario_simulator_v2_adapter/CHANGELOG.rst +++ b/evaluator/autoware_scenario_simulator_v2_adapter/CHANGELOG.rst @@ -1,7 +1,27 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package scenario_simulator_v2_adapter +Changelog for package autoware_scenario_simulator_v2_adapter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `scenario_simulator_v2_adapter` (`#9957 `_) + * feat(autoware_scenario_simulator_v2_adapter): apply `autoware\_` prefix (see below): + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(scenario_simulator_v2_adapter): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(scenario_simulator_v2_adapter): fix include paths + * To follow the previous commit + * rename: `scenario_simulator_v2_adapter` => `autoware_scenario_simulator_v2_adapter` + * bug(autoware_scenario_simulator_v2_adapter): revert wrongly updated copyrights + * bug(autoware_scenario_simulator_v2_adapter): `autoware\_` prefix is not needed here + * bug(autoware_scenario_simulator_v2_adapter): wrong package name in launch side + --------- +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/evaluator/scenario_simulator_v2_adapter/CMakeLists.txt b/evaluator/autoware_scenario_simulator_v2_adapter/CMakeLists.txt similarity index 60% rename from evaluator/scenario_simulator_v2_adapter/CMakeLists.txt rename to evaluator/autoware_scenario_simulator_v2_adapter/CMakeLists.txt index 576bd9682725a..7e45858383e71 100644 --- a/evaluator/scenario_simulator_v2_adapter/CMakeLists.txt +++ b/evaluator/autoware_scenario_simulator_v2_adapter/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.16.3) # Ubuntu 20.04 default CMake version -project(scenario_simulator_v2_adapter) +project(autoware_scenario_simulator_v2_adapter) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -16,24 +16,24 @@ find_package(pluginlib REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(${PROJECT_NAME}_node SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/converter_node.cpp ) -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "scenario_simulator_v2_adapter::MetricConverter" - EXECUTABLE ${PROJECT_NAME} +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::scenario_simulator_v2_adapter::MetricConverter" + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - ament_add_gtest(test_${PROJECT_NAME} + ament_add_gtest(test_scenario_simulator_v2_adapter test/test_converter_node.cpp ) - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME}_node + target_link_libraries(test_scenario_simulator_v2_adapter + ${PROJECT_NAME} ) endif() diff --git a/evaluator/scenario_simulator_v2_adapter/README.md b/evaluator/autoware_scenario_simulator_v2_adapter/README.md similarity index 100% rename from evaluator/scenario_simulator_v2_adapter/README.md rename to evaluator/autoware_scenario_simulator_v2_adapter/README.md diff --git a/evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml b/evaluator/autoware_scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml similarity index 100% rename from evaluator/scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml rename to evaluator/autoware_scenario_simulator_v2_adapter/config/scenario_simulator_v2_adapter.param.yaml diff --git a/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp b/evaluator/autoware_scenario_simulator_v2_adapter/include/autoware/scenario_simulator_v2_adapter/converter_node.hpp similarity index 84% rename from evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp rename to evaluator/autoware_scenario_simulator_v2_adapter/include/autoware/scenario_simulator_v2_adapter/converter_node.hpp index 1ac4585035277..a8afc6dcd9e21 100644 --- a/evaluator/scenario_simulator_v2_adapter/include/scenario_simulator_v2_adapter/converter_node.hpp +++ b/evaluator/autoware_scenario_simulator_v2_adapter/include/autoware/scenario_simulator_v2_adapter/converter_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ -#define SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ +#ifndef AUTOWARE__SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ +#define AUTOWARE__SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ #include @@ -27,7 +27,7 @@ #include #include -namespace scenario_simulator_v2_adapter +namespace autoware::scenario_simulator_v2_adapter { using tier4_metric_msgs::msg::Metric; using tier4_metric_msgs::msg::MetricArray; @@ -61,6 +61,6 @@ class MetricConverter : public rclcpp::Node std::vector::SharedPtr>> params_pub_; }; -} // namespace scenario_simulator_v2_adapter +} // namespace autoware::scenario_simulator_v2_adapter -#endif // SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ +#endif // AUTOWARE__SCENARIO_SIMULATOR_V2_ADAPTER__CONVERTER_NODE_HPP_ diff --git a/evaluator/scenario_simulator_v2_adapter/package.xml b/evaluator/autoware_scenario_simulator_v2_adapter/package.xml similarity index 87% rename from evaluator/scenario_simulator_v2_adapter/package.xml rename to evaluator/autoware_scenario_simulator_v2_adapter/package.xml index 3922bc84d78d5..481be97c9a871 100644 --- a/evaluator/scenario_simulator_v2_adapter/package.xml +++ b/evaluator/autoware_scenario_simulator_v2_adapter/package.xml @@ -1,13 +1,14 @@ - scenario_simulator_v2_adapter - 0.40.0 + autoware_scenario_simulator_v2_adapter + 0.41.0 Node for converting autoware's messages into UserDefinedValue messages Kyoichi Sugahara Maxime CLEMENT Takamasa Horibe Takamasa Horibe + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json b/evaluator/autoware_scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json similarity index 100% rename from evaluator/scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json rename to evaluator/autoware_scenario_simulator_v2_adapter/schema/scenario_simulator_v2_adapter.schema.json diff --git a/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp b/evaluator/autoware_scenario_simulator_v2_adapter/src/converter_node.cpp similarity index 90% rename from evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp rename to evaluator/autoware_scenario_simulator_v2_adapter/src/converter_node.cpp index 4c0464b8b2d41..8ccfc2e3854d3 100644 --- a/evaluator/scenario_simulator_v2_adapter/src/converter_node.cpp +++ b/evaluator/autoware_scenario_simulator_v2_adapter/src/converter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scenario_simulator_v2_adapter/converter_node.hpp" +#include "autoware/scenario_simulator_v2_adapter/converter_node.hpp" #include #include #include -namespace +namespace autoware::scenario_simulator_v2_adapter { std::string removeInvalidTopicString(const std::string & input_string) { @@ -31,10 +31,7 @@ std::string removeInvalidTopicString(const std::string & input_string) } return result; } -} // namespace -namespace scenario_simulator_v2_adapter -{ MetricConverter::MetricConverter(const rclcpp::NodeOptions & node_options) : Node("scenario_simulator_v2_adapter", node_options) { @@ -81,7 +78,7 @@ rclcpp::Publisher::SharedPtr MetricConverter::getPublisher( } return pubs.at(topic_name); } -} // namespace scenario_simulator_v2_adapter +} // namespace autoware::scenario_simulator_v2_adapter #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(scenario_simulator_v2_adapter::MetricConverter) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::scenario_simulator_v2_adapter::MetricConverter) diff --git a/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp b/evaluator/autoware_scenario_simulator_v2_adapter/test/test_converter_node.cpp similarity index 95% rename from evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp rename to evaluator/autoware_scenario_simulator_v2_adapter/test/test_converter_node.cpp index de2a12ac2caaa..d7ecfc50a1bcf 100644 --- a/evaluator/scenario_simulator_v2_adapter/test/test_converter_node.cpp +++ b/evaluator/autoware_scenario_simulator_v2_adapter/test/test_converter_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scenario_simulator_v2_adapter/converter_node.hpp" +#include "autoware/scenario_simulator_v2_adapter/converter_node.hpp" #include @@ -28,7 +28,7 @@ #include #include -using ConverterNode = scenario_simulator_v2_adapter::MetricConverter; +using ConverterNode = autoware::scenario_simulator_v2_adapter::MetricConverter; using tier4_metric_msgs::msg::Metric; using tier4_metric_msgs::msg::MetricArray; using tier4_simulation_msgs::msg::UserDefinedValue; diff --git a/evaluator/kinematic_evaluator/README.md b/evaluator/kinematic_evaluator/README.md deleted file mode 100644 index 7cc826195c1b4..0000000000000 --- a/evaluator/kinematic_evaluator/README.md +++ /dev/null @@ -1,7 +0,0 @@ -# Kinematic Evaluator - -TBD - -## Parameters - -{{json_to_markdown("evaluator/kinematic_evaluator/schema/kinematic_evaluator.schema.json")}} diff --git a/evaluator/kinematic_evaluator/launch/kinematic_evaluator.launch.xml b/evaluator/kinematic_evaluator/launch/kinematic_evaluator.launch.xml deleted file mode 100644 index ebb89c2897ce3..0000000000000 --- a/evaluator/kinematic_evaluator/launch/kinematic_evaluator.launch.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/evaluator/perception_online_evaluator/CMakeLists.txt b/evaluator/perception_online_evaluator/CMakeLists.txt deleted file mode 100644 index b323d07a882e8..0000000000000 --- a/evaluator/perception_online_evaluator/CMakeLists.txt +++ /dev/null @@ -1,44 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(perception_online_evaluator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(pluginlib REQUIRED) - -ament_auto_add_library(${PROJECT_NAME}_node SHARED - src/metrics_calculator.cpp - src/${PROJECT_NAME}_node.cpp - src/metrics/deviation_metrics.cpp - src/metrics/detection_count.cpp - src/utils/marker_utils.cpp - src/utils/objects_filtering.cpp -) - -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "perception_diagnostics::PerceptionOnlineEvaluatorNode" - EXECUTABLE ${PROJECT_NAME} -) - -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "perception_diagnostics::MotionEvaluatorNode" - EXECUTABLE motion_evaluator -) - -target_link_libraries(${PROJECT_NAME}_node glog::glog) - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_perception_online_evaluator_node.cpp - TIMEOUT 300 - ) - target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME}_node - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE - param - launch -) diff --git a/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml b/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml deleted file mode 100644 index 08c4e11126ec1..0000000000000 --- a/evaluator/perception_online_evaluator/launch/motion_evaluator.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/launch/tier4_autoware_api_launch/CHANGELOG.rst b/launch/tier4_autoware_api_launch/CHANGELOG.rst index 5892c238a3bd6..3e4e35d92c0a0 100644 --- a/launch/tier4_autoware_api_launch/CHANGELOG.rst +++ b/launch/tier4_autoware_api_launch/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package tier4_autoware_api_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `default_ad_api_helpers` (`#9965 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Takagi, Isamu +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml index 143fbc9be5bfb..4c204fe2795e8 100644 --- a/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml +++ b/launch/tier4_autoware_api_launch/launch/autoware_api.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/launch/tier4_autoware_api_launch/package.xml b/launch/tier4_autoware_api_launch/package.xml index 45df577f28f29..5cd633a65277d 100644 --- a/launch/tier4_autoware_api_launch/package.xml +++ b/launch/tier4_autoware_api_launch/package.xml @@ -2,7 +2,7 @@ tier4_autoware_api_launch - 0.40.0 + 0.41.0 The tier4_autoware_api_launch package Takagi, Isamu Ryohsuke Mitsudome @@ -11,7 +11,7 @@ ament_cmake_auto autoware_cmake - ad_api_adaptors + autoware_adapi_adaptors autoware_default_adapi autoware_iv_external_api_adaptor autoware_iv_internal_api_adaptor diff --git a/launch/tier4_control_launch/CHANGELOG.rst b/launch/tier4_control_launch/CHANGELOG.rst index 3f94ac4f2b2e0..c762f0fe3a27c 100644 --- a/launch/tier4_control_launch/CHANGELOG.rst +++ b/launch/tier4_control_launch/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package tier4_control_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `predicted_path_checker` (`#9985 `_) + * feat(predicted_path_checker): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(predicted_path_checker): move headers under `include/autoware` + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(predicted_path_checker): fix include header paths + * To follow the previous commit + * rename: `predicted_path_checker` => `autoware_predicted_path_checker` + * style(pre-commit): autofix + * bug(autoware_predicted_path_checker): fix inconsistent namespacings + * bug(autoware_predicted_path_checker): do not change node name + * This might contaminate topic name + * style(pre-commit): autofix + * bug(tier4_control_launch): fix wrong package/plugin names + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/launch/tier4_control_launch/launch/control.launch.xml b/launch/tier4_control_launch/launch/control.launch.xml index 2b0339f1c5cad..f35f02e1d0a80 100644 --- a/launch/tier4_control_launch/launch/control.launch.xml +++ b/launch/tier4_control_launch/launch/control.launch.xml @@ -257,7 +257,7 @@ - + diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index d9a2f9883cbce..8f9fec3c289ff 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -2,7 +2,7 @@ tier4_control_launch - 0.40.0 + 0.41.0 The tier4_control_launch package Takamasa Horibe Takayuki Murooka diff --git a/launch/tier4_localization_launch/CHANGELOG.rst b/launch/tier4_localization_launch/CHANGELOG.rst index 991f04240d04d..8e11fb9ff877a 100644 --- a/launch/tier4_localization_launch/CHANGELOG.rst +++ b/launch/tier4_localization_launch/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package tier4_localization_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `default_ad_api_helpers` (`#9965 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Takagi, Isamu +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml index 02d4f169cbc0d..9ec7ed2d62886 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/pose_twist_estimator.launch.xml @@ -134,7 +134,7 @@ - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index 55c9c59a2b000..5a79e29858725 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -2,7 +2,7 @@ tier4_localization_launch - 0.40.0 + 0.41.0 The tier4_localization_launch package Yamato Ando Kento Yabuuchi @@ -17,8 +17,8 @@ ament_cmake_auto autoware_cmake - automatic_pose_initializer autoware_ar_tag_based_localizer + autoware_automatic_pose_initializer autoware_ekf_localizer autoware_geo_pose_projector autoware_gyro_odometer diff --git a/launch/tier4_map_launch/CHANGELOG.rst b/launch/tier4_map_launch/CHANGELOG.rst index 5ba95030bf15b..beb24e7e79641 100644 --- a/launch/tier4_map_launch/CHANGELOG.rst +++ b/launch/tier4_map_launch/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_map_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/launch/tier4_map_launch/package.xml b/launch/tier4_map_launch/package.xml index ba512960af1b2..66f2f9490192a 100644 --- a/launch/tier4_map_launch/package.xml +++ b/launch/tier4_map_launch/package.xml @@ -2,7 +2,7 @@ tier4_map_launch - 0.40.0 + 0.41.0 The tier4_map_launch package Ryu Yamamoto Kento Yabuuchi diff --git a/launch/tier4_perception_launch/CHANGELOG.rst b/launch/tier4_perception_launch/CHANGELOG.rst index b8ac734476a36..f6315cc25ac6d 100644 --- a/launch/tier4_perception_launch/CHANGELOG.rst +++ b/launch/tier4_perception_launch/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package tier4_perception_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `dummy_perception_publisher` (`#9987 `_) +* fix(launch): fix missing changes for launch (`#10007 `_) + bug(launch): fix missing changes for following PRs: + * https://github.com/autowarefoundation/autoware.universe/pull/9956 + * https://github.com/autowarefoundation/autoware.universe/pull/9970 +* fix(tier4_perception_launch): rearrange roi based cluster pipeline (`#9938 `_) +* fix(image_projection_based_fusion): revise message publishers (`#9865 `_) + * refactor: fix condition for publishing painted pointcloud message + * fix: publish output revised + * feat: fix condition for publishing painted pointcloud message + * feat: roi-pointclout fusion - publish empty image even when there is no target roi + * fix: remap output topic for clusters in roi_pointcloud_fusion + * style(pre-commit): autofix + * feat: fix condition for publishing painted pointcloud message + * feat: Add debug publisher for internal debugging + * feat: remove !! pointer to bool conversion + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat(autoware_object_merger, autoware_tracking_object_merger): enable anonymized node names to be configurable (`#9733 `_) + feat: enable anonymized node names to be configurable +* refactor(tier4_perception_launch): refactoring detection launchers (`#9611 `_) + * feat: Update object detection launch files to include input and output arguments + The object detection launch files have been updated to include input and output arguments for better flexibility and modularity. This allows for easier integration with other components and improves the overall performance of the system. + ``` + * feat: Update object detection launch files to include input and output arguments + * refactor: Update object detection launch files for better readability + * Update object detection launch files to include clustering output argument + * fix: pass ns argument to the lidar_rule_detector + * refactor: make euclidean_cluster not to use use_pointcloud_container and mark explicitly + --------- +* fix(tier4_perception_launch): update multi-channel subscribing channel name to lidar_detection_model_type (`#9624 `_) + * feat: update object detection channels in tracking.launch.xml + The object detection channels in the `tracking.launch.xml` file have been updated to include the lidar detection model type. + * feat: support even the validator is not used + add variable use_validator to the tracking launch and determine the subscribing channel depends on the use_validator value + --------- +* Contributors: Fumiya Watanabe, Junya Sasaki, Taekjin LEE, badai nguyen + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" 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 499cf3c192164..912a7f3e48f41 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 @@ -47,37 +47,40 @@ - + - - - - - - - - - + + + + + + + + + + + + - - - - - - + + + + + + - - - + + + - - - - + + + + @@ -129,6 +132,7 @@ + @@ -159,6 +163,7 @@ + @@ -182,9 +187,11 @@ + + @@ -217,6 +224,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml index e06682853d956..ff890b5940f8a 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/camera_lidar_detector.launch.xml @@ -1,5 +1,8 @@ + + + @@ -41,6 +44,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + + @@ -110,14 +155,11 @@ - - - - - - + + + @@ -151,31 +193,22 @@ - - + + - - - - - - - - - + - - + + - @@ -207,8 +240,8 @@ - - + + @@ -216,24 +249,31 @@ - - + + + + + + + + + + + - - - - + + - - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml index 76ed8ad5b2700..50cde24473d2c 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_rule_detector.launch.xml @@ -1,20 +1,31 @@ + + + + + + + + + + + + - - + + - @@ -22,16 +33,16 @@ - - + + - - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml index 754542f65b0ba..286bd43a8cc1e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/tracker_based_detector.launch.xml @@ -1,11 +1,17 @@ + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml index fcfa9baf4ae20..236fea11d9dda 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_merger.launch.xml @@ -120,6 +120,7 @@ + @@ -132,6 +133,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml index c289a81c906fe..5da942e8ff8f3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/camera_lidar_radar_merger.launch.xml @@ -143,6 +143,7 @@ + @@ -155,6 +156,7 @@ + @@ -178,6 +180,7 @@ Control parameter 'use_radar_tracking_fusion' should defined in perception.launch.xml --> + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml index a492e7667c347..4242903c1082d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/merger/lidar_merger.launch.xml @@ -58,6 +58,7 @@ + @@ -70,6 +71,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index cc9fe78b748a4..1d34dd279b5fa 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -15,6 +15,7 @@ + @@ -56,6 +57,7 @@ + @@ -68,21 +70,23 @@ + + - - + + - - + + - - + + - - + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 5c04cc3e10679..ffa6291257b87 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -77,7 +77,7 @@ - + @@ -272,6 +272,7 @@ + @@ -286,7 +287,7 @@ - + @@ -310,6 +311,6 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 613223fa960bd..41a019a40590c 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -2,7 +2,7 @@ tier4_perception_launch - 0.40.0 + 0.41.0 The tier4_perception_launch package Yukihiro Saito Shunsuke Miura diff --git a/launch/tier4_planning_launch/CHANGELOG.rst b/launch/tier4_planning_launch/CHANGELOG.rst index efa4a02803c46..92c390600ee79 100644 --- a/launch/tier4_planning_launch/CHANGELOG.rst +++ b/launch/tier4_planning_launch/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package tier4_planning_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_mission_planner)!: feat(autoware_mission_planner_universe)!: add _universe suffix to package name (`#9941 `_) +* feat(motion_velocity_planner)!: add _universe suffix to autoware_motion_velocity_planner_common and autoware_motion_velocity_planner_node (`#9942 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 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 cd33e7517ddc3..c5c4debd80956 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/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 555e8da787013..2d5177d4252d8 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 @@ -119,7 +119,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 0a6e9d7fce13a..431450c4f24a2 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -2,7 +2,7 @@ tier4_planning_launch - 0.40.0 + 0.41.0 The tier4_planning_launch package @@ -64,7 +64,7 @@ autoware_external_velocity_limit_selector autoware_freespace_planner autoware_glog_component - autoware_mission_planner + autoware_mission_planner_universe autoware_obstacle_cruise_planner autoware_obstacle_stop_planner autoware_path_optimizer diff --git a/launch/tier4_sensing_launch/CHANGELOG.rst b/launch/tier4_sensing_launch/CHANGELOG.rst index 7e6e12e53a08c..272cd43756e65 100644 --- a/launch/tier4_sensing_launch/CHANGELOG.rst +++ b/launch/tier4_sensing_launch/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_sensing_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/launch/tier4_sensing_launch/package.xml b/launch/tier4_sensing_launch/package.xml index 6e7553ac9f8f0..8ee3618b75eed 100644 --- a/launch/tier4_sensing_launch/package.xml +++ b/launch/tier4_sensing_launch/package.xml @@ -2,7 +2,7 @@ tier4_sensing_launch - 0.40.0 + 0.41.0 The tier4_sensing_launch package Yukihiro Saito Apache License 2.0 diff --git a/launch/tier4_simulator_launch/CHANGELOG.rst b/launch/tier4_simulator_launch/CHANGELOG.rst index 569da150b16ae..a600556bace8e 100644 --- a/launch/tier4_simulator_launch/CHANGELOG.rst +++ b/launch/tier4_simulator_launch/CHANGELOG.rst @@ -2,6 +2,76 @@ Changelog for package tier4_simulator_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `dummy_perception_publisher` (`#9987 `_) +* fix(tier4_simulator_launch): fix a wrong package name: `fault_injection` => `autoware_fault_injection` (`#10014 `_) +* feat: apply `autoware\_` prefix for `scenario_simulator_v2_adapter` (`#9957 `_) + * feat(autoware_scenario_simulator_v2_adapter): apply `autoware\_` prefix (see below): + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(scenario_simulator_v2_adapter): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(scenario_simulator_v2_adapter): fix include paths + * To follow the previous commit + * rename: `scenario_simulator_v2_adapter` => `autoware_scenario_simulator_v2_adapter` + * bug(autoware_scenario_simulator_v2_adapter): revert wrongly updated copyrights + * bug(autoware_scenario_simulator_v2_adapter): `autoware\_` prefix is not needed here + * bug(autoware_scenario_simulator_v2_adapter): wrong package name in launch side + --------- +* feat: apply `autoware\_` prefix for `simple_planning_simulator` (`#9995 `_) + * feat(simple_planning_simulator): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(simple_planning_simulator): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(simple_planning_simulator): fix include header paths + * To follow the previous commit + * rename: `simple_planning_simulator` => `autoware_simple_planning_simulator` + * bug(autoware_simple_planning_simulator): fix missing changes + * style(pre-commit): autofix + * bug(autoware_simple_planning_simulator): fix errors in build and tests + * I had to run after `rm -rf install build`, ... sorry + * style(pre-commit): autofix + * Fixed NOLINT + * Added NOLINT + * Fixed to autoware_simple_planning_simulator + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Shintaro Sakoda +* feat: apply `autoware\_` prefix for `vehicle_door_simulator` (`#9997 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat: apply `autoware\_` prefix for `fault_injection` (`#9989 `_) + * feat(fault_injection): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(fault_injection): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(fault_injection): fix include header paths + * To follow the previous commit + * rename: `fault_injection` => `autoware_fault_injection` + * Fixed exec_depend + --------- + Co-authored-by: SakodaShintaro +* fix(launch): fix missing changes for launch (`#10007 `_) + bug(launch): fix missing changes for following PRs: + * https://github.com/autowarefoundation/autoware.universe/pull/9956 + * https://github.com/autowarefoundation/autoware.universe/pull/9970 +* fix(tier4_simulator_launch): add use_validator argument to simulator launch (`#9634 `_) + * feat: add use_validator argument to simulator launch + * feat: set variables explicitly + --------- +* Contributors: Fumiya Watanabe, Junya Sasaki, Taekjin LEE + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 4b2cefa02c2fa..1ff16f2f4fe81 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -40,14 +40,14 @@ - + - + @@ -89,6 +89,9 @@ + + + @@ -102,13 +105,13 @@ - + - + @@ -200,12 +203,12 @@ - + - + @@ -216,7 +219,7 @@ - + diff --git a/launch/tier4_simulator_launch/package.xml b/launch/tier4_simulator_launch/package.xml index 19bc81b056b60..85a57324b6851 100644 --- a/launch/tier4_simulator_launch/package.xml +++ b/launch/tier4_simulator_launch/package.xml @@ -2,7 +2,7 @@ tier4_simulator_launch - 0.40.0 + 0.41.0 The tier4_simulator_launch package Keisuke Shima Takayuki Murooka @@ -14,9 +14,9 @@ ament_cmake_auto autoware_cmake - dummy_perception_publisher - fault_injection - simple_planning_simulator + autoware_dummy_perception_publisher + autoware_fault_injection + autoware_simple_planning_simulator ament_lint_auto autoware_lint_common diff --git a/launch/tier4_system_launch/CHANGELOG.rst b/launch/tier4_system_launch/CHANGELOG.rst index 9ffb34c4599a9..1f03f4c66db02 100644 --- a/launch/tier4_system_launch/CHANGELOG.rst +++ b/launch/tier4_system_launch/CHANGELOG.rst @@ -2,6 +2,81 @@ Changelog for package tier4_system_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware` prefix for `component_state_monitor` and its dependencies (`#9961 `_) +* feat: apply `autoware\_` prefix for `hazard_status_converter` (`#9971 `_) +* feat: apply `autoware\_` prefix for `system_monitor` (`#10017 `_) + * feat(system_monitor): apply `autoware\_` prefix (see below): + * The `system_monitor` operates independently from other modules in `autoware.universe`, so the `autoware\_` prefix is added only to the package name. + * The `autoware::` namespace is not used because C language does not support namespaces. + * Headers are not moved under `include/autoware` to maintain compatibility for use outside the `autoware` context. + * For users utilizing this package within `autoware.universe`, only the package name includes the `autoware\_` prefix. + This approach explains the unique namespacing and naming conventions for `system_monitor` compared to other packages. + * bug(system_monitor): fix missing package name update + * rename: `system_monitor` => `autoware_system_monitor` + * style(pre-commit): autofix + * update: `CODEOWNERS` + * bug(autoware_system_monitor): apply missing fix + * update: `CODEOWNERS` + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat: apply `autoware\_` prefix for `mrm_comfortable_stop_operator` (`#10011 `_) + * feat(mrm_comfortable_stop_operator): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(mrm_comfortable_stop_operator): move a header under `include/autoware` + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(mrm_comfortable_stop_operator): fix include header path + * To follow the previous commit + * rename: `mrm_comfortable_stop_operator` => `autoware_mrm_comfortable_stop_operator` + * update: `CODEOWNERS` + --------- +* feat: apply `autoware\_` prefix for `mrm_emergency_stop_operator` (`#9973 `_) + * feat(mrm_emergency_stop_operator): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(mrm_emergency_stop_operator): move a header under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(mrm_emergency_stop_operator): fix include header path + * To follow the previous commit + * rename: `mrm_emergency_stop_operator` => `autoware_mrm_emergency_stop_operator` + * bug(autoware_mrm_emergency_stop_operator): revert wrongly updated copyrights + * bug(tier4_system_launch): fix a missing `autoware\_` for `mrm_emergency_stop_operator` + * bug(autoware_mrm_emergency_stop_operator): fix critical bugs that contaminate topic names + --------- +* feat: apply `autoware\_` prefix for `mrm_handler` (`#9974 `_) + * feat(mrm_handler): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(mrm_handler): move a header under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(mrm_handler): fix include header path + * To follow the previous commit + * rename: `mrm_handler` => `autoware_mrm_handler` + * bug(tier4_system_launch): fix a missing `autoware\_` for `mrm_handler` + * bug(mrm_handler): revert wrongly updated copyrights + * update(mrm_handler): `README.md` + * bug(autoware_mrm_handler): fix a critical bug that contaminates topic name + --------- +* fix(launch): fix missing changes for launch (`#10007 `_) + bug(launch): fix missing changes for following PRs: + * https://github.com/autowarefoundation/autoware.universe/pull/9956 + * https://github.com/autowarefoundation/autoware.universe/pull/9970 +* fix(tier4_system_launch): add autoware prefix to dummy diag publisher launcher (`#9959 `_) + fix: add autoare\_ to dummy_diag_publisher +* Contributors: Fumiya Watanabe, Junya Sasaki, TetsuKawa + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 5090f9ab83293..c22b8b091bf59 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -39,7 +39,7 @@ - + @@ -53,7 +53,7 @@ - + @@ -72,7 +72,7 @@ - + @@ -80,19 +80,19 @@ - + - + - + @@ -100,12 +100,12 @@ - + - + @@ -113,7 +113,7 @@ - + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 9f5678b937dbc..318d6a2673c50 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -2,7 +2,7 @@ tier4_system_launch - 0.40.0 + 0.41.0 The tier4_system_launch package Fumihito Ito TetsuKawa @@ -11,8 +11,8 @@ ament_cmake_auto autoware_cmake - component_state_monitor - system_monitor + autoware_component_state_monitor + autoware_system_monitor ament_lint_auto autoware_lint_common diff --git a/launch/tier4_vehicle_launch/CHANGELOG.rst b/launch/tier4_vehicle_launch/CHANGELOG.rst index 1b5a1271e9fe3..6da13259c5612 100644 --- a/launch/tier4_vehicle_launch/CHANGELOG.rst +++ b/launch/tier4_vehicle_launch/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_vehicle_launch ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/launch/tier4_vehicle_launch/package.xml b/launch/tier4_vehicle_launch/package.xml index 583bbf325db05..80f07dab5383e 100644 --- a/launch/tier4_vehicle_launch/package.xml +++ b/launch/tier4_vehicle_launch/package.xml @@ -2,7 +2,7 @@ tier4_vehicle_launch - 0.40.0 + 0.41.0 The tier4_vehicle_launch package Yukihiro Saito Apache License 2.0 diff --git a/localization/autoware_ekf_localizer/CHANGELOG.rst b/localization/autoware_ekf_localizer/CHANGELOG.rst index b8e74c530e059..9f520e0d9c112 100644 --- a/localization/autoware_ekf_localizer/CHANGELOG.rst +++ b/localization/autoware_ekf_localizer/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package autoware_ekf_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* revert: revert "feat(autoware_ekf_localizer)!: porting from universe to core (`#9978 `_)" (`#10004 `_) + This reverts commit 037c315fbee69bb5923ec10bb8e8e70f890725ea. +* feat(autoware_ekf_localizer)!: porting from universe to core (`#9978 `_) + * feat: delete ekf_localizer files + * doc: Modify ekf_localizer directory links + * ci: remove ekf_localizer from the codecov target list + --------- +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fies localization/autoware_ekf_localizer (`#9860 `_) + Co-authored-by: SakodaShintaro +* feat(ekf_localizer): check whether the initialpose has been set (`#9787 `_) + * check set intialpose + * update png + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Motz, Ryohsuke Mitsudome, Vishal Chauhan, Yamato Ando + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_ekf_localizer/README.md b/localization/autoware_ekf_localizer/README.md index aad65da2516d1..a46ea7181315f 100644 --- a/localization/autoware_ekf_localizer/README.md +++ b/localization/autoware_ekf_localizer/README.md @@ -44,17 +44,17 @@ This package includes the following features: ### Published Topics -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------ | ----------------------------------------------------- | -| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | -| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | -| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | -| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | -| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | -| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | -| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | -| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | -| `debug/processing_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | The processing time [ms]. | +| Name | Type | Description | +| --------------------------------- | --------------------------------------------------- | ----------------------------------------------------- | +| `ekf_odom` | `nav_msgs::msg::Odometry` | Estimated odometry. | +| `ekf_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose. | +| `ekf_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance. | +| `ekf_biased_pose` | `geometry_msgs::msg::PoseStamped` | Estimated pose including the yaw bias | +| `ekf_biased_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated pose with covariance including the yaw bias | +| `ekf_twist` | `geometry_msgs::msg::TwistStamped` | Estimated twist. | +| `ekf_twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The estimated twist with covariance. | +| `diagnostics` | `diagnostics_msgs::msg::DiagnosticArray` | The diagnostic information. | +| `debug/processing_time_ms` | `autoware_internal_debug_msgs::msg::Float64Stamped` | The processing time [ms]. | ### Published TF @@ -191,6 +191,7 @@ Note that, although the dimension gets larger since the analytical expansion can ### The conditions that result in a WARN state - The node is not in the activate state. +- The initial pose is not set. - The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`. - The timestamp of the Pose/Twist topic is beyond the delay compensation range. - The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation. diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp index 20a77354c0996..b194c3e956341 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/diagnostics.hpp @@ -24,6 +24,7 @@ namespace autoware::ekf_localizer { diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); +diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose); diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, diff --git a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp index 1b437f26e9c7d..84018c043cc14 100644 --- a/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp +++ b/localization/autoware_ekf_localizer/include/autoware/ekf_localizer/ekf_localizer.hpp @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include #include @@ -34,8 +36,6 @@ #include #include #include -#include -#include #include #include @@ -78,7 +78,7 @@ class EKFLocalizer : public rclcpp::Node //!< @brief ekf estimated twist with covariance publisher rclcpp::Publisher::SharedPtr pub_twist_cov_; //!< @brief ekf estimated yaw bias publisher - rclcpp::Publisher::SharedPtr pub_yaw_bias_; + rclcpp::Publisher::SharedPtr pub_yaw_bias_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher @@ -86,7 +86,8 @@ class EKFLocalizer : public rclcpp::Node //!< @brief diagnostics publisher rclcpp::Publisher::SharedPtr pub_diag_; //!< @brief processing_time publisher - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber @@ -119,6 +120,7 @@ class EKFLocalizer : public rclcpp::Node double ekf_dt_; bool is_activated_; + bool is_set_initialpose_; EKFDiagnosticInfo pose_diag_info_; EKFDiagnosticInfo twist_diag_info_; diff --git a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png b/localization/autoware_ekf_localizer/media/ekf_diagnostics.png index 234b2f1e19b6d..a1561c3f52707 100644 Binary files a/localization/autoware_ekf_localizer/media/ekf_diagnostics.png and b/localization/autoware_ekf_localizer/media/ekf_diagnostics.png differ diff --git a/localization/autoware_ekf_localizer/package.xml b/localization/autoware_ekf_localizer/package.xml index 8dc3cc9844c50..ff510862eb089 100644 --- a/localization/autoware_ekf_localizer/package.xml +++ b/localization/autoware_ekf_localizer/package.xml @@ -2,7 +2,7 @@ autoware_ekf_localizer - 0.40.0 + 0.41.0 The autoware_ekf_localizer package Takamasa Horibe Yamato Ando @@ -22,6 +22,7 @@ eigen + autoware_internal_debug_msgs autoware_kalman_filter autoware_localization_util autoware_universe_utils @@ -34,7 +35,6 @@ std_srvs tf2 tf2_ros - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_ekf_localizer/src/diagnostics.cpp b/localization/autoware_ekf_localizer/src/diagnostics.cpp index a1af492487fe4..45468abf72d6c 100644 --- a/localization/autoware_ekf_localizer/src/diagnostics.cpp +++ b/localization/autoware_ekf_localizer/src/diagnostics.cpp @@ -41,6 +41,25 @@ diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_act return stat; } +diagnostic_msgs::msg::DiagnosticStatus check_set_initialpose(const bool is_set_initialpose) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "is_set_initialpose"; + key_value.value = is_set_initialpose ? "True" : "False"; + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_set_initialpose) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]initial pose is not set"; + } + + return stat; +} + 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) diff --git a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp index d34be2a537ef1..5638a5416e6ab 100644 --- a/localization/autoware_ekf_localizer/src/ekf_localizer.cpp +++ b/localization/autoware_ekf_localizer/src/ekf_localizer.cpp @@ -56,6 +56,7 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) twist_queue_(params_.twist_smoothing_steps) { is_activated_ = false; + is_set_initialpose_ = false; /* initialize ros system */ timer_control_ = rclcpp::create_timer( @@ -69,13 +70,14 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) pub_twist_ = create_publisher("ekf_twist", 1); pub_twist_cov_ = create_publisher( "ekf_twist_with_covariance", 1); - pub_yaw_bias_ = create_publisher("estimated_yaw_bias", 1); + pub_yaw_bias_ = + create_publisher("estimated_yaw_bias", 1); pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); pub_biased_pose_cov_ = create_publisher( "ekf_biased_pose_with_covariance", 1); pub_diag_ = this->create_publisher("/diagnostics", 10); - pub_processing_time_ = - create_publisher("debug/processing_time_ms", 1); + pub_processing_time_ = create_publisher( + "debug/processing_time_ms", 1); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -143,6 +145,13 @@ void EKFLocalizer::timer_callback() return; } + if (!is_set_initialpose_) { + warning_->warn_throttle( + "Initial pose is not set. Provide initial pose to pose_initializer", 2000); + publish_diagnostics(geometry_msgs::msg::PoseStamped{}, current_time); + return; + } + DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ @@ -227,9 +236,10 @@ void EKFLocalizer::timer_callback() /* publish processing time */ const double elapsed_time = stop_watch_timer_cb_.toc(); - pub_processing_time_->publish(tier4_debug_msgs::build() - .stamp(current_time) - .data(elapsed_time)); + pub_processing_time_->publish( + autoware_internal_debug_msgs::build() + .stamp(current_time) + .data(elapsed_time)); } /* @@ -264,6 +274,8 @@ void EKFLocalizer::callback_initial_pose( params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } ekf_module_->initialize(*msg, transform); + + is_set_initialpose_ = true; } /* @@ -272,7 +284,7 @@ void EKFLocalizer::callback_initial_pose( void EKFLocalizer::callback_pose_with_covariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - if (!is_activated_) { + if (!is_activated_ && !is_set_initialpose_) { return; } @@ -333,7 +345,7 @@ void EKFLocalizer::publish_estimate_result( pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ - tier4_debug_msgs::msg::Float64Stamped yawb; + autoware_internal_debug_msgs::msg::Float64Stamped yawb; yawb.stamp = current_ekf_twist.header.stamp; yawb.data = ekf_module_->get_yaw_bias(); pub_yaw_bias_->publish(yawb); @@ -359,8 +371,9 @@ void EKFLocalizer::publish_diagnostics( std::vector diag_status_array; diag_status_array.push_back(check_process_activated(is_activated_)); + diag_status_array.push_back(check_set_initialpose(is_set_initialpose_)); - if (is_activated_) { + if (is_activated_ && is_set_initialpose_) { 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)); @@ -439,6 +452,7 @@ void EKFLocalizer::service_trigger_node( is_activated_ = true; } else { is_activated_ = false; + is_set_initialpose_ = false; } res->success = true; } diff --git a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp index 165102adec1d7..5ce39df484e98 100644 --- a/localization/autoware_ekf_localizer/test/test_diagnostics.cpp +++ b/localization/autoware_ekf_localizer/test/test_diagnostics.cpp @@ -35,6 +35,19 @@ TEST(TestEkfDiagnostics, check_process_activated) EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } +TEST(TestEkfDiagnostics, check_set_initialpose) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + bool is_set_initialpose = true; + stat = check_set_initialpose(is_set_initialpose); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_set_initialpose = false; + stat = check_set_initialpose(is_set_initialpose); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + TEST(TestEkfDiagnostics, check_measurement_updated) { diagnostic_msgs::msg::DiagnosticStatus stat; diff --git a/localization/autoware_geo_pose_projector/CHANGELOG.rst b/localization/autoware_geo_pose_projector/CHANGELOG.rst index 1b45974da9cc2..2c8750eb5fd31 100644 --- a/localization/autoware_geo_pose_projector/CHANGELOG.rst +++ b/localization/autoware_geo_pose_projector/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_geo_pose_projector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_geo_pose_projector/package.xml b/localization/autoware_geo_pose_projector/package.xml index 6dda1d6418406..5981e61836b05 100644 --- a/localization/autoware_geo_pose_projector/package.xml +++ b/localization/autoware_geo_pose_projector/package.xml @@ -2,7 +2,7 @@ autoware_geo_pose_projector - 0.40.0 + 0.41.0 The autoware_geo_pose_projector package Yamato Ando Masahiro Sakamoto @@ -17,7 +17,7 @@ ament_cmake_auto autoware_cmake - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_geography_utils geographic_msgs diff --git a/localization/autoware_geo_pose_projector/src/geo_pose_projector.hpp b/localization/autoware_geo_pose_projector/src/geo_pose_projector.hpp index f45605968c8c9..cae37d4b682d8 100644 --- a/localization/autoware_geo_pose_projector/src/geo_pose_projector.hpp +++ b/localization/autoware_geo_pose_projector/src/geo_pose_projector.hpp @@ -15,7 +15,7 @@ #ifndef GEO_POSE_PROJECTOR_HPP_ #define GEO_POSE_PROJECTOR_HPP_ -#include +#include #include #include @@ -35,7 +35,7 @@ class GeoPoseProjector : public rclcpp::Node private: using GeoPoseWithCovariance = geographic_msgs::msg::GeoPoseWithCovarianceStamped; using PoseWithCovariance = geometry_msgs::msg::PoseWithCovarianceStamped; - using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo; + using MapProjectorInfo = autoware::component_interface_specs_universe::map::MapProjectorInfo; public: explicit GeoPoseProjector(const rclcpp::NodeOptions & options); diff --git a/localization/autoware_gyro_odometer/CHANGELOG.rst b/localization/autoware_gyro_odometer/CHANGELOG.rst index 529fe48f7b459..063d1aaa4fd62 100644 --- a/localization/autoware_gyro_odometer/CHANGELOG.rst +++ b/localization/autoware_gyro_odometer/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package autoware_gyro_odometer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(autoware_universe_utils): add missing 's' in the class of diagnostics_interface (`#9777 `_) +* feat!: move diagnostics_module from localization_util to unverse_utils (`#9714 `_) + * feat!: move diagnostics_module from localization_util to unverse_utils + * remove diagnostics module from localization_util + * style(pre-commit): autofix + * minor fix in pose_initializer + * add test + * style(pre-commit): autofix + * remove unnecessary declaration + * module -> interface + * remove unnecessary equal expression + * revert the remove of template function + * style(pre-commit): autofix + * use overload instead + * include what you use -- test_diagnostics_interface.cpp + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, kminoda + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/localization/autoware_gyro_odometer/package.xml b/localization/autoware_gyro_odometer/package.xml index 87474c0b13c0f..c05867de34aaa 100644 --- a/localization/autoware_gyro_odometer/package.xml +++ b/localization/autoware_gyro_odometer/package.xml @@ -2,7 +2,7 @@ autoware_gyro_odometer - 0.40.0 + 0.41.0 The autoware_gyro_odometer package as a ROS 2 node Yamato Ando Masahiro Sakamoto diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp index 9511f168f346f..0a85f75b74ad7 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.cpp @@ -73,7 +73,7 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) "twist_with_covariance", rclcpp::QoS{10}); diagnostics_ = - std::make_unique(this, "gyro_odometer_status"); + std::make_unique(this, "gyro_odometer_status"); // TODO(YamatoAndo) createTimer } diff --git a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp index 036b3d7cab527..b59e6af341ec2 100644 --- a/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp +++ b/localization/autoware_gyro_odometer/src/gyro_odometer_core.hpp @@ -15,7 +15,7 @@ #ifndef GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER_CORE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware/universe_utils/ros/transform_listener.hpp" @@ -80,7 +80,7 @@ class GyroOdometerNode : public rclcpp::Node std::deque vehicle_twist_queue_; std::deque gyro_queue_; - std::unique_ptr diagnostics_; + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CHANGELOG.rst b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CHANGELOG.rst index 36d7431a499ff..9c61094b6596e 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CHANGELOG.rst +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_ar_tag_based_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml index 4aab7e0b5cdaf..b69ebe21480d4 100644 --- a/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/package.xml @@ -2,7 +2,7 @@ autoware_ar_tag_based_localizer - 0.40.0 + 0.41.0 The autoware_ar_tag_based_localizer package Shintaro Sakoda Masahiro Sakamoto diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CHANGELOG.rst b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CHANGELOG.rst index f6d4cf6766e84..167dd0bbf5e0d 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CHANGELOG.rst +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_landmark_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml index c33b8a751ce7f..3339070979535 100644 --- a/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_landmark_manager/package.xml @@ -2,7 +2,7 @@ autoware_landmark_manager - 0.40.0 + 0.41.0 The autoware_landmark_manager package Shintaro Sakoda Masahiro Sakamoto diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CHANGELOG.rst b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CHANGELOG.rst index a7edd5b7d2a69..d2a3371ad096b 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CHANGELOG.rst +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package autoware_lidar_marker_localizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_lidar_marker_localization): fix segmentation fault (`#8943 `_) + * fix segmentation fault + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* refactor(autoware_universe_utils): add missing 's' in the class of diagnostics_interface (`#9777 `_) +* feat!: move diagnostics_module from localization_util to unverse_utils (`#9714 `_) + * feat!: move diagnostics_module from localization_util to unverse_utils + * remove diagnostics module from localization_util + * style(pre-commit): autofix + * minor fix in pose_initializer + * add test + * style(pre-commit): autofix + * remove unnecessary declaration + * module -> interface + * remove unnecessary equal expression + * revert the remove of template function + * style(pre-commit): autofix + * use overload instead + * include what you use -- test_diagnostics_interface.cpp + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Yamato Ando, kminoda + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml index c365f7ef040dc..dc0b8cf663c06 100755 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/package.xml @@ -2,7 +2,7 @@ autoware_lidar_marker_localizer - 0.40.0 + 0.41.0 The autoware_lidar_marker_localizer package Yamato Ando Shintaro Sakoda diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp index 2faf2d19168a5..5a43253964154 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.cpp @@ -124,22 +124,22 @@ LidarMarkerLocalizer::LidarMarkerLocalizer(const rclcpp::NodeOptions & node_opti tf_buffer_ = std::make_shared(this->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_, this, false); - diagnostics_module_.reset( - new autoware::localization_util::DiagnosticsModule(this, "marker_detection_status")); + diagnostics_interface_.reset( + new autoware::universe_utils::DiagnosticsInterface(this, "marker_detection_status")); } void LidarMarkerLocalizer::initialize_diagnostics() { - diagnostics_module_->clear(); - diagnostics_module_->add_key_value("is_received_map", false); - diagnostics_module_->add_key_value("is_received_self_pose", false); - diagnostics_module_->add_key_value("detect_marker_num", 0); - diagnostics_module_->add_key_value("distance_self_pose_to_nearest_marker", 0.0); - diagnostics_module_->add_key_value( + diagnostics_interface_->clear(); + diagnostics_interface_->add_key_value("is_received_map", false); + diagnostics_interface_->add_key_value("is_received_self_pose", false); + diagnostics_interface_->add_key_value("detect_marker_num", 0); + diagnostics_interface_->add_key_value("distance_self_pose_to_nearest_marker", 0.0); + diagnostics_interface_->add_key_value( "limit_distance_from_self_pose_to_nearest_marker", param_.limit_distance_from_self_pose_to_nearest_marker); - diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", 0.0); - diagnostics_module_->add_key_value( + diagnostics_interface_->add_key_value("distance_lanelet2_marker_to_detected_marker", 0.0); + diagnostics_interface_->add_key_value( "limit_distance_from_lanelet2_marker_to_detected_marker", param_.limit_distance_from_self_pose_to_marker); } @@ -176,7 +176,7 @@ void LidarMarkerLocalizer::points_callback(const PointCloud2::ConstSharedPtr & p main_process(points_msg_ptr); - diagnostics_module_->publish(sensor_ros_time); + diagnostics_interface_->publish(sensor_ros_time); } void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & points_msg_ptr) @@ -186,13 +186,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin // (1) check if the map have be received const std::vector map_landmarks = landmark_manager_.get_landmarks(); const bool is_received_map = !map_landmarks.empty(); - diagnostics_module_->add_key_value("is_received_map", is_received_map); + diagnostics_interface_->add_key_value("is_received_map", is_received_map); if (!is_received_map) { std::stringstream message; message << "Not receive the landmark information. Please check if the vector map is being " << "published and if the landmark information is correctly specified."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -202,13 +202,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin interpolate_result = ekf_pose_buffer_->interpolate(sensor_ros_time); const bool is_received_self_pose = interpolate_result != std::nullopt; - diagnostics_module_->add_key_value("is_received_self_pose", is_received_self_pose); + diagnostics_interface_->add_key_value("is_received_self_pose", is_received_self_pose); if (!is_received_self_pose) { std::stringstream message; message << "Could not get self_pose. Please check if the self pose is being published and if " << "the timestamp of the self pose is correctly specified"; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -221,7 +221,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin detect_landmarks(points_msg_ptr); const bool is_detected_marker = !detected_landmarks.empty(); - diagnostics_module_->add_key_value("detect_marker_num", detected_landmarks.size()); + diagnostics_interface_->add_key_value("detect_marker_num", detected_landmarks.size()); // (4) check distance to the nearest marker const landmark_manager::Landmark nearest_marker = get_nearest_landmark(self_pose, map_landmarks); @@ -230,7 +230,7 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin const double distance_from_self_pose_to_nearest_marker = std::abs(nearest_marker_pose_on_base_link.position.x); - diagnostics_module_->add_key_value( + diagnostics_interface_->add_key_value( "distance_self_pose_to_nearest_marker", distance_from_self_pose_to_nearest_marker); const bool is_exist_marker_within_self_pose = @@ -242,14 +242,14 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin std::stringstream message; message << "Could not detect marker, because the distance from self_pose to nearest_marker " << "is too far (" << distance_from_self_pose_to_nearest_marker << " [m])."; - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::OK, message.str()); } else { std::stringstream message; message << "Could not detect marker, although the distance from self_pose to nearest_marker " << "is near (" << distance_from_self_pose_to_nearest_marker << " [m])."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); } return; @@ -279,13 +279,13 @@ void LidarMarkerLocalizer::main_process(const PointCloud2::ConstSharedPtr & poin const bool is_exist_marker_within_lanelet2_map = diff_norm < param_.limit_distance_from_self_pose_to_marker; - diagnostics_module_->add_key_value("distance_lanelet2_marker_to_detected_marker", diff_norm); + diagnostics_interface_->add_key_value("distance_lanelet2_marker_to_detected_marker", diff_norm); if (!is_exist_marker_within_lanelet2_map) { std::stringstream message; message << "The distance from lanelet2 to the detect marker is too far(" << diff_norm << " [m]). The limit is " << param_.limit_distance_from_self_pose_to_marker << "."; RCLCPP_INFO_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_module_->update_level_and_message( + diagnostics_interface_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); return; } @@ -362,6 +362,12 @@ std::vector LidarMarkerLocalizer::detect_landmarks( // Check that the leaf size is not too small, given the size of the data const int bin_num = static_cast((max_x - min_x) / param_.resolution + 1); + if (bin_num < static_cast(param_.intensity_pattern.size())) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "bin_num is too small!"); + return std::vector{}; + } + // initialize variables std::vector vote(bin_num, 0); std::vector min_y(bin_num, std::numeric_limits::max()); diff --git a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp index d1482c6912592..22a0c3f642563 100644 --- a/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp +++ b/localization/autoware_landmark_based_localizer/autoware_lidar_marker_localizer/src/lidar_marker_localizer.hpp @@ -15,8 +15,8 @@ #ifndef LIDAR_MARKER_LOCALIZER_HPP_ #define LIDAR_MARKER_LOCALIZER_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include @@ -134,7 +134,7 @@ class LidarMarkerLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_debug_pose_with_covariance_; rclcpp::Publisher::SharedPtr pub_marker_pointcloud_; - std::shared_ptr diagnostics_module_; + std::shared_ptr diagnostics_interface_; Param param_; bool is_activated_; diff --git a/localization/autoware_localization_error_monitor/CHANGELOG.rst b/localization/autoware_localization_error_monitor/CHANGELOG.rst index e9583916e2e8f..8b99b4870eaa1 100644 --- a/localization/autoware_localization_error_monitor/CHANGELOG.rst +++ b/localization/autoware_localization_error_monitor/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package autoware_localization_error_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(autoware_universe_utils): add missing 's' in the class of diagnostics_interface (`#9777 `_) +* feat!: move diagnostics_module from localization_util to unverse_utils (`#9714 `_) + * feat!: move diagnostics_module from localization_util to unverse_utils + * remove diagnostics module from localization_util + * style(pre-commit): autofix + * minor fix in pose_initializer + * add test + * style(pre-commit): autofix + * remove unnecessary declaration + * module -> interface + * remove unnecessary equal expression + * revert the remove of template function + * style(pre-commit): autofix + * use overload instead + * include what you use -- test_diagnostics_interface.cpp + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, kminoda + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" @@ -43,8 +65,8 @@ Changelog for package autoware_localization_error_monitor * unify package.xml version to 0.37.0 * refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) add autoware prefix to localization_util -* fix(localization_error_monitor, system diag): fix to use diagnostics_module in localization_util (`#8543 `_) - * fix(localization_error_monitor): fix to use diagnostics_module in localization_util +* fix(localization_error_monitor, system diag): fix to use diagnostics_interface in localization_util (`#8543 `_) + * fix(localization_error_monitor): fix to use diagnostics_interface in localization_util * fix: update media * fix: update component name * fix: rename include file diff --git a/localization/autoware_localization_error_monitor/package.xml b/localization/autoware_localization_error_monitor/package.xml index f7f9e635e85d6..d3ba2e2852e51 100644 --- a/localization/autoware_localization_error_monitor/package.xml +++ b/localization/autoware_localization_error_monitor/package.xml @@ -2,7 +2,7 @@ autoware_localization_error_monitor - 0.40.0 + 0.41.0 ros node for monitoring localization error Yamato Ando Masahiro Sakamoto diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp index 63edbe5f6a9c7..1f26f6b80aa17 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.cpp @@ -59,7 +59,7 @@ LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & o logger_configure_ = std::make_unique(this); diagnostics_error_monitor_ = - std::make_unique(this, "ellipse_error_status"); + std::make_unique(this, "ellipse_error_status"); } void LocalizationErrorMonitor::on_odom(nav_msgs::msg::Odometry::ConstSharedPtr input_msg) diff --git a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp index 7b26573964b38..b7d2454eb9f75 100644 --- a/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp +++ b/localization/autoware_localization_error_monitor/src/localization_error_monitor.hpp @@ -16,7 +16,7 @@ #define LOCALIZATION_ERROR_MONITOR_HPP_ #include "autoware/localization_util/covariance_ellipse.hpp" -#include "autoware/localization_util/diagnostics_module.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include #include @@ -39,7 +39,7 @@ class LocalizationErrorMonitor : public rclcpp::Node std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_error_monitor_; + std::unique_ptr diagnostics_error_monitor_; double scale_; double error_ellipse_size_; diff --git a/localization/autoware_localization_util/CHANGELOG.rst b/localization/autoware_localization_util/CHANGELOG.rst deleted file mode 100644 index be40e3ee8560e..0000000000000 --- a/localization/autoware_localization_util/CHANGELOG.rst +++ /dev/null @@ -1,51 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_localization_util -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - localization (`#9567 `_) -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Ryohsuke Mitsudome, Yutaka Kondo - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(localization_util)!: prefix package and namespace with autoware (`#8922 `_) - add autoware prefix to localization_util -* Contributors: Masaki Baba, Yutaka Kondo - -0.26.0 (2024-04-03) -------------------- diff --git a/localization/autoware_localization_util/CMakeLists.txt b/localization/autoware_localization_util/CMakeLists.txt deleted file mode 100644 index dd18f3cbad932..0000000000000 --- a/localization/autoware_localization_util/CMakeLists.txt +++ /dev/null @@ -1,30 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_localization_util) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(${PROJECT_NAME} SHARED - src/util_func.cpp - src/diagnostics_module.cpp - src/smart_pose_buffer.cpp - src/tree_structured_parzen_estimator.cpp - src/covariance_ellipse.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_auto_add_gtest(test_smart_pose_buffer - test/test_smart_pose_buffer.cpp - src/smart_pose_buffer.cpp - ) - - ament_auto_add_gtest(test_tpe - test/test_tpe.cpp - src/tree_structured_parzen_estimator.cpp - ) -endif() - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/localization/autoware_localization_util/README.md b/localization/autoware_localization_util/README.md deleted file mode 100644 index f7fddd9eebf05..0000000000000 --- a/localization/autoware_localization_util/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# autoware_localization_util - -`autoware_localization_util` is a localization utility package. - -This package does not have a node, it is just a library. diff --git a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp b/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp deleted file mode 100644 index abd0af46856b0..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/covariance_ellipse.hpp +++ /dev/null @@ -1,44 +0,0 @@ -// Copyright 2024 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__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ - -#include - -#include -#include - -namespace autoware::localization_util -{ - -struct Ellipse -{ - double long_radius; - double short_radius; - double yaw; - Eigen::Matrix2d P; - double size_lateral_direction; -}; - -Ellipse calculate_xy_ellipse( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale); - -visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, const std_msgs::msg::Header & header, - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__COVARIANCE_ELLIPSE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp b/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp deleted file mode 100644 index bda6ff19f2867..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/matrix_type.hpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2021 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT 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__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ - -#include - -namespace autoware::localization_util -{ -using Matrix6d = Eigen::Matrix; -using RowMatrixXd = Eigen::Matrix; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__MATRIX_TYPE_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp b/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp deleted file mode 100644 index 8c10506c36753..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/smart_pose_buffer.hpp +++ /dev/null @@ -1,71 +0,0 @@ -// 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__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ - -#include "autoware/localization_util/util_func.hpp" - -#include - -#include - -#include - -namespace autoware::localization_util -{ -class SmartPoseBuffer -{ -private: - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - -public: - struct InterpolateResult - { - PoseWithCovarianceStamped old_pose; - PoseWithCovarianceStamped new_pose; - PoseWithCovarianceStamped interpolated_pose; - }; - - SmartPoseBuffer() = delete; - SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters); - - std::optional interpolate(const rclcpp::Time & target_ros_time); - - void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr); - - void pop_old(const rclcpp::Time & target_ros_time); - - void clear(); - -private: - rclcpp::Logger logger_; - std::deque pose_buffer_; - std::mutex mutex_; // This mutex is for pose_buffer_ - - const double pose_timeout_sec_; - const double pose_distance_tolerance_meters_; - - [[nodiscard]] bool validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const; - [[nodiscard]] bool validate_position_difference( - const geometry_msgs::msg::Point & target_point, - const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp b/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp deleted file mode 100644 index ddf7625c202ec..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/tree_structured_parzen_estimator.hpp +++ /dev/null @@ -1,87 +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 AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ - -/* -A implementation of tree-structured parzen estimator (TPE) -See below pdf for the TPE algorithm detail. -https://papers.nips.cc/paper_files/paper/2011/file/86e8f7ab32cfd12577bc2619bc635690-Paper.pdf - -Optuna is also used as a reference for implementation. -https://github.com/optuna/optuna -*/ - -#include -#include -#include - -namespace autoware::localization_util -{ -class TreeStructuredParzenEstimator -{ -public: - using Input = std::vector; - using Score = double; - struct Trial - { - Input input; - Score score; - }; - - enum Direction { - MINIMIZE = 0, - MAXIMIZE = 1, - }; - - enum Index { - TRANS_X = 0, - TRANS_Y = 1, - TRANS_Z = 2, - ANGLE_X = 3, - ANGLE_Y = 4, - ANGLE_Z = 5, - INDEX_NUM = 6, - }; - - TreeStructuredParzenEstimator() = delete; - TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev); - void add_trial(const Trial & trial); - [[nodiscard]] Input get_next_input() const; - -private: - static constexpr double max_good_rate = 0.10; - static constexpr int64_t n_ei_candidates = 100; - - static std::mt19937_64 engine; - - [[nodiscard]] double compute_log_likelihood_ratio(const Input & input) const; - [[nodiscard]] static double log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma); - - std::vector trials_; - int64_t above_num_; - const Direction direction_; - const int64_t n_startup_trials_; - const int64_t input_dimension_; - const std::vector sample_mean_; - const std::vector sample_stddev_; - Input base_stddev_; -}; -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_ diff --git a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp b/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp deleted file mode 100644 index bef9968f34b6f..0000000000000 --- a/localization/autoware_localization_util/include/autoware/localization_util/util_func.hpp +++ /dev/null @@ -1,88 +0,0 @@ -// 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__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ -#define AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ - -#include -#include -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x); - -double calc_diff_for_radian(const double lhs_rad, const double rhs_rad); - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad); -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg); - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp); - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); - -template -T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) -{ - T output; - tf2::doTransform(input, output, transform); - return output; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); - -} // namespace autoware::localization_util - -#endif // AUTOWARE__LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/autoware_localization_util/package.xml b/localization/autoware_localization_util/package.xml deleted file mode 100644 index 89ad7c24840dd..0000000000000 --- a/localization/autoware_localization_util/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - - autoware_localization_util - 0.40.0 - The autoware_localization_util package - Yamato Ando - Masahiro Sakamoto - Shintaro Sakoda - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - Yamato Ando - - ament_cmake_auto - autoware_cmake - - diagnostic_msgs - geometry_msgs - rclcpp - std_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - visualization_msgs - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/localization/autoware_localization_util/src/covariance_ellipse.cpp b/localization/autoware_localization_util/src/covariance_ellipse.cpp deleted file mode 100644 index 847f89e0da9b3..0000000000000 --- a/localization/autoware_localization_util/src/covariance_ellipse.cpp +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright 2024 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 "autoware/localization_util/covariance_ellipse.hpp" - -#include - -#include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -namespace autoware::localization_util -{ - -Ellipse calculate_xy_ellipse( - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, const double scale) -{ - // input geometry_msgs::PoseWithCovariance contain 6x6 matrix - Eigen::Matrix2d xy_covariance; - const auto cov = pose_with_covariance.covariance; - xy_covariance(0, 0) = cov[0 * 6 + 0]; - xy_covariance(0, 1) = cov[0 * 6 + 1]; - xy_covariance(1, 0) = cov[1 * 6 + 0]; - xy_covariance(1, 1) = cov[1 * 6 + 1]; - - Eigen::SelfAdjointEigenSolver eigensolver(xy_covariance); - - Ellipse ellipse; - - // eigen values and vectors are sorted in ascending order - ellipse.long_radius = scale * std::sqrt(eigensolver.eigenvalues()(1)); - ellipse.short_radius = scale * std::sqrt(eigensolver.eigenvalues()(0)); - - // principal component vector - const Eigen::Vector2d pc_vector = eigensolver.eigenvectors().col(1); - ellipse.yaw = std::atan2(pc_vector.y(), pc_vector.x()); - - // ellipse size along lateral direction (body-frame) - ellipse.P = xy_covariance; - const double yaw_vehicle = tf2::getYaw(pose_with_covariance.pose.orientation); - const Eigen::Matrix2d & p_inv = ellipse.P.inverse(); - Eigen::MatrixXd e(2, 1); - e(0, 0) = std::cos(yaw_vehicle); - e(1, 0) = std::sin(yaw_vehicle); - const double d = std::sqrt((e.transpose() * p_inv * e)(0, 0) / p_inv.determinant()); - ellipse.size_lateral_direction = scale * d; - - return ellipse; -} - -visualization_msgs::msg::Marker create_ellipse_marker( - const Ellipse & ellipse, const std_msgs::msg::Header & header, - const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance) -{ - tf2::Quaternion quat; - quat.setEuler(0, 0, ellipse.yaw); - - const double ellipse_long_radius = std::min(ellipse.long_radius, 30.0); - const double ellipse_short_radius = std::min(ellipse.short_radius, 30.0); - visualization_msgs::msg::Marker marker; - marker.header = header; - marker.ns = "error_ellipse"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.pose = pose_with_covariance.pose; - marker.pose.orientation = tf2::toMsg(quat); - marker.scale.x = ellipse_long_radius * 2; - marker.scale.y = ellipse_short_radius * 2; - marker.scale.z = 0.01; - marker.color.a = 0.1; - marker.color.r = 0.0; - marker.color.g = 0.0; - marker.color.b = 1.0; - return marker; -} - -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/smart_pose_buffer.cpp b/localization/autoware_localization_util/src/smart_pose_buffer.cpp deleted file mode 100644 index 3b529d68cf6c5..0000000000000 --- a/localization/autoware_localization_util/src/smart_pose_buffer.cpp +++ /dev/null @@ -1,158 +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. - -#include "autoware/localization_util/smart_pose_buffer.hpp" - -namespace autoware::localization_util -{ -SmartPoseBuffer::SmartPoseBuffer( - const rclcpp::Logger & logger, const double & pose_timeout_sec, - const double & pose_distance_tolerance_meters) -: logger_(logger), - pose_timeout_sec_(pose_timeout_sec), - pose_distance_tolerance_meters_(pose_distance_tolerance_meters) -{ -} - -std::optional SmartPoseBuffer::interpolate( - const rclcpp::Time & target_ros_time) -{ - InterpolateResult result; - - { - std::lock_guard lock(mutex_); - - if (pose_buffer_.size() < 2) { - RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); - return std::nullopt; - } - - const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; - const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; - - if (target_ros_time < time_first) { - RCLCPP_INFO(logger_, "Mismatch between pose timestamp and current timestamp"); - return std::nullopt; - } - - // [time_last < target_ros_time] is acceptable here. - // It is possible that the target_ros_time (often sensor timestamp) is newer than the latest - // timestamp of buffered pose (often EKF). - // However, if the timestamp difference is too large, - // it will later be rejected by validate_time_stamp_difference. - - // get the nearest poses - result.old_pose = *pose_buffer_.front(); - for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { - result.new_pose = *pose_cov_msg_ptr; - const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; - if (pose_time_stamp > target_ros_time) { - break; - } - result.old_pose = *pose_cov_msg_ptr; - } - } - - // check the time stamp - const bool is_old_pose_valid = validate_time_stamp_difference( - result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); - const bool is_new_pose_valid = validate_time_stamp_difference( - result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); - - // check the position jumping (ex. immediately after the initial pose estimation) - const bool is_pose_diff_valid = validate_position_difference( - result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, - pose_distance_tolerance_meters_); - - // all validations must be true - if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { - return std::nullopt; - } - - // interpolate the pose - const geometry_msgs::msg::PoseStamped interpolated_pose_msg = - interpolate_pose(result.old_pose, result.new_pose, target_ros_time); - result.interpolated_pose.header = interpolated_pose_msg.header; - result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; - // This does not interpolate the covariance. - // interpolated_pose.pose.covariance is always set to old_pose.covariance - result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; - return result; -} - -void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) -{ - std::lock_guard lock(mutex_); - if (!pose_buffer_.empty()) { - // Check for non-chronological timestamp order - // This situation can arise when replaying a rosbag multiple times - const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; - const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; - if (msg_time < end_time) { - // Clear the buffer if timestamps are reversed to maintain chronological order - pose_buffer_.clear(); - } - } - pose_buffer_.push_back(pose_msg_ptr); -} - -void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) -{ - std::lock_guard lock(mutex_); - while (!pose_buffer_.empty()) { - if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { - break; - } - pose_buffer_.pop_front(); - } -} - -void SmartPoseBuffer::clear() -{ - std::lock_guard lock(mutex_); - pose_buffer_.clear(); -} - -bool SmartPoseBuffer::validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const -{ - const double dt = std::abs((target_time - reference_time).seconds()); - const bool success = dt < time_tolerance_sec; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " - "difference is %lf[sec] (the tolerance is %lf[sec]).", - reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); - } - return success; -} - -bool SmartPoseBuffer::validate_position_difference( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, - const double distance_tolerance_m_) const -{ - const double distance = norm(target_point, reference_point); - const bool success = distance < distance_tolerance_m_; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The distance from reference position to target position is %lf[m] (the " - "tolerance is %lf[m]).", - distance, distance_tolerance_m_); - } - return success; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp b/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp deleted file mode 100644 index e9f0318d1e06d..0000000000000 --- a/localization/autoware_localization_util/src/tree_structured_parzen_estimator.cpp +++ /dev/null @@ -1,185 +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. - -#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::localization_util -{ -// random number generator -std::mt19937_64 TreeStructuredParzenEstimator::engine(0); - -TreeStructuredParzenEstimator::TreeStructuredParzenEstimator( - const Direction direction, const int64_t n_startup_trials, std::vector sample_mean, - std::vector sample_stddev) -: above_num_(0), - direction_(direction), - n_startup_trials_(n_startup_trials), - input_dimension_(INDEX_NUM), - sample_mean_(std::move(sample_mean)), - sample_stddev_(std::move(sample_stddev)) -{ - if (sample_mean_.size() != ANGLE_Z) { - std::cerr << "sample_mean size is invalid" << std::endl; - throw std::runtime_error("sample_mean size is invalid"); - } - if (sample_stddev_.size() != ANGLE_Z) { - std::cerr << "sample_stddev size is invalid" << std::endl; - throw std::runtime_error("sample_stddev size is invalid"); - } - // base_stddev_ is defined as the stable convergence range of ndt_scan_matcher. - base_stddev_.resize(input_dimension_); - base_stddev_[TRANS_X] = 0.25; // [m] - base_stddev_[TRANS_Y] = 0.25; // [m] - base_stddev_[TRANS_Z] = 0.25; // [m] - base_stddev_[ANGLE_X] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Y] = 1.0 / 180.0 * M_PI; // [rad] - base_stddev_[ANGLE_Z] = 2.5 / 180.0 * M_PI; // [rad] -} - -void TreeStructuredParzenEstimator::add_trial(const Trial & trial) -{ - trials_.push_back(trial); - std::sort(trials_.begin(), trials_.end(), [this](const Trial & lhs, const Trial & rhs) { - return (direction_ == Direction::MAXIMIZE ? lhs.score > rhs.score : lhs.score < rhs.score); - }); - above_num_ = std::min( - {static_cast(10), - static_cast(static_cast(trials_.size()) * max_good_rate)}); -} - -TreeStructuredParzenEstimator::Input TreeStructuredParzenEstimator::get_next_input() const -{ - std::normal_distribution dist_normal_trans_x( - sample_mean_[TRANS_X], sample_stddev_[TRANS_X]); - std::normal_distribution dist_normal_trans_y( - sample_mean_[TRANS_Y], sample_stddev_[TRANS_Y]); - std::normal_distribution dist_normal_trans_z( - sample_mean_[TRANS_Z], sample_stddev_[TRANS_Z]); - std::normal_distribution dist_normal_angle_x( - sample_mean_[ANGLE_X], sample_stddev_[ANGLE_X]); - std::normal_distribution dist_normal_angle_y( - sample_mean_[ANGLE_Y], sample_stddev_[ANGLE_Y]); - std::uniform_real_distribution dist_uniform_angle_z(-M_PI, M_PI); - - if (static_cast(trials_.size()) < n_startup_trials_ || above_num_ == 0) { - // Random sampling based on prior until the number of trials reaches `n_startup_trials_`. - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - return input; - } - - Input best_input; - double best_log_likelihood_ratio = std::numeric_limits::lowest(); - for (int64_t i = 0; i < n_ei_candidates; i++) { - Input input(input_dimension_); - input[TRANS_X] = dist_normal_trans_x(engine); - input[TRANS_Y] = dist_normal_trans_y(engine); - input[TRANS_Z] = dist_normal_trans_z(engine); - input[ANGLE_X] = dist_normal_angle_x(engine); - input[ANGLE_Y] = dist_normal_angle_y(engine); - input[ANGLE_Z] = dist_uniform_angle_z(engine); - const double log_likelihood_ratio = compute_log_likelihood_ratio(input); - if (log_likelihood_ratio > best_log_likelihood_ratio) { - best_log_likelihood_ratio = log_likelihood_ratio; - best_input = input; - } - } - return best_input; -} - -double TreeStructuredParzenEstimator::compute_log_likelihood_ratio(const Input & input) const -{ - const auto n = static_cast(trials_.size()); - - // The above KDE and the below KDE are calculated respectively, and the ratio is the criteria to - // select best sample. - std::vector above_logs; - std::vector below_logs; - - for (int64_t i = 0; i < n; i++) { - const double log_p = log_gaussian_pdf(input, trials_[i].input, base_stddev_); - if (i < above_num_) { - const double w = 1.0 / static_cast(above_num_); - const double log_w = std::log(w); - above_logs.push_back(log_p + log_w); - } else { - const double w = 1.0 / static_cast(n - above_num_); - const double log_w = std::log(w); - below_logs.push_back(log_p + log_w); - } - } - - auto log_sum_exp = [](const std::vector & log_vec) { - const double max = *std::max_element(log_vec.begin(), log_vec.end()); - double sum = std::accumulate( - log_vec.begin(), log_vec.end(), 0.0, - [max](double total, double log_v) { return total + std::exp(log_v - max); }); - return max + std::log(sum); - }; - - const double above = log_sum_exp(above_logs); - const double below = log_sum_exp(below_logs); - - // Multiply by a constant so that the score near the "below sample" becomes lower. - // cspell:disable-line TODO(Shintaro Sakoda): It's theoretically incorrect, consider it again - // later. - const double r = above - below * 5.0; - return r; -} - -double TreeStructuredParzenEstimator::log_gaussian_pdf( - const Input & input, const Input & mu, const Input & sigma) -{ - const double log_2pi = std::log(2.0 * M_PI); - auto log_gaussian_pdf_1d = [&](const double diff, const double sigma) { - return -0.5 * log_2pi - std::log(sigma) - (diff * diff) / (2.0 * sigma * sigma); - }; - - const auto n = static_cast(input.size()); - - double result = 0.0; - for (int64_t i = 0; i < n; i++) { - double diff = input[i] - mu[i]; - if (i == ANGLE_Z) { - // Normalize the loop variable to [-pi, pi) - while (diff >= M_PI) { - diff -= 2 * M_PI; - } - while (diff < -M_PI) { - diff += 2 * M_PI; - } - } - // Experimentally, it is better to consider only trans_xy and yaw, so ignore trans_z, angle_x, - // angle_y. - if (i == TRANS_Z || i == ANGLE_X || i == ANGLE_Y) { - continue; - } - result += log_gaussian_pdf_1d(diff, sigma[i]); - } - return result; -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/src/util_func.cpp b/localization/autoware_localization_util/src/util_func.cpp deleted file mode 100644 index 17187a8d732f0..0000000000000 --- a/localization/autoware_localization_util/src/util_func.cpp +++ /dev/null @@ -1,257 +0,0 @@ -// 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. - -#include "autoware/localization_util/util_func.hpp" - -#include "autoware/localization_util/matrix_type.hpp" - -#include -#include - -namespace autoware::localization_util -{ -// ref by http://takacity.blog.fc2.com/blog-entry-69.html -std_msgs::msg::ColorRGBA exchange_color_crc(double x) -{ - std_msgs::msg::ColorRGBA color; - - x = std::max(x, 0.0); - x = std::min(x, 0.9999); - - if (x <= 0.25) { - color.b = 1.0; - color.g = static_cast(std::sin(x * 2.0 * M_PI)); - color.r = 0; - } 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.75) { - color.b = 0; - color.g = 1.0; - color.r = static_cast(-std::sin(x * 2.0 * M_PI)); - } else { - color.b = 0; - color.g = static_cast(-std::sin(x * 2.0 * M_PI)); - color.r = 1.0; - } - color.a = 0.999; - return color; -} - -double calc_diff_for_radian(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; -} - -Eigen::Map make_eigen_covariance(const std::array & covariance) -{ - return {covariance.data(), 6, 6}; -} - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) -{ - 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; -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose) -{ - return get_rpy(pose.pose); -} - -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - return get_rpy(pose.pose.pose); -} - -geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( - const double r_rad, const double p_rad, const double y_rad) -{ - tf2::Quaternion q; - q.setRPY(r_rad, p_rad, y_rad); - geometry_msgs::msg::Quaternion quaternion_msg; - quaternion_msg.x = q.x(); - quaternion_msg.y = q.y(); - quaternion_msg.z = q.z(); - quaternion_msg.w = q.w(); - return quaternion_msg; -} - -geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( - const double r_deg, const double p_deg, const double y_deg) -{ - const double r_rad = r_deg * M_PI / 180.0; - const double p_rad = p_deg * M_PI / 180.0; - const double y_rad = y_deg * M_PI / 180.0; - return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); -} - -geometry_msgs::msg::Twist calc_twist( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) -{ - const rclcpp::Duration dt = rclcpp::Time(pose_b.header.stamp) - rclcpp::Time(pose_a.header.stamp); - const double dt_s = dt.seconds(); - - if (dt_s == 0) { - return geometry_msgs::msg::Twist(); - } - - const auto pose_a_rpy = get_rpy(pose_a); - const auto pose_b_rpy = get_rpy(pose_b); - - geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; - - 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 = calc_diff_for_radian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calc_diff_for_radian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calc_diff_for_radian(pose_b_rpy.z, pose_a_rpy.z); - - geometry_msgs::msg::Twist twist; - twist.linear.x = diff_xyz.x / dt_s; - twist.linear.y = diff_xyz.y / dt_s; - twist.linear.z = diff_xyz.z / dt_s; - twist.angular.x = diff_rpy.x / dt_s; - twist.angular.y = diff_rpy.y / dt_s; - twist.angular.z = diff_rpy.z / dt_s; - - return twist; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, - const rclcpp::Time & time_stamp) -{ - const rclcpp::Time pose_a_time_stamp = pose_a.header.stamp; - const rclcpp::Time pose_b_time_stamp = pose_b.header.stamp; - if ( - (pose_a_time_stamp.seconds() == 0.0) || (pose_b_time_stamp.seconds() == 0.0) || - (time_stamp.seconds() == 0.0)) { - return geometry_msgs::msg::PoseStamped(); - } - - const auto twist = calc_twist(pose_a, pose_b); - const double dt = (time_stamp - pose_a_time_stamp).seconds(); - - const auto pose_a_rpy = get_rpy(pose_a); - - geometry_msgs::msg::Vector3 xyz; - geometry_msgs::msg::Vector3 rpy; - xyz.x = pose_a.pose.position.x + twist.linear.x * dt; - xyz.y = pose_a.pose.position.y + twist.linear.y * dt; - xyz.z = pose_a.pose.position.z + twist.linear.z * dt; - rpy.x = pose_a_rpy.x + twist.angular.x * dt; - rpy.y = pose_a_rpy.y + twist.angular.y * dt; - rpy.z = pose_a_rpy.z + twist.angular.z * dt; - - tf2::Quaternion tf_quaternion; - tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); - - geometry_msgs::msg::PoseStamped pose; - pose.header = pose_a.header; - pose.header.stamp = time_stamp; - pose.pose.position.x = xyz.x; - pose.pose.position.y = xyz.y; - pose.pose.position.z = xyz.z; - pose.pose.orientation = tf2::toMsg(tf_quaternion); - return pose; -} - -geometry_msgs::msg::PoseStamped interpolate_pose( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp) -{ - geometry_msgs::msg::PoseStamped tmp_pose_a; - tmp_pose_a.header = pose_a.header; - tmp_pose_a.pose = pose_a.pose.pose; - - geometry_msgs::msg::PoseStamped tmp_pose_b; - tmp_pose_b.header = pose_b.header; - tmp_pose_b.pose = pose_b.pose.pose; - - return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); -} - -Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose; - tf2::fromMsg(ros_pose, eigen_pose); - return eigen_pose; -} - -Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose) -{ - Eigen::Affine3d eigen_pose_affine = pose_to_affine3d(ros_pose); - Eigen::Matrix4f eigen_pose_matrix = eigen_pose_affine.matrix().cast(); - return eigen_pose_matrix; -} - -Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos) -{ - Eigen::Vector3d eigen_pos; - eigen_pos.x() = ros_pos.x; - eigen_pos.y() = ros_pos.y; - eigen_pos.z() = ros_pos.z; - return eigen_pos; -} - -geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix) -{ - Eigen::Affine3d eigen_pose_affine; - eigen_pose_affine.matrix() = eigen_pose_matrix.cast(); - geometry_msgs::msg::Pose ros_pose = tf2::toMsg(eigen_pose_affine); - return ros_pose; -} - -double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - const double dx = p1.x - p2.x; - const double dy = p1.y - p2.y; - const double dz = p1.z - p2.z; - return std::sqrt(dx * dx + dy * dy + dz * dz); -} - -void output_pose_with_cov_to_log( - const rclcpp::Logger & logger, const std::string & prefix, - const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) -{ - const Eigen::Map covariance = - make_eigen_covariance(pose_with_cov.pose.covariance); - const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; - geometry_msgs::msg::Vector3 rpy = get_rpy(pose); - rpy.x = rpy.x * 180.0 / M_PI; - rpy.y = rpy.y * 180.0 / M_PI; - rpy.z = rpy.z * 180.0 / M_PI; - - RCLCPP_DEBUG_STREAM( - logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," - << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y - << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x - << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," - << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) - << "," << covariance(4, 4) << "," << covariance(5, 5)); -} -} // namespace autoware::localization_util diff --git a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp b/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp deleted file mode 100644 index d55555682be84..0000000000000 --- a/localization/autoware_localization_util/test/test_smart_pose_buffer.cpp +++ /dev/null @@ -1,109 +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. - -#include "autoware/localization_util/smart_pose_buffer.hpp" -#include "autoware/localization_util/util_func.hpp" - -#include -#include - -#include -#include - -#include -#include -#include - -using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -using SmartPoseBuffer = autoware::localization_util::SmartPoseBuffer; - -bool compare_pose( - const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) -{ - return pose_a.header.stamp == pose_b.header.stamp && - pose_a.header.frame_id == pose_b.header.frame_id && - pose_a.pose.covariance == pose_b.pose.covariance && - pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && - pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && - pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && - pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && - pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && - pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && - pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; -} - -TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT -{ - rclcpp::Logger logger = rclcpp::get_logger("test_logger"); - const double pose_timeout_sec = 10.0; - const double pose_distance_tolerance_meters = 100.0; - SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); - - // first data - PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); - old_pose_ptr->header.stamp.sec = 10; - old_pose_ptr->header.stamp.nanosec = 0; - old_pose_ptr->pose.pose.position.x = 10.0; - old_pose_ptr->pose.pose.position.y = 20.0; - old_pose_ptr->pose.pose.position.z = 0.0; - old_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 0.0); - smart_pose_buffer.push_back(old_pose_ptr); - - // second data - PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); - new_pose_ptr->header.stamp.sec = 20; - new_pose_ptr->header.stamp.nanosec = 0; - new_pose_ptr->pose.pose.position.x = 20.0; - new_pose_ptr->pose.pose.position.y = 40.0; - new_pose_ptr->pose.pose.position.z = 0.0; - new_pose_ptr->pose.pose.orientation = - autoware::localization_util::rpy_deg_to_quaternion(0.0, 0.0, 90.0); - smart_pose_buffer.push_back(new_pose_ptr); - - // interpolate - builtin_interfaces::msg::Time target_ros_time_msg; - target_ros_time_msg.sec = 15; - target_ros_time_msg.nanosec = 0; - const std::optional & interpolate_result = - smart_pose_buffer.interpolate(target_ros_time_msg); - ASSERT_TRUE(interpolate_result.has_value()); - const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); - - // check old - EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); - - // check new - EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); - - // check interpolated - EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); - EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); - EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); - const auto rpy = autoware::localization_util::get_rpy(result.interpolated_pose.pose.pose); - EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); - EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - int result = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return result; -} diff --git a/localization/autoware_localization_util/test/test_tpe.cpp b/localization/autoware_localization_util/test/test_tpe.cpp deleted file mode 100644 index 2d71a385246b7..0000000000000 --- a/localization/autoware_localization_util/test/test_tpe.cpp +++ /dev/null @@ -1,75 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "autoware/localization_util/tree_structured_parzen_estimator.hpp" - -#include - -#include -#include -#include -#include -#include - -using TreeStructuredParzenEstimator = autoware::localization_util::TreeStructuredParzenEstimator; - -TEST(TreeStructuredParzenEstimatorTest, TPE_is_better_than_random_search_on_sphere_function) -{ - auto sphere_function = [](const TreeStructuredParzenEstimator::Input & input) { - double value = 0.0; - const auto n = static_cast(input.size()); - for (int64_t i = 0; i < n; i++) { - const double v = input[i] * 10; - value += v * v; - } - return value; - }; - - constexpr int64_t k_outer_trials_num = 20; - constexpr int64_t k_inner_trials_num = 200; - std::cout << std::fixed; - std::vector mean_scores; - std::vector sample_mean(5, 0.0); - std::vector sample_stddev{1.0, 1.0, 0.1, 0.1, 0.1}; - - for (const int64_t n_startup_trials : {k_inner_trials_num, k_inner_trials_num / 2}) { - const std::string method = ((n_startup_trials == k_inner_trials_num) ? "Random" : "TPE"); - - std::vector scores; - for (int64_t i = 0; i < k_outer_trials_num; i++) { - double best_score = std::numeric_limits::lowest(); - TreeStructuredParzenEstimator estimator( - TreeStructuredParzenEstimator::Direction::MAXIMIZE, n_startup_trials, sample_mean, - sample_stddev); - for (int64_t trial = 0; trial < k_inner_trials_num; trial++) { - const TreeStructuredParzenEstimator::Input input = estimator.get_next_input(); - const double score = -sphere_function(input); - estimator.add_trial({input, score}); - best_score = std::max(best_score, score); - } - scores.push_back(best_score); - } - - const double sum = std::accumulate(scores.begin(), scores.end(), 0.0); - const double mean = sum / static_cast(scores.size()); - mean_scores.push_back(mean); - double sq_sum = std::accumulate( - scores.begin(), scores.end(), 0.0, - [mean](double total, double score) { return total + (score - mean) * (score - mean); }); - const double stddev = std::sqrt(sq_sum / static_cast(scores.size())); - - std::cout << method << ", mean = " << mean << ", stddev = " << stddev << std::endl; - } - ASSERT_LT(mean_scores[0], mean_scores[1]); -} diff --git a/localization/autoware_ndt_scan_matcher/CHANGELOG.rst b/localization/autoware_ndt_scan_matcher/CHANGELOG.rst index 8040641aa2cef..c9ef88ac152f5 100644 --- a/localization/autoware_ndt_scan_matcher/CHANGELOG.rst +++ b/localization/autoware_ndt_scan_matcher/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package autoware_ndt_scan_matcher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware-internal_debug_msgs in files localization/autoware_ndt_scan_matcher (`#9861 `_) + Co-authored-by: SakodaShintaro +* refactor(autoware_universe_utils): add missing 's' in the class of diagnostics_interface (`#9777 `_) +* feat!: move diagnostics_module from localization_util to unverse_utils (`#9714 `_) + * feat!: move diagnostics_module from localization_util to unverse_utils + * remove diagnostics module from localization_util + * style(pre-commit): autofix + * minor fix in pose_initializer + * add test + * style(pre-commit): autofix + * remove unnecessary declaration + * module -> interface + * remove unnecessary equal expression + * revert the remove of template function + * style(pre-commit): autofix + * use overload instead + * include what you use -- test_diagnostics_interface.cpp + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Vishal Chauhan, kminoda + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_ndt_scan_matcher/README.md b/localization/autoware_ndt_scan_matcher/README.md index 22e56930a0048..ee177608bdefe 100644 --- a/localization/autoware_ndt_scan_matcher/README.md +++ b/localization/autoware_ndt_scan_matcher/README.md @@ -25,26 +25,26 @@ One optional function is regularization. Please see the regularization chapter i ### Output -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | -| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | -| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | -| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | -| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | -| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | -| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | -| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | -| `exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | -| `transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | -| `no_ground_transform_probability` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | -| `iteration_num` | `tier4_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | -| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | -| `initial_to_result_distance` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | -| `initial_to_result_distance_old` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | -| `initial_to_result_distance_new` | `tier4_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | -| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | -| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | +| Name | Type | Description | +| --------------------------------- | --------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- | +| `ndt_pose` | `geometry_msgs::msg::PoseStamped` | estimated pose | +| `ndt_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | estimated pose with covariance | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| `points_aligned` | `sensor_msgs::msg::PointCloud2` | [debug topic] pointcloud aligned by scan matching | +| `points_aligned_no_ground` | `sensor_msgs::msg::PointCloud2` | [debug topic] no ground pointcloud aligned by scan matching | +| `initial_pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | [debug topic] initial pose used in scan matching | +| `multi_ndt_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] estimated poses from multiple initial poses in real-time covariance estimation | +| `multi_initial_pose` | `geometry_msgs::msg::PoseArray` | [debug topic] initial poses for real-time covariance estimation | +| `exe_time_ms` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] execution time for scan matching [ms] | +| `transform_probability` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching | +| `no_ground_transform_probability` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] score of scan matching based on no ground LiDAR scan | +| `iteration_num` | `autoware_internal_debug_msgs::msg::Int32Stamped` | [debug topic] number of scan matching iterations | +| `initial_to_result_relative_pose` | `geometry_msgs::msg::PoseStamped` | [debug topic] relative pose between the initial point and the convergence point | +| `initial_to_result_distance` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the initial point and the convergence point [m] | +| `initial_to_result_distance_old` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the older of the two initial points used in linear interpolation and the convergence point [m] | +| `initial_to_result_distance_new` | `autoware_internal_debug_msgs::msg::Float32Stamped` | [debug topic] distance difference between the newer of the two initial points used in linear interpolation and the convergence point [m] | +| `ndt_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] markers for debugging | +| `monte_carlo_initial_pose_marker` | `visualization_msgs::msg::MarkerArray` | [debug topic] particles used in initial position estimation | ### Service diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp index d2dce42e3923e..12990259f88cd 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/map_update_module.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ #define AUTOWARE__NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/util_func.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "ndt_omp/multigrid_ndt_omp.h" #include "particle.hpp" @@ -42,7 +42,7 @@ namespace autoware::ndt_scan_matcher { -using DiagnosticsModule = autoware::localization_util::DiagnosticsModule; +using DiagnosticsInterface = autoware::universe_utils::DiagnosticsInterface; class MapUpdateModule { @@ -63,19 +63,19 @@ class MapUpdateModule void callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); [[nodiscard]] bool should_update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void update_map( const geometry_msgs::msg::Point & position, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); // Update the specified NDT bool update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr); + std::unique_ptr & diagnostics_ptr); void publish_partial_pcd_map(); rclcpp::Publisher::SharedPtr loaded_pcd_pub_; diff --git a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 22b6bfb2ff740..d6e629ee2f1c3 100644 --- a/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/autoware_ndt_scan_matcher/include/autoware/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,8 +17,8 @@ #define FMT_HEADER_ONLY -#include "autoware/localization_util/diagnostics_module.hpp" #include "autoware/localization_util/smart_pose_buffer.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "hyper_parameters.hpp" #include "map_update_module.hpp" #include "ndt_omp/multigrid_ndt_omp.h" @@ -26,6 +26,8 @@ #include #include +#include +#include #include #include #include @@ -33,8 +35,6 @@ #include #include #include -#include -#include #include #include @@ -166,23 +166,24 @@ class NDTScanMatcher : public rclcpp::Node initial_pose_with_covariance_pub_; rclcpp::Publisher::SharedPtr multi_ndt_pose_pub_; rclcpp::Publisher::SharedPtr multi_initial_pose_pub_; - rclcpp::Publisher::SharedPtr exe_time_pub_; - rclcpp::Publisher::SharedPtr transform_probability_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr exe_time_pub_; + rclcpp::Publisher::SharedPtr + transform_probability_pub_; + rclcpp::Publisher::SharedPtr nearest_voxel_transformation_likelihood_pub_; rclcpp::Publisher::SharedPtr voxel_score_points_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr no_ground_transform_probability_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr no_ground_nearest_voxel_transformation_likelihood_pub_; - rclcpp::Publisher::SharedPtr iteration_num_pub_; + rclcpp::Publisher::SharedPtr iteration_num_pub_; rclcpp::Publisher::SharedPtr initial_to_result_relative_pose_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_old_pub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr initial_to_result_distance_new_pub_; rclcpp::Publisher::SharedPtr ndt_marker_pub_; rclcpp::Publisher::SharedPtr @@ -211,12 +212,12 @@ class NDTScanMatcher : public rclcpp::Node std::unique_ptr regularization_pose_buffer_; std::atomic is_activated_; - std::unique_ptr diagnostics_scan_points_; - std::unique_ptr diagnostics_initial_pose_; - std::unique_ptr diagnostics_regularization_pose_; - std::unique_ptr diagnostics_map_update_; - std::unique_ptr diagnostics_ndt_align_; - std::unique_ptr diagnostics_trigger_node_; + std::unique_ptr diagnostics_scan_points_; + std::unique_ptr diagnostics_initial_pose_; + std::unique_ptr diagnostics_regularization_pose_; + std::unique_ptr diagnostics_map_update_; + std::unique_ptr diagnostics_ndt_align_; + std::unique_ptr diagnostics_trigger_node_; std::unique_ptr map_update_module_; std::unique_ptr logger_configure_; diff --git a/localization/autoware_ndt_scan_matcher/package.xml b/localization/autoware_ndt_scan_matcher/package.xml index f2047418310aa..bdcee3a98f043 100644 --- a/localization/autoware_ndt_scan_matcher/package.xml +++ b/localization/autoware_ndt_scan_matcher/package.xml @@ -2,7 +2,7 @@ autoware_ndt_scan_matcher - 0.40.0 + 0.41.0 The autoware_ndt_scan_matcher package Yamato Ando Kento Yabuuchi @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_localization_util autoware_map_msgs autoware_universe_utils @@ -35,7 +36,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - tier4_debug_msgs tier4_localization_msgs visualization_msgs diff --git a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp index 25b51b55aebd7..299d596401b19 100644 --- a/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp +++ b/localization/autoware_ndt_scan_matcher/src/map_update_module.cpp @@ -56,7 +56,7 @@ MapUpdateModule::MapUpdateModule( void MapUpdateModule::callback_timer( const bool is_activated, const std::optional & position, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { // check is_activated diagnostics_ptr->add_key_value("is_activated", is_activated); @@ -86,7 +86,8 @@ void MapUpdateModule::callback_timer( } bool MapUpdateModule::should_update_map( - const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) + const geometry_msgs::msg::Point & position, + std::unique_ptr & diagnostics_ptr) { last_update_position_mtx_.lock(); @@ -141,7 +142,8 @@ bool MapUpdateModule::out_of_map_range(const geometry_msgs::msg::Point & positio } void MapUpdateModule::update_map( - const geometry_msgs::msg::Point & position, std::unique_ptr & diagnostics_ptr) + const geometry_msgs::msg::Point & position, + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("is_need_rebuild", need_rebuild_); @@ -229,7 +231,7 @@ void MapUpdateModule::update_map( bool MapUpdateModule::update_ndt( const geometry_msgs::msg::Point & position, NdtType & ndt, - std::unique_ptr & diagnostics_ptr) + std::unique_ptr & diagnostics_ptr) { diagnostics_ptr->add_key_value("maps_size_before", ndt.getCurrentMapIDs().size()); diff --git a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index a1871023d525b..3d06dfffa16ed 100644 --- a/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/autoware_ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -49,22 +49,22 @@ using autoware::localization_util::matrix4f_to_pose; using autoware::localization_util::point_to_vector3d; using autoware::localization_util::pose_to_matrix4f; -using autoware::localization_util::DiagnosticsModule; using autoware::localization_util::SmartPoseBuffer; using autoware::localization_util::TreeStructuredParzenEstimator; +using autoware::universe_utils::DiagnosticsInterface; -tier4_debug_msgs::msg::Float32Stamped make_float32_stamped( +autoware_internal_debug_msgs::msg::Float32Stamped make_float32_stamped( const builtin_interfaces::msg::Time & stamp, const float data) { - using T = tier4_debug_msgs::msg::Float32Stamped; - return tier4_debug_msgs::build().stamp(stamp).data(data); + using T = autoware_internal_debug_msgs::msg::Float32Stamped; + return autoware_internal_debug_msgs::build().stamp(stamp).data(data); } -tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( +autoware_internal_debug_msgs::msg::Int32Stamped make_int32_stamped( const builtin_interfaces::msg::Time & stamp, const int32_t data) { - using T = tier4_debug_msgs::msg::Int32Stamped; - return tier4_debug_msgs::build().stamp(stamp).data(data); + using T = autoware_internal_debug_msgs::msg::Int32Stamped; + return autoware_internal_debug_msgs::build().stamp(stamp).data(data); } std::array rotate_covariance( @@ -141,7 +141,7 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); diagnostics_regularization_pose_ = - std::make_unique(this, "regularization_pose_subscriber_status"); + std::make_unique(this, "regularization_pose_subscriber_status"); } sensor_aligned_pose_pub_ = @@ -158,31 +158,34 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) multi_ndt_pose_pub_ = this->create_publisher("multi_ndt_pose", 10); multi_initial_pose_pub_ = this->create_publisher("multi_initial_pose", 10); - exe_time_pub_ = this->create_publisher("exe_time_ms", 10); + exe_time_pub_ = + this->create_publisher("exe_time_ms", 10); transform_probability_pub_ = - this->create_publisher("transform_probability", 10); + this->create_publisher( + "transform_probability", 10); nearest_voxel_transformation_likelihood_pub_ = - this->create_publisher( + this->create_publisher( "nearest_voxel_transformation_likelihood", 10); voxel_score_points_pub_ = this->create_publisher("voxel_score_points", 10); no_ground_transform_probability_pub_ = - this->create_publisher( + this->create_publisher( "no_ground_transform_probability", 10); no_ground_nearest_voxel_transformation_likelihood_pub_ = - this->create_publisher( + this->create_publisher( "no_ground_nearest_voxel_transformation_likelihood", 10); iteration_num_pub_ = - this->create_publisher("iteration_num", 10); + this->create_publisher("iteration_num", 10); initial_to_result_relative_pose_pub_ = this->create_publisher("initial_to_result_relative_pose", 10); initial_to_result_distance_pub_ = - this->create_publisher("initial_to_result_distance", 10); + this->create_publisher( + "initial_to_result_distance", 10); initial_to_result_distance_old_pub_ = - this->create_publisher( + this->create_publisher( "initial_to_result_distance_old", 10); initial_to_result_distance_new_pub_ = - this->create_publisher( + this->create_publisher( "initial_to_result_distance_new", 10); ndt_marker_pub_ = this->create_publisher("ndt_marker", 10); ndt_monte_carlo_initial_pose_marker_pub_ = @@ -209,13 +212,13 @@ NDTScanMatcher::NDTScanMatcher(const rclcpp::NodeOptions & options) map_update_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, param_.dynamic_map_loading); - diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); + diagnostics_scan_points_ = std::make_unique(this, "scan_matching_status"); diagnostics_initial_pose_ = - std::make_unique(this, "initial_pose_subscriber_status"); - diagnostics_map_update_ = std::make_unique(this, "map_update_status"); - diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); + std::make_unique(this, "initial_pose_subscriber_status"); + diagnostics_map_update_ = std::make_unique(this, "map_update_status"); + diagnostics_ndt_align_ = std::make_unique(this, "ndt_align_service_status"); diagnostics_trigger_node_ = - std::make_unique(this, "trigger_node_service_status"); + std::make_unique(this, "trigger_node_service_status"); logger_configure_ = std::make_unique(this); } diff --git a/localization/autoware_pose2twist/CHANGELOG.rst b/localization/autoware_pose2twist/CHANGELOG.rst index 3f17982f52f31..497a2f0cebc6a 100644 --- a/localization/autoware_pose2twist/CHANGELOG.rst +++ b/localization/autoware_pose2twist/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_pose2twist ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files localization/autoware_pose2twist (`#9866 `_) +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/localization/autoware_pose2twist/README.md b/localization/autoware_pose2twist/README.md index 55ca4667c423d..7f81e0dece5b0 100644 --- a/localization/autoware_pose2twist/README.md +++ b/localization/autoware_pose2twist/README.md @@ -17,11 +17,11 @@ The `twist.angular` is calculated as `relative_rotation_vector / dt` for each fi ### Output -| Name | Type | Description | -| --------- | ------------------------------------- | --------------------------------------------- | -| twist | geometry_msgs::msg::TwistStamped | twist calculated from the input pose history. | -| linear_x | tier4_debug_msgs::msg::Float32Stamped | linear-x field of the output twist. | -| angular_z | tier4_debug_msgs::msg::Float32Stamped | angular-z field of the output twist. | +| Name | Type | Description | +| --------- | ------------------------------------------------- | --------------------------------------------- | +| twist | geometry_msgs::msg::TwistStamped | twist calculated from the input pose history. | +| linear_x | autoware_internal_debug_msgs::msg::Float32Stamped | linear-x field of the output twist. | +| angular_z | autoware_internal_debug_msgs::msg::Float32Stamped | angular-z field of the output twist. | ## Parameters diff --git a/localization/autoware_pose2twist/package.xml b/localization/autoware_pose2twist/package.xml index 3fd16856ef6af..88e046f2e5cad 100644 --- a/localization/autoware_pose2twist/package.xml +++ b/localization/autoware_pose2twist/package.xml @@ -2,7 +2,7 @@ autoware_pose2twist - 0.40.0 + 0.41.0 The autoware_pose2twist package Yamato Ando Masahiro Sakamoto @@ -17,11 +17,11 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs geometry_msgs rclcpp rclcpp_components tf2_geometry_msgs - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/localization/autoware_pose2twist/src/pose2twist_core.cpp b/localization/autoware_pose2twist/src/pose2twist_core.cpp index 4dc7b5fb04209..f071d7c8c66e2 100644 --- a/localization/autoware_pose2twist/src/pose2twist_core.cpp +++ b/localization/autoware_pose2twist/src/pose2twist_core.cpp @@ -29,9 +29,10 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose durable_qos.transient_local(); twist_pub_ = create_publisher("twist", durable_qos); - linear_x_pub_ = create_publisher("linear_x", durable_qos); + linear_x_pub_ = + create_publisher("linear_x", durable_qos); angular_z_pub_ = - create_publisher("angular_z", durable_qos); + create_publisher("angular_z", durable_qos); // Note: this callback publishes topics above pose_sub_ = create_subscription( "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1)); @@ -105,12 +106,12 @@ void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_m twist_msg.header.frame_id = "base_link"; twist_pub_->publish(twist_msg); - tier4_debug_msgs::msg::Float32Stamped linear_x_msg; + autoware_internal_debug_msgs::msg::Float32Stamped linear_x_msg; linear_x_msg.stamp = this->now(); 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; + autoware_internal_debug_msgs::msg::Float32Stamped angular_z_msg; angular_z_msg.stamp = this->now(); angular_z_msg.data = static_cast(twist_msg.twist.angular.z); angular_z_pub_->publish(angular_z_msg); diff --git a/localization/autoware_pose2twist/src/pose2twist_core.hpp b/localization/autoware_pose2twist/src/pose2twist_core.hpp index ed3e542beb857..22540c9aee473 100644 --- a/localization/autoware_pose2twist/src/pose2twist_core.hpp +++ b/localization/autoware_pose2twist/src/pose2twist_core.hpp @@ -17,9 +17,9 @@ #include +#include #include #include -#include #ifdef ROS_DISTRO_GALACTIC #include @@ -44,8 +44,8 @@ class Pose2Twist : public rclcpp::Node rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Publisher::SharedPtr twist_pub_; - rclcpp::Publisher::SharedPtr linear_x_pub_; - rclcpp::Publisher::SharedPtr angular_z_pub_; + rclcpp::Publisher::SharedPtr linear_x_pub_; + rclcpp::Publisher::SharedPtr angular_z_pub_; }; } // namespace autoware::pose2twist diff --git a/localization/autoware_pose_covariance_modifier/CHANGELOG.rst b/localization/autoware_pose_covariance_modifier/CHANGELOG.rst index 6fc366d3d01d5..8861c5725883c 100644 --- a/localization/autoware_pose_covariance_modifier/CHANGELOG.rst +++ b/localization/autoware_pose_covariance_modifier/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package autoware_pose_covariance_modifier ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* revert: revert "feat(autoware_ekf_localizer)!: porting from universe to core (`#9978 `_)" (`#10004 `_) + This reverts commit 037c315fbee69bb5923ec10bb8e8e70f890725ea. +* feat(autoware_ekf_localizer)!: porting from universe to core (`#9978 `_) + * feat: delete ekf_localizer files + * doc: Modify ekf_localizer directory links + * ci: remove ekf_localizer from the codecov target list + --------- +* Contributors: Motz, Ryohsuke Mitsudome + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_pose_covariance_modifier/package.xml b/localization/autoware_pose_covariance_modifier/package.xml index 7825bd59b78f9..49bf3362983d8 100644 --- a/localization/autoware_pose_covariance_modifier/package.xml +++ b/localization/autoware_pose_covariance_modifier/package.xml @@ -2,7 +2,7 @@ autoware_pose_covariance_modifier - 0.40.0 + 0.41.0 Add a description. Melike Tanrikulu diff --git a/localization/autoware_pose_estimator_arbiter/CHANGELOG.rst b/localization/autoware_pose_estimator_arbiter/CHANGELOG.rst index 381b77546e008..7fabd13c0f3ff 100644 --- a/localization/autoware_pose_estimator_arbiter/CHANGELOG.rst +++ b/localization/autoware_pose_estimator_arbiter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_pose_estimator_arbiter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_pose_estimator_arbiter/package.xml b/localization/autoware_pose_estimator_arbiter/package.xml index c6a47ae5755b3..b9ffae2137037 100644 --- a/localization/autoware_pose_estimator_arbiter/package.xml +++ b/localization/autoware_pose_estimator_arbiter/package.xml @@ -2,7 +2,7 @@ autoware_pose_estimator_arbiter - 0.40.0 + 0.41.0 The arbiter of multiple pose estimators Yamato Ando Kento Yabuuchi diff --git a/localization/autoware_pose_initializer/CHANGELOG.rst b/localization/autoware_pose_initializer/CHANGELOG.rst index 45473a9444270..b89892adb13a8 100644 --- a/localization/autoware_pose_initializer/CHANGELOG.rst +++ b/localization/autoware_pose_initializer/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package autoware_pose_initializer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `default_ad_api_helpers` (`#9965 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Takagi, Isamu +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* refactor(autoware_universe_utils): add missing 's' in the class of diagnostics_interface (`#9777 `_) +* feat!: move diagnostics_module from localization_util to unverse_utils (`#9714 `_) + * feat!: move diagnostics_module from localization_util to unverse_utils + * remove diagnostics module from localization_util + * style(pre-commit): autofix + * minor fix in pose_initializer + * add test + * style(pre-commit): autofix + * remove unnecessary declaration + * module -> interface + * remove unnecessary equal expression + * revert the remove of template function + * style(pre-commit): autofix + * use overload instead + * include what you use -- test_diagnostics_interface.cpp + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki, Ryohsuke Mitsudome, kminoda + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_pose_initializer/README.md b/localization/autoware_pose_initializer/README.md index 2d8f0343f3493..d48520fa7ea30 100644 --- a/localization/autoware_pose_initializer/README.md +++ b/localization/autoware_pose_initializer/README.md @@ -137,4 +137,4 @@ pose: ``` It behaves the same as "initialpose (from rviz)". -The position.z and the covariance will be overwritten by [ad_api_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/default_ad_api_helpers/ad_api_adaptors), so there is no need to input them. +The position.z and the covariance will be overwritten by [autoware_adapi_adaptors](https://github.com/autowarefoundation/autoware.universe/tree/main/system/autoware_default_adapi_helpers/autoware_adapi_adaptors), so there is no need to input them. diff --git a/localization/autoware_pose_initializer/package.xml b/localization/autoware_pose_initializer/package.xml index eeeb46a8db3e5..cff9fc1fec3e5 100644 --- a/localization/autoware_pose_initializer/package.xml +++ b/localization/autoware_pose_initializer/package.xml @@ -2,7 +2,7 @@ autoware_pose_initializer - 0.40.0 + 0.41.0 The autoware_pose_initializer package Yamato Ando Takagi, Isamu @@ -19,9 +19,8 @@ ament_cmake autoware_cmake - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils - autoware_localization_util autoware_map_height_fitter autoware_motion_utils autoware_universe_utils diff --git a/localization/autoware_pose_initializer/src/ekf_localization_trigger_module.cpp b/localization/autoware_pose_initializer/src/ekf_localization_trigger_module.cpp index 089b941141074..0325a3d9a5e94 100644 --- a/localization/autoware_pose_initializer/src/ekf_localization_trigger_module.cpp +++ b/localization/autoware_pose_initializer/src/ekf_localization_trigger_module.cpp @@ -14,7 +14,7 @@ #include "ekf_localization_trigger_module.hpp" -#include +#include #include #include @@ -23,7 +23,7 @@ namespace autoware::pose_initializer { using ServiceException = autoware::component_interface_utils::ServiceException; -using Initialize = autoware::component_interface_specs::localization::Initialize; +using Initialize = autoware::component_interface_specs_universe::localization::Initialize; EkfLocalizationTriggerModule::EkfLocalizationTriggerModule(rclcpp::Node * node) : node_(node) { diff --git a/localization/autoware_pose_initializer/src/gnss_module.cpp b/localization/autoware_pose_initializer/src/gnss_module.cpp index b238100a764e7..3ef12ef66f0c8 100644 --- a/localization/autoware_pose_initializer/src/gnss_module.cpp +++ b/localization/autoware_pose_initializer/src/gnss_module.cpp @@ -14,7 +14,7 @@ #include "gnss_module.hpp" -#include +#include #include #include @@ -37,7 +37,7 @@ void GnssModule::on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg) geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose() { - using Initialize = autoware::component_interface_specs::localization::Initialize; + using Initialize = autoware::component_interface_specs_universe::localization::Initialize; if (!pose_) { throw autoware::component_interface_utils::ServiceException( diff --git a/localization/autoware_pose_initializer/src/localization_module.cpp b/localization/autoware_pose_initializer/src/localization_module.cpp index e3fdeec6934fe..e38e8f14c6dd4 100644 --- a/localization/autoware_pose_initializer/src/localization_module.cpp +++ b/localization/autoware_pose_initializer/src/localization_module.cpp @@ -14,7 +14,7 @@ #include "localization_module.hpp" -#include +#include #include #include @@ -24,7 +24,7 @@ namespace autoware::pose_initializer { using ServiceException = autoware::component_interface_utils::ServiceException; -using Initialize = autoware::component_interface_specs::localization::Initialize; +using Initialize = autoware::component_interface_specs_universe::localization::Initialize; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; LocalizationModule::LocalizationModule(rclcpp::Node * node, const std::string & service_name) diff --git a/localization/autoware_pose_initializer/src/ndt_localization_trigger_module.cpp b/localization/autoware_pose_initializer/src/ndt_localization_trigger_module.cpp index f66fbe72333ac..fa3c88d04e7bb 100644 --- a/localization/autoware_pose_initializer/src/ndt_localization_trigger_module.cpp +++ b/localization/autoware_pose_initializer/src/ndt_localization_trigger_module.cpp @@ -14,7 +14,7 @@ #include "ndt_localization_trigger_module.hpp" -#include +#include #include #include @@ -23,7 +23,7 @@ namespace autoware::pose_initializer { using ServiceException = autoware::component_interface_utils::ServiceException; -using Initialize = autoware::component_interface_specs::localization::Initialize; +using Initialize = autoware::component_interface_specs_universe::localization::Initialize; NdtLocalizationTriggerModule::NdtLocalizationTriggerModule(rclcpp::Node * node) : node_(node) { diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp index 5e9c68d2acc80..5bde25a564f1d 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.cpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.cpp @@ -40,7 +40,7 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) output_pose_covariance_ = get_covariance_parameter(this, "output_pose_covariance"); gnss_particle_covariance_ = get_covariance_parameter(this, "gnss_particle_covariance"); - diagnostics_pose_reliable_ = std::make_unique( + diagnostics_pose_reliable_ = std::make_unique( this, "pose_initializer_status"); if (declare_parameter("ekf_enabled")) { diff --git a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp index 28d2eae08c3f1..2e152663dd170 100644 --- a/localization/autoware_pose_initializer/src/pose_initializer_core.hpp +++ b/localization/autoware_pose_initializer/src/pose_initializer_core.hpp @@ -15,10 +15,9 @@ #ifndef POSE_INITIALIZER_CORE_HPP_ #define POSE_INITIALIZER_CORE_HPP_ -#include "autoware/localization_util/diagnostics_module.hpp" - -#include +#include #include +#include #include #include @@ -42,8 +41,8 @@ class PoseInitializer : public rclcpp::Node private: using ServiceException = autoware::component_interface_utils::ServiceException; - using Initialize = autoware::component_interface_specs::localization::Initialize; - using State = autoware::component_interface_specs::localization::InitializationState; + using Initialize = autoware::component_interface_specs_universe::localization::Initialize; + using State = autoware::component_interface_specs_universe::localization::InitializationState; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; rclcpp::CallbackGroup::SharedPtr group_srv_; @@ -61,7 +60,7 @@ class PoseInitializer : public rclcpp::Node std::unique_ptr ekf_localization_trigger_; std::unique_ptr ndt_localization_trigger_; std::unique_ptr logger_configure_; - std::unique_ptr diagnostics_pose_reliable_; + std::unique_ptr diagnostics_pose_reliable_; double stop_check_duration_; void change_node_trigger(bool flag, bool need_spin = false); diff --git a/localization/autoware_pose_instability_detector/CHANGELOG.rst b/localization/autoware_pose_instability_detector/CHANGELOG.rst index b95dcaf6f01c8..d629e317c17f9 100644 --- a/localization/autoware_pose_instability_detector/CHANGELOG.rst +++ b/localization/autoware_pose_instability_detector/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_pose_instability_detector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/autoware_pose_instability_detector/package.xml b/localization/autoware_pose_instability_detector/package.xml index 91a565446f9e9..249ea6d625673 100644 --- a/localization/autoware_pose_instability_detector/package.xml +++ b/localization/autoware_pose_instability_detector/package.xml @@ -2,7 +2,7 @@ autoware_pose_instability_detector - 0.40.0 + 0.41.0 The autoware_pose_instability_detector package Yamato Ando Kento Yabuuchi diff --git a/localization/autoware_stop_filter/CHANGELOG.rst b/localization/autoware_stop_filter/CHANGELOG.rst index f91a3b8de7f05..f3379ac25cd32 100644 --- a/localization/autoware_stop_filter/CHANGELOG.rst +++ b/localization/autoware_stop_filter/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package autoware_stop_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs to autoware_internal_debug_msgs in files localization/autoware_stop_filter (`#9867 `_) + Co-authored-by: SakodaShintaro +* fix(autoware_stop_filter): fix bugprone-reserved-identifier (`#9643 `_) + * fix: bugprone-reserved-identifier + * fix: fmt + --------- +* Contributors: Fumiya Watanabe, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/localization/autoware_stop_filter/README.md b/localization/autoware_stop_filter/README.md index 9904707a59996..9136f6b6fc8a0 100644 --- a/localization/autoware_stop_filter/README.md +++ b/localization/autoware_stop_filter/README.md @@ -18,10 +18,10 @@ This node aims to: ### Output -| Name | Type | Description | -| ----------------- | ------------------------------------ | -------------------------------------------------------- | -| `output/odom` | `nav_msgs::msg::Odometry` | odometry with suppressed longitudinal and yaw twist | -| `debug/stop_flag` | `tier4_debug_msgs::msg::BoolStamped` | flag to represent whether the vehicle is stopping or not | +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | -------------------------------------------------------- | +| `output/odom` | `nav_msgs::msg::Odometry` | odometry with suppressed longitudinal and yaw twist | +| `debug/stop_flag` | `autoware_internal_debug_msgs::msg::BoolStamped` | flag to represent whether the vehicle is stopping or not | ## Parameters diff --git a/localization/autoware_stop_filter/package.xml b/localization/autoware_stop_filter/package.xml index 1ed5e2c2082be..b047b0cd81baa 100644 --- a/localization/autoware_stop_filter/package.xml +++ b/localization/autoware_stop_filter/package.xml @@ -2,7 +2,7 @@ autoware_stop_filter - 0.40.0 + 0.41.0 The stop filter package Yamato Ando Masahiro Sakamoto @@ -17,12 +17,12 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs geometry_msgs nav_msgs rclcpp rclcpp_components tf2 - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_stop_filter/src/stop_filter.cpp b/localization/autoware_stop_filter/src/stop_filter.cpp index f5e29419105d4..b0f1bc8eba053 100644 --- a/localization/autoware_stop_filter/src/stop_filter.cpp +++ b/localization/autoware_stop_filter/src/stop_filter.cpp @@ -22,8 +22,6 @@ #include #include -using std::placeholders::_1; - namespace autoware::stop_filter { StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) @@ -33,15 +31,16 @@ StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( - "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, _1)); + "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, std::placeholders::_1)); pub_odom_ = create_publisher("output/odom", 1); - pub_stop_flag_ = create_publisher("debug/stop_flag", 1); + pub_stop_flag_ = + create_publisher("debug/stop_flag", 1); } void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { - tier4_debug_msgs::msg::BoolStamped stop_flag_msg; + autoware_internal_debug_msgs::msg::BoolStamped stop_flag_msg; stop_flag_msg.stamp = msg->header.stamp; stop_flag_msg.data = false; diff --git a/localization/autoware_stop_filter/src/stop_filter.hpp b/localization/autoware_stop_filter/src/stop_filter.hpp index b6d56b5f77059..71864fab0a580 100644 --- a/localization/autoware_stop_filter/src/stop_filter.hpp +++ b/localization/autoware_stop_filter/src/stop_filter.hpp @@ -17,10 +17,10 @@ #include +#include #include #include #include -#include #include #include @@ -43,7 +43,7 @@ class StopFilter : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr pub_stop_flag_; //!< @brief stop flag publisher rclcpp::Subscription::SharedPtr sub_odom_; //!< @brief measurement odometry subscriber diff --git a/localization/autoware_twist2accel/CHANGELOG.rst b/localization/autoware_twist2accel/CHANGELOG.rst index 89b22e6ae4f26..d1e7bb13638d1 100644 --- a/localization/autoware_twist2accel/CHANGELOG.rst +++ b/localization/autoware_twist2accel/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_twist2accel ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(autoware_twist2accel): remove an unused dependency (`#9881 `_) + Removed an unused dependency +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files localization/autoware_twist2accel (`#9868 `_) + Co-authored-by: SakodaShintaro +* Contributors: Fumiya Watanabe, SakodaShintaro, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/localization/autoware_twist2accel/package.xml b/localization/autoware_twist2accel/package.xml index 75cea94546516..1a19bb3132ef2 100644 --- a/localization/autoware_twist2accel/package.xml +++ b/localization/autoware_twist2accel/package.xml @@ -2,7 +2,7 @@ autoware_twist2accel - 0.40.0 + 0.41.0 The acceleration estimation package Yamato Ando Masahiro Sakamoto @@ -23,7 +23,6 @@ rclcpp rclcpp_components tf2 - tier4_debug_msgs ament_cmake_ros ament_lint_auto diff --git a/localization/autoware_twist2accel/src/twist2accel.hpp b/localization/autoware_twist2accel/src/twist2accel.hpp index d338b256fec77..48bc03a326091 100644 --- a/localization/autoware_twist2accel/src/twist2accel.hpp +++ b/localization/autoware_twist2accel/src/twist2accel.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include diff --git a/localization/yabloc/yabloc_common/CHANGELOG.rst b/localization/yabloc/yabloc_common/CHANGELOG.rst index 6812e5ca633ca..66c6039b9fd67 100644 --- a/localization/yabloc/yabloc_common/CHANGELOG.rst +++ b/localization/yabloc/yabloc_common/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package yabloc_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index 5a8f19ff16bd7..58a77fe138f28 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -1,7 +1,7 @@ yabloc_common - 0.40.0 + 0.41.0 YabLoc common library Kento Yabuuchi Masahiro Sakamoto diff --git a/localization/yabloc/yabloc_image_processing/CHANGELOG.rst b/localization/yabloc/yabloc_image_processing/CHANGELOG.rst index bf4c1279fffdc..72f305b9ca340 100644 --- a/localization/yabloc/yabloc_image_processing/CHANGELOG.rst +++ b/localization/yabloc/yabloc_image_processing/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package yabloc_image_processing ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index c3202e8bfaebf..2cc9f817b1ec8 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -2,7 +2,7 @@ yabloc_image_processing - 0.40.0 + 0.41.0 YabLoc image processing package Kento Yabuuchi Masahiro Sakamoto diff --git a/localization/yabloc/yabloc_monitor/CHANGELOG.rst b/localization/yabloc/yabloc_monitor/CHANGELOG.rst index c7cb3c7dc307d..fe9126f5e0442 100644 --- a/localization/yabloc/yabloc_monitor/CHANGELOG.rst +++ b/localization/yabloc/yabloc_monitor/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package yabloc_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/localization/yabloc/yabloc_monitor/package.xml b/localization/yabloc/yabloc_monitor/package.xml index 1a0dbb6c7fdfa..b77f96c9dab81 100644 --- a/localization/yabloc/yabloc_monitor/package.xml +++ b/localization/yabloc/yabloc_monitor/package.xml @@ -2,7 +2,7 @@ yabloc_monitor - 0.40.0 + 0.41.0 YabLoc monitor package Kento Yabuuchi Masahiro Sakamoto diff --git a/localization/yabloc/yabloc_particle_filter/CHANGELOG.rst b/localization/yabloc/yabloc_particle_filter/CHANGELOG.rst index 935aecb810449..fdaecc4d86187 100644 --- a/localization/yabloc/yabloc_particle_filter/CHANGELOG.rst +++ b/localization/yabloc/yabloc_particle_filter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package yabloc_particle_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/yabloc/yabloc_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml index 074ff19334852..244cf88da7b0a 100644 --- a/localization/yabloc/yabloc_particle_filter/package.xml +++ b/localization/yabloc/yabloc_particle_filter/package.xml @@ -2,7 +2,7 @@ yabloc_particle_filter - 0.40.0 + 0.41.0 YabLoc particle filter package Kento Yabuuchi Masahiro Sakamoto diff --git a/localization/yabloc/yabloc_pose_initializer/CHANGELOG.rst b/localization/yabloc/yabloc_pose_initializer/CHANGELOG.rst index 0b78a98bf07fd..88ee7b5b3c4c3 100644 --- a/localization/yabloc/yabloc_pose_initializer/CHANGELOG.rst +++ b/localization/yabloc/yabloc_pose_initializer/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package yabloc_pose_initializer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index 4fd346f6327ad..c4bdfb63c150e 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -1,7 +1,7 @@ yabloc_pose_initializer - 0.40.0 + 0.41.0 The pose initializer Kento Yabuuchi Masahiro Sakamoto diff --git a/map/autoware_lanelet2_map_visualizer/CHANGELOG.rst b/map/autoware_lanelet2_map_visualizer/CHANGELOG.rst index 3d95ca5f804b2..fef302aececdf 100644 --- a/map/autoware_lanelet2_map_visualizer/CHANGELOG.rst +++ b/map/autoware_lanelet2_map_visualizer/CHANGELOG.rst @@ -3,6 +3,9 @@ Changelog for package autoware_lanelet2_map_visualizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/map/autoware_lanelet2_map_visualizer/package.xml b/map/autoware_lanelet2_map_visualizer/package.xml index eef83f90ec142..717445bef13ad 100644 --- a/map/autoware_lanelet2_map_visualizer/package.xml +++ b/map/autoware_lanelet2_map_visualizer/package.xml @@ -2,7 +2,7 @@ autoware_lanelet2_map_visualizer - 0.40.0 + 0.41.0 The autoware_lanelet2_map_visualizer package Yamato Ando Ryu Yamamoto diff --git a/map/autoware_map_height_fitter/CHANGELOG.rst b/map/autoware_map_height_fitter/CHANGELOG.rst index d600473d9756f..8d29e1323cfa8 100644 --- a/map/autoware_map_height_fitter/CHANGELOG.rst +++ b/map/autoware_map_height_fitter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_map_height_fitter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/map/autoware_map_height_fitter/package.xml b/map/autoware_map_height_fitter/package.xml index 98a48704c0c4b..50c4a4447f364 100644 --- a/map/autoware_map_height_fitter/package.xml +++ b/map/autoware_map_height_fitter/package.xml @@ -2,7 +2,7 @@ autoware_map_height_fitter - 0.40.0 + 0.41.0 The autoware_map_height_fitter package Takagi, Isamu Yamato Ando diff --git a/map/autoware_map_loader/CHANGELOG.rst b/map/autoware_map_loader/CHANGELOG.rst index a9e1080f1d6ec..f769ba0bbe7ad 100644 --- a/map/autoware_map_loader/CHANGELOG.rst +++ b/map/autoware_map_loader/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_map_loader ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp b/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp index 0922b1cb2bdd6..2e0eb14c02eae 100644 --- a/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp +++ b/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ #define AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ -#include +#include #include #include #include @@ -46,7 +46,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node const rclcpp::Time & now); private: - using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo; + using MapProjectorInfo = autoware::component_interface_specs_universe::map::MapProjectorInfo; void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); diff --git a/map/autoware_map_loader/package.xml b/map/autoware_map_loader/package.xml index 4d4eabc6e3f25..638014a65f04f 100644 --- a/map/autoware_map_loader/package.xml +++ b/map/autoware_map_loader/package.xml @@ -2,7 +2,7 @@ autoware_map_loader - 0.40.0 + 0.41.0 The autoware_map_loader package Yamato Ando Ryu Yamamoto @@ -19,7 +19,7 @@ ament_cmake_auto autoware_cmake - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_geography_utils autoware_lanelet2_extension diff --git a/map/autoware_map_projection_loader/CHANGELOG.rst b/map/autoware_map_projection_loader/CHANGELOG.rst index eef30e5c5f159..8ccadf6bccac0 100644 --- a/map/autoware_map_projection_loader/CHANGELOG.rst +++ b/map/autoware_map_projection_loader/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_map_projection_loader ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp index 1ca876dc035b2..6ae1e5b136777 100644 --- a/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp +++ b/map/autoware_map_projection_loader/include/autoware/map_projection_loader/map_projection_loader.hpp @@ -17,7 +17,7 @@ #include "rclcpp/rclcpp.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ class MapProjectionLoader : public rclcpp::Node explicit MapProjectionLoader(const rclcpp::NodeOptions & options); private: - using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo; + using MapProjectorInfo = autoware::component_interface_specs_universe::map::MapProjectorInfo; autoware::component_interface_utils::Publisher::SharedPtr publisher_; }; } // namespace autoware::map_projection_loader diff --git a/map/autoware_map_projection_loader/package.xml b/map/autoware_map_projection_loader/package.xml index ad31dfdcc094d..160cca26ba891 100644 --- a/map/autoware_map_projection_loader/package.xml +++ b/map/autoware_map_projection_loader/package.xml @@ -2,7 +2,7 @@ autoware_map_projection_loader - 0.40.0 + 0.41.0 autoware_map_projection_loader package as a ROS 2 node Yamato Ando Masahiro Sakamoto @@ -16,7 +16,7 @@ ament_cmake_auto autoware_cmake - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_lanelet2_extension autoware_map_msgs diff --git a/map/autoware_map_tf_generator/CHANGELOG.rst b/map/autoware_map_tf_generator/CHANGELOG.rst index 963e450bd4856..e856c3fef19ba 100644 --- a/map/autoware_map_tf_generator/CHANGELOG.rst +++ b/map/autoware_map_tf_generator/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_map_tf_generator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/map/autoware_map_tf_generator/package.xml b/map/autoware_map_tf_generator/package.xml index 0e81321daea2e..c9646f04687bf 100644 --- a/map/autoware_map_tf_generator/package.xml +++ b/map/autoware_map_tf_generator/package.xml @@ -2,7 +2,7 @@ autoware_map_tf_generator - 0.40.0 + 0.41.0 map_tf_generator package as a ROS 2 node Yamato Ando Kento Yabuuchi diff --git a/perception/autoware_bytetrack/CHANGELOG.rst b/perception/autoware_bytetrack/CHANGELOG.rst index 2db62f9a4e374..ec6f3f935b05c 100644 --- a/perception/autoware_bytetrack/CHANGELOG.rst +++ b/perception/autoware_bytetrack/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_bytetrack ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_bytetrack): fix bugprone-reserved-identifier (`#9647 `_) + fix: bugprone-reserved-identifier +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_bytetrack/lib/src/lapjv.cpp b/perception/autoware_bytetrack/lib/src/lapjv.cpp index 1b8b39ccbb9f7..8949c199f9b21 100644 --- a/perception/autoware_bytetrack/lib/src/lapjv.cpp +++ b/perception/autoware_bytetrack/lib/src/lapjv.cpp @@ -44,7 +44,7 @@ /** Column-reduction and reduction transfer for a dense cost matrix. */ -int_t _ccrrt_dense( +int_t ccrrt_dense( const uint_t n, cost_t * cost[], int_t * free_rows, int_t * x, int_t * y, cost_t * v) { int_t n_free_rows; @@ -108,7 +108,7 @@ int_t _ccrrt_dense( /** Augmenting row reduction for a dense cost matrix. */ -int_t _carr_dense( +int_t carr_dense( const uint_t n, cost_t * cost[], const uint_t n_free_rows, int_t * free_rows, int_t * x, int_t * y, cost_t * v) { @@ -181,7 +181,7 @@ int_t _carr_dense( /** Find columns with minimum d[j] and put them on the SCAN list. */ -uint_t _find_dense( +uint_t find_dense( const uint_t n, uint_t lo, const cost_t * d, int_t * cols, [[maybe_unused]] int_t * y) { uint_t hi = lo + 1; @@ -202,7 +202,7 @@ uint_t _find_dense( // Scan all columns in TODO starting from arbitrary column in SCAN // and try to decrease d of the TODO columns using the SCAN column. -int_t _scan_dense( +int_t scan_dense( const uint_t n, cost_t * cost[], uint_t * plo, uint_t * phi, cost_t * d, int_t * cols, int_t * pred, const int_t * y, const cost_t * v) { @@ -267,7 +267,7 @@ int_t find_path_dense( if (lo == hi) { PRINTF("%d..%d -> find\n", lo, hi); n_ready = lo; - hi = _find_dense(n, lo, d, cols, y); + hi = find_dense(n, lo, d, cols, y); PRINTF("check %d..%d\n", lo, hi); PRINT_INDEX_ARRAY(cols, n); for (uint_t k = lo; k < hi; k++) { @@ -279,7 +279,7 @@ int_t find_path_dense( } if (final_j == -1) { PRINTF("%d..%d -> scan\n", lo, hi); - final_j = _scan_dense(n, cost, &lo, &hi, d, cols, pred, y, v); + final_j = scan_dense(n, cost, &lo, &hi, d, cols, pred, y, v); PRINT_COST_ARRAY(d, n); PRINT_INDEX_ARRAY(cols, n); PRINT_INDEX_ARRAY(pred, n); @@ -304,7 +304,7 @@ int_t find_path_dense( /** Augment for a dense cost matrix. */ -int_t _ca_dense( +int_t ca_dense( const uint_t n, cost_t * cost[], const uint_t n_free_rows, int_t * free_rows, int_t * x, int_t * y, cost_t * v) { @@ -348,14 +348,14 @@ int lapjv_internal(const uint_t n, cost_t * cost[], int_t * x, int_t * y) NEW(free_rows, int_t, n); NEW(v, cost_t, n); - ret = _ccrrt_dense(n, cost, free_rows, x, y, v); + ret = ccrrt_dense(n, cost, free_rows, x, y, v); int i = 0; while (ret > 0 && i < 2) { - ret = _carr_dense(n, cost, ret, free_rows, x, y, v); + ret = carr_dense(n, cost, ret, free_rows, x, y, v); i++; } if (ret > 0) { - ret = _ca_dense(n, cost, ret, free_rows, x, y, v); + ret = ca_dense(n, cost, ret, free_rows, x, y, v); } FREE(v); FREE(free_rows); diff --git a/perception/autoware_bytetrack/package.xml b/perception/autoware_bytetrack/package.xml index 4dc982595c588..bc05293b80e0c 100644 --- a/perception/autoware_bytetrack/package.xml +++ b/perception/autoware_bytetrack/package.xml @@ -2,7 +2,7 @@ autoware_bytetrack - 0.40.0 + 0.41.0 ByteTrack implementation ported toward Autoware Manato HIRABAYASHI Yoshi RI diff --git a/perception/autoware_cluster_merger/CHANGELOG.rst b/perception/autoware_cluster_merger/CHANGELOG.rst index 1acdbf9333f30..a98cd3e5dba7f 100644 --- a/perception/autoware_cluster_merger/CHANGELOG.rst +++ b/perception/autoware_cluster_merger/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_cluster_merger ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_cluster_merger/package.xml b/perception/autoware_cluster_merger/package.xml index 92e1a8aadf40c..78959b657f724 100644 --- a/perception/autoware_cluster_merger/package.xml +++ b/perception/autoware_cluster_merger/package.xml @@ -2,7 +2,7 @@ autoware_cluster_merger - 0.40.0 + 0.41.0 The ROS 2 cluster merger package Yukihiro Saito Shunsuke Miura diff --git a/perception/autoware_compare_map_segmentation/CHANGELOG.rst b/perception/autoware_compare_map_segmentation/CHANGELOG.rst index 8d784ca304874..d6034e76e5ab5 100644 --- a/perception/autoware_compare_map_segmentation/CHANGELOG.rst +++ b/perception/autoware_compare_map_segmentation/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package autoware_compare_map_segmentation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_compare_map_segmentation): tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9869 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_compare_map_segmentation + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_compare_map_segmentation/package.xml b/perception/autoware_compare_map_segmentation/package.xml index 8c07d29156f8a..c633ebc808af9 100644 --- a/perception/autoware_compare_map_segmentation/package.xml +++ b/perception/autoware_compare_map_segmentation/package.xml @@ -2,7 +2,7 @@ autoware_compare_map_segmentation - 0.40.0 + 0.41.0 The ROS 2 autoware_compare_map_segmentation package amc-nu Yukihiro Saito diff --git a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp index ae07d54ad53d6..ac759504de557 100644 --- a/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/distance_based_compare_map_filter/node.cpp @@ -173,9 +173,9 @@ void DistanceBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp index 9f325b36676be..5794cfe37a45b 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_approximate_compare_map_filter/node.cpp @@ -141,9 +141,9 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index a31d5ec5dd6d6..db5e46a0eff5a 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -100,9 +100,9 @@ void VoxelBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp index d0bcf381cd62f..775a931ac4c4c 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_distance_based_compare_map_filter/node.cpp @@ -173,9 +173,9 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_crosswalk_traffic_light_estimator/CHANGELOG.rst b/perception/autoware_crosswalk_traffic_light_estimator/CHANGELOG.rst index 34e514d13e007..c628c4d310161 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/CHANGELOG.rst +++ b/perception/autoware_crosswalk_traffic_light_estimator/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package autoware_crosswalk_traffic_light_estimator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_crosswalk_traffic_light_estimator)!: tier4_debug_msgs changes to autoware_internal_debug_msgs in autoware_crosswalk_traffic_light_estimator (`#9870 `_) +* chore(autoware_crosswalk_traffic_light_estimator): fix docs (`#9822 `_) + * fix docs + * fix docs + * add tlr output image + * modify sentense + * modify sentense + * refactor readme + * fix docs + * fix + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat(autoware_crosswalk_traffic_light_estimator): overwrite invalid detection result (`#9667 `_) + * add code in order to check invalid detection + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Masato Saeki, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_crosswalk_traffic_light_estimator/README.md b/perception/autoware_crosswalk_traffic_light_estimator/README.md index b14fefbd43beb..068b588adfc62 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/README.md +++ b/perception/autoware_crosswalk_traffic_light_estimator/README.md @@ -1,40 +1,86 @@ -# crosswalk_traffic_light_estimator +# autoware_crosswalk_traffic_light_estimator ## Purpose -`crosswalk_traffic_light_estimator` is a module that estimates pedestrian traffic signals from HDMap and detected vehicle traffic signals. +`autoware_crosswalk_traffic_light_estimator` estimates pedestrian traffic signals which can be summarized as the following two tasks: + +- Estimate pedestrian traffic signals that are not subject to be detected by perception pipeline. +- Estimate whether pedestrian traffic signals are flashing and modify the result. + +This module works without `~/input/route`, but its behavior is outputting the subscribed results as is. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ------------------------------------ | ------------------------------------------------ | ------------------ | -| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | -| `~/input/route` | `autoware_planning_msgs::msg::LaneletRoute` | route | -| `~/input/classified/traffic_signals` | `tier4_perception_msgs::msg::TrafficSignalArray` | classified signals | +| Name | Type | Description | +| ------------------------------------ | ----------------------------------------------------- | ------------------ | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/route` | autoware_planning_msgs::msg::LaneletRoute | optional: route | +| `~/input/classified/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | classified signals | ### Output -| Name | Type | Description | -| -------------------------- | ------------------------------------------------------- | --------------------------------------------------------- | -| `~/output/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | output that contains estimated pedestrian traffic signals | +| Name | Type | Description | +| ---------------------------- | ----------------------------------------------------- | --------------------------------------------------------- | +| `~/output/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | output that contains estimated pedestrian traffic signals | +| `~/debug/processing_time_ms` | autoware_internal_debug_msgs::msg::Float64Stamped | pipeline latency time (ms) | ## Parameters -| Name | Type | Description | Default value | -| :---------------------------- | :------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| `use_last_detect_color` | `bool` | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | `true` | -| `last_detect_color_hold_time` | `double` | The time threshold to hold for last detect color. | `2.0` | +| Name | Type | Description | Default value | +| :---------------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `use_last_detect_color` | bool | If this parameter is `true`, this module estimates pedestrian's traffic signal as RED not only when vehicle's traffic signal is detected as GREEN/AMBER but also when detection results change GREEN/AMBER to UNKNOWN. (If detection results change RED or AMBER to UNKNOWN, this module estimates pedestrian's traffic signal as UNKNOWN.) If this parameter is `false`, this module use only latest detection results for estimation. (Only when the detection result is GREEN/AMBER, this module estimates pedestrian's traffic signal as RED.) | true | +| `last_detect_color_hold_time` | double | The time threshold to hold for last detect color. The unit is second. | 2.0 | +| `last_colors_hold_time` | double | The time threshold to hold for history detected pedestrian traffic light color. The unit is second. | 1.0 | ## Inner-workings / Algorithms +When the pedestrian traffic signals **are detected** by perception pipeline + +- If estimates the pedestrian traffic signals are flashing, overwrite the results +- Prefer the output from perception pipeline, but overwrite it if the pedestrian traffic signals are invalid(`no detection`, `backlight`, or `occlusion`) + +When the pedestrian traffic signals **are NOT detected** by perception pipeline + +- Estimate the color of pedestrian traffic signals based on detected vehicle traffic signals, HDMap, and route + +### Estimate whether pedestrian traffic signals are flashing + +```plantumul +start +if (the pedestrian traffic light classification result exists)then + : update the flashing flag according to the classification result(in_signal) and last_signals + if (the traffic light is flashing?)then(yes) + : update the traffic light state + else(no) + : the traffic light state is the same with the classification result +if (the classification result not exists) + : the traffic light state is the same with the estimation + : output the current traffic light state +end +``` + +#### Update flashing flag + +
+ +
+ +#### Update traffic light status + +
+ +
+ +### Estimate the color of pedestrian traffic signals + ```plantuml start -:subscribe detected traffic signals & HDMap; +:subscribe detected traffic signals, HDMap, and route; :extract crosswalk lanelets from HDMap; -:extract road lanelets that conflicts crosswalk; +:extract road lanelets that conflicts crosswalk from route; :initialize non_red_lanelets(lanelet::ConstLanelets); if (Latest detection result is **GREEN** or **AMBER**?) then (yes) :push back non_red_lanelets; @@ -58,7 +104,7 @@ end If traffic between pedestrians and vehicles is controlled by traffic signals, the crosswalk traffic signal maybe **RED** in order to prevent pedestrian from crossing when the following conditions are satisfied. -### Situation1 +#### Situation1 - crosswalk conflicts **STRAIGHT** lanelet - the lanelet refers **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) @@ -70,7 +116,7 @@ If traffic between pedestrians and vehicles is controlled by traffic signals, th -### Situation2 +#### Situation2 - crosswalk conflicts different turn direction lanelets (STRAIGHT and LEFT, LEFT and RIGHT, RIGHT and STRAIGHT) - the lanelets refer **GREEN** or **AMBER** traffic signal (The following pictures show only **GREEN** case) diff --git a/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png b/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png new file mode 100644 index 0000000000000..7686f3842e75c Binary files /dev/null and b/perception/autoware_crosswalk_traffic_light_estimator/images/flashing_state.png differ diff --git a/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png b/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png new file mode 100644 index 0000000000000..a03e7a88ee100 Binary files /dev/null and b/perception/autoware_crosswalk_traffic_light_estimator/images/traffic_light.png differ 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 index 8efb90cc87c89..e41ede2f574ed 100644 --- 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 @@ -19,10 +19,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -41,9 +41,9 @@ namespace autoware::crosswalk_traffic_light_estimator using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::StopWatch; +using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; -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; @@ -97,6 +97,8 @@ class CrosswalkTrafficLightEstimatorNode : public rclcpp::Node void removeDuplicateIds(TrafficSignalArray & signal_array) const; + bool isInvalidDetectionStatus(const TrafficSignal & signal) const; + // Node param bool use_last_detect_color_; double last_detect_color_hold_time_; diff --git a/perception/autoware_crosswalk_traffic_light_estimator/package.xml b/perception/autoware_crosswalk_traffic_light_estimator/package.xml index a760c3bd950e3..e0cd5d9b917ca 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/package.xml +++ b/perception/autoware_crosswalk_traffic_light_estimator/package.xml @@ -1,7 +1,7 @@ autoware_crosswalk_traffic_light_estimator - 0.40.0 + 0.41.0 The autoware_crosswalk_traffic_light_estimator package Satoshi Ota @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_perception_msgs diff --git a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp index b0ec4d0e5ffd0..d7cc6c725edfd 100644 --- a/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/autoware_crosswalk_traffic_light_estimator/src/node.cpp @@ -298,6 +298,14 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( if (valid_id2idx_map.count(id)) { size_t idx = valid_id2idx_map[id]; auto signal = msg.traffic_light_groups[idx]; + if (isInvalidDetectionStatus(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_light_groups[idx].elements[0] = output_traffic_signal_element; + continue; + } 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); @@ -314,6 +322,19 @@ void CrosswalkTrafficLightEstimatorNode::setCrosswalkTrafficSignal( } } +bool CrosswalkTrafficLightEstimatorNode::isInvalidDetectionStatus( + const TrafficSignal & signal) const +{ + // check occlusion, backlight(shape is unknown) and no detection(shape is circle) + if ( + signal.elements.front().color == TrafficSignalElement::UNKNOWN && + signal.elements.front().confidence == 0.0) { + return true; + } + + return false; +} + void CrosswalkTrafficLightEstimatorNode::updateFlashingState(const TrafficSignal & signal) { const auto id = signal.traffic_light_group_id; diff --git a/perception/autoware_detected_object_feature_remover/CHANGELOG.rst b/perception/autoware_detected_object_feature_remover/CHANGELOG.rst index f0d4f1f746eaa..79965097ad8e3 100644 --- a/perception/autoware_detected_object_feature_remover/CHANGELOG.rst +++ b/perception/autoware_detected_object_feature_remover/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_detected_object_feature_remover ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_detected_object_feature_remover/package.xml b/perception/autoware_detected_object_feature_remover/package.xml index a112598728614..a2b7e89adc1f2 100644 --- a/perception/autoware_detected_object_feature_remover/package.xml +++ b/perception/autoware_detected_object_feature_remover/package.xml @@ -2,7 +2,7 @@ autoware_detected_object_feature_remover - 0.40.0 + 0.41.0 The autoware_detected_object_feature_remover package Tomoya Kimura Yoshi Ri diff --git a/perception/autoware_detected_object_validation/CHANGELOG.rst b/perception/autoware_detected_object_validation/CHANGELOG.rst index bde610a8e3fd5..0486c6cbe2a41 100644 --- a/perception/autoware_detected_object_validation/CHANGELOG.rst +++ b/perception/autoware_detected_object_validation/CHANGELOG.rst @@ -2,6 +2,28 @@ Changelog for package autoware_detected_object_validation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_detected_object_validation): add height filter in lanelet filtering (`#10003 `_) + * feat: add height filter param + * feat: use ego base height + * fix: build error + * feat: add lanelet filter test + * feat: add height filter test + * docs: update README and lanelet filter + * fix: do not getTransform when flag is off + --------- +* feat(autoware_detected_object_validation): tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9871 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_detected_object_validation + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* feat(autoware_detected_object_validation): set validate distance in the obstacle pointcloud based validator (`#9663 `_) + * chore: add validate_max_distance_m parameter for obstacle_pointcloud_based_validator + * chore: optimize object distance validation in obstacle_pointcloud_validator + * chore: add validate_max_distance_m parameter for obstacle_pointcloud_based_validator + --------- +* Contributors: Fumiya Watanabe, Taekjin LEE, Vishal Chauhan, Yoshi Ri + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_detected_object_validation/CMakeLists.txt b/perception/autoware_detected_object_validation/CMakeLists.txt index bf205fb32bb1d..dd8d3616c362e 100644 --- a/perception/autoware_detected_object_validation/CMakeLists.txt +++ b/perception/autoware_detected_object_validation/CMakeLists.txt @@ -83,6 +83,10 @@ if(BUILD_TESTING) test/test_utils.cpp test/object_position_filter/test_object_position_filter.cpp ) + ament_auto_add_gtest(object_lanelet_filter_tests + test/test_utils.cpp + test/lanelet_filter/test_lanelet_filter.cpp + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml index d15b2c81cf6db..c6b3b5538fc44 100644 --- a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml +++ b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml @@ -21,3 +21,6 @@ object_speed_threshold: 3.0 # [m/s] debug: false lanelet_extra_margin: 0.0 + use_height_threshold: false + max_height_threshold: 10.0 # [m] from the base_link height in map frame + min_height_threshold: -1.0 # [m] from the base_link height in map frame diff --git a/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index cddee782e8af0..37b77eb436c7d 100644 --- a/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -12,5 +12,7 @@ #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] + validate_max_distance_m: 70.0 # [m] + using_2d_validator: false enable_debugger: false diff --git a/perception/autoware_detected_object_validation/object-lanelet-filter.md b/perception/autoware_detected_object_validation/object-lanelet-filter.md index b748885c188b4..4da7b7fdea3de 100644 --- a/perception/autoware_detected_object_validation/object-lanelet-filter.md +++ b/perception/autoware_detected_object_validation/object-lanelet-filter.md @@ -24,10 +24,19 @@ The objects only inside of the vector map will be published. ## Parameters -| Name | Type | Description | -| ---------------------- | -------- | ------------------------------------------------------------------------------------ | -| `debug` | `bool` | if true, publish debug markers | -| `lanelet_extra_margin` | `double` | `if > 0` lanelet polygons are expanded by extra margin, `if <= 0` margin is disabled | +Description of the `filter_settings` in the parameters of the `object_lanelet_filter` node. + +| Name | Type | Description | +| ------------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------- | +| `debug` | `bool` | If `true`, publishes additional debug information, including visualization markers on the `~/debug/marker` topic for tools like RViz. | +| `lanelet_extra_margin` | `double` | If `> 0`, expands the lanelet polygons used for overlap checks by this margin (in meters). If `<= 0`, polygon expansion is disabled. | +| `polygon_overlap_filter.enabled` | `bool` | If `true`, enables filtering of objects based on their overlap with lanelet polygons. | +| `lanelet_direction_filter.enabled` | `bool` | If `true`, enables filtering of objects based on their velocity direction relative to the lanelet. | +| `lanelet_direction_filter.velocity_yaw_threshold` | `double` | The yaw angle difference threshold (in radians) between the object’s velocity vector and the lanelet direction. | +| `lanelet_direction_filter.object_speed_threshold` | `double` | The minimum speed (in m/s) of an object required for the direction filter to be applied. | +| `use_height_threshold` | `bool` | If `true`, enables filtering of objects based on their height relative to the base_link frame. | +| `max_height_threshold` | `double` | The maximum allowable height (in meters) of an object relative to the base_link in the map frame. | +| `min_height_threshold` | `double` | The minimum allowable height (in meters) of an object relative to the base_link in the map frame. | ### Core Parameters diff --git a/perception/autoware_detected_object_validation/package.xml b/perception/autoware_detected_object_validation/package.xml index 9aa6f018de5fa..01c48bbebc756 100644 --- a/perception/autoware_detected_object_validation/package.xml +++ b/perception/autoware_detected_object_validation/package.xml @@ -2,7 +2,7 @@ autoware_detected_object_validation - 0.40.0 + 0.41.0 The ROS 2 detected_object_validation package Yukihiro Saito Shunsuke Miura diff --git a/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json index fd4e39f0ca02e..8e1b641eebbf8 100644 --- a/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json +++ b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json @@ -95,9 +95,42 @@ } }, "required": ["enabled", "velocity_yaw_threshold", "object_speed_threshold"] + }, + "debug": { + "type": "boolean", + "default": false, + "description": "If true, debug information is enabled." + }, + "lanelet_extra_margin": { + "type": "number", + "default": 0.0, + "description": "Extra margin added to the lanelet boundaries." + }, + "use_height_threshold": { + "type": "boolean", + "default": false, + "description": "If true, height thresholds are used to filter objects." + }, + "max_height_threshold": { + "type": "number", + "default": 10.0, + "description": "Maximum height threshold for filtering objects (in meters)." + }, + "min_height_threshold": { + "type": "number", + "default": -1.0, + "description": "Minimum height threshold for filtering objects (in meters)." } }, - "required": ["polygon_overlap_filter", "lanelet_direction_filter"] + "required": [ + "polygon_overlap_filter", + "lanelet_direction_filter", + "debug", + "lanelet_extra_margin", + "use_height_threshold", + "max_height_threshold", + "min_height_threshold" + ] } }, "required": ["filter_target_label", "filter_settings"], diff --git a/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json index d7aa970993ca1..95c83a90f075f 100644 --- a/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json +++ b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json @@ -30,6 +30,11 @@ "default": [800, 800, 800, 800, 800, 800, 800, 800], "description": "Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink." }, + "validate_max_distance_m": { + "type": "number", + "default": 70.0, + "description": "The maximum distance from the baselink to the object to be validated" + }, "using_2d_validator": { "type": "boolean", "default": false, diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp index f91c4e2036227..85add97e20078 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp @@ -66,6 +66,12 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod filter_settings_.debug = declare_parameter("filter_settings.debug"); filter_settings_.lanelet_extra_margin = declare_parameter("filter_settings.lanelet_extra_margin"); + filter_settings_.use_height_threshold = + declare_parameter("filter_settings.use_height_threshold"); + filter_settings_.max_height_threshold = + declare_parameter("filter_settings.max_height_threshold"); + filter_settings_.min_height_threshold = + declare_parameter("filter_settings.min_height_threshold"); // Set publisher/subscriber map_sub_ = this->create_subscription( @@ -146,6 +152,19 @@ void ObjectLaneletFilterNode::objectCallback( RCLCPP_ERROR(get_logger(), "Failed transform to %s.", lanelet_frame_id_.c_str()); return; } + // vehicle base pose :map -> base_link + if (filter_settings_.use_height_threshold) { + try { + ego_base_height_ = tf_buffer_ + .lookupTransform( + lanelet_frame_id_, "base_link", transformed_objects.header.stamp, + rclcpp::Duration::from_seconds(0.5)) + .transform.translation.z; + } catch (const tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(get_logger(), "Failed to get transform: " << ex.what()); + return; + } + } if (!transformed_objects.objects.empty()) { // calculate convex hull @@ -180,9 +199,9 @@ void ObjectLaneletFilterNode::objectCallback( std::chrono::nanoseconds( (this->get_clock()->now() - output_object_msg.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } @@ -199,6 +218,16 @@ bool ObjectLaneletFilterNode::filterObject( return false; } + // 0. check height threshold + if (filter_settings_.use_height_threshold) { + const double object_height = transformed_object.shape.dimensions.z; + if ( + object_height > ego_base_height_ + filter_settings_.max_height_threshold || + object_height < ego_base_height_ + filter_settings_.min_height_threshold) { + return false; + } + } + bool filter_pass = true; // 1. is polygon overlap with road lanelets or shoulder lanelets if (filter_settings_.polygon_overlap_filter) { diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp index 724e60df27e49..bbb0f7c0d43ce 100644 --- a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp +++ b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -88,6 +89,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; utils::FilterTargetLabel filter_target_; + double ego_base_height_ = 0.0; struct FilterSettings { bool polygon_overlap_filter; @@ -96,6 +98,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node double lanelet_direction_filter_object_speed_threshold; bool debug; double lanelet_extra_margin; + bool use_height_threshold; + double max_height_threshold = std::numeric_limits::infinity(); + double min_height_threshold = -std::numeric_limits::infinity(); } filter_settings_; bool filterObject( diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 194f333acd87e..0c69b45291281 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -295,6 +295,8 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( declare_parameter>("max_points_num"); points_num_threshold_param_.min_points_and_distance_ratio = declare_parameter>("min_points_and_distance_ratio"); + const double validate_max_distance = declare_parameter("validate_max_distance_m"); + validate_max_distance_sq_ = validate_max_distance * validate_max_distance; using_2d_validator_ = declare_parameter("using_2d_validator"); @@ -346,6 +348,18 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); const auto & object = input_objects->objects.at(i); + // check object distance + const double distance_sq = + transformed_object.kinematics.pose_with_covariance.pose.position.x * + transformed_object.kinematics.pose_with_covariance.pose.position.x + + transformed_object.kinematics.pose_with_covariance.pose.position.y * + transformed_object.kinematics.pose_with_covariance.pose.position.y; + if (distance_sq > validate_max_distance_sq_) { + // pass to output + output.objects.push_back(object); + continue; + } + const auto validated = validation_is_ready ? validator_->validate_object(transformed_object) : false; if (debugger_) { @@ -372,7 +386,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency); } diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index 6d9a11634ea08..f10dbb7531d36 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -152,6 +152,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node typedef message_filters::Synchronizer Sync; Sync sync_; PointsNumThresholdParam points_num_threshold_param_; + double validate_max_distance_sq_; // maximum object distance to validate, squared [m^2] std::shared_ptr debugger_; bool using_2d_validator_; diff --git a/perception/autoware_detected_object_validation/test/lanelet_filter/test_lanelet_filter.cpp b/perception/autoware_detected_object_validation/test/lanelet_filter/test_lanelet_filter.cpp new file mode 100644 index 0000000000000..b1e061261f8b6 --- /dev/null +++ b/perception/autoware_detected_object_validation/test/lanelet_filter/test_lanelet_filter.cpp @@ -0,0 +1,302 @@ +// Copyright 2025 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +// Include the filter node header with include guard + +#include "../../src/lanelet_filter/lanelet_filter.hpp" +#include "../test_lanelet_utils.hpp" + +using autoware::detected_object_validation::lanelet_filter::ObjectLaneletFilterNode; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::ObjectClassification; + +namespace +{ +/// @brief Create a test manager (custom utility for Autoware tests) +std::shared_ptr generateTestManager() +{ + return std::make_shared(); +} + +/// @brief Create an instance of ObjectLaneletFilterNode with a parameter file +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + // Example parameter file path + const auto detected_object_validation_dir = + ament_index_cpp::get_package_share_directory("autoware_detected_object_validation"); + node_options.arguments( + {"--ros-args", "--params-file", + detected_object_validation_dir + "/config/object_lanelet_filter.param.yaml"}); + + return std::make_shared(node_options); +} + +std::shared_ptr createStaticTfBroadcasterNode( + const std::string & parent_frame_id, const std::string & child_frame_id, + const geometry_msgs::msg::Vector3 & translation, const geometry_msgs::msg::Quaternion & rotation, + const std::string & node_name = "test_tf_broadcaster") +{ + // Create a dedicated node for broadcasting the static transform + auto broadcaster_node = std::make_shared(node_name); + + // Create the static transform broadcaster + auto static_broadcaster = std::make_shared(broadcaster_node); + + // Prepare the transform message + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.stamp = broadcaster_node->now(); + transform_stamped.header.frame_id = parent_frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.transform.translation = translation; + transform_stamped.transform.rotation = rotation; + + // Broadcast the transform + static_broadcaster->sendTransform(transform_stamped); + + return broadcaster_node; +} + +// publish sample LaneletMapBin message +void publishLaneletMapBin( + const std::shared_ptr & test_manager, + const std::shared_ptr & test_target_node, + const std::string & input_map_topic = "input/vector_map") +{ + auto qos = rclcpp::QoS(1).transient_local(); + LaneletMapBin map_msg = createSimpleLaneletMapMsg(); + test_manager->test_pub_msg(test_target_node, input_map_topic, map_msg, qos); +} +} // namespace + +/// @brief Test LaneletMapBin publishing + object filtering +TEST(DetectedObjectValidationTest, testObjectLaneletFilterWithMap) +{ + rclcpp::init(0, nullptr); + + // Create the test node (lanelet_filter_node) + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + // Create and add a TF broadcaster node + auto tf_node = createStaticTfBroadcasterNode( + "map", "base_link", geometry_msgs::build().x(0.0).y(0.0).z(0.0), + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0), + "my_test_tf_broadcaster"); + + // Subscribe to the output topic + const std::string output_topic = "output/object"; + DetectedObjects latest_msg; + auto output_callback = [&latest_msg](const DetectedObjects::ConstSharedPtr msg) { + latest_msg = *msg; + }; + test_manager->set_subscriber(output_topic, output_callback); + + // 1) Publish a LaneletMapBin + // In a real test, you'd have an actual map message. + publishLaneletMapBin(test_manager, test_target_node); + + // 2) Publish DetectedObjects + const std::string input_object_topic = "input/object"; + DetectedObjects input_objects; + input_objects.header.frame_id = "base_link"; + + // Prepare two unknown objects + { // Object 1: not in the lanelet + DetectedObject obj; + obj.kinematics.pose_with_covariance.pose.position.x = 100.0; + obj.kinematics.pose_with_covariance.pose.position.y = 5.0; + obj.classification.resize(1); + obj.classification[0].label = ObjectClassification::UNKNOWN; + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(1.0).y(1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(1.0).y(-1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(-1.0).y(-1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(-1.0).y(1.0).z(0.0)); + input_objects.objects.push_back(obj); + } + { // Object 2: in the lanelet. To be filtered out. + DetectedObject obj; + obj.kinematics.pose_with_covariance.pose.position.x = 0.0; + obj.kinematics.pose_with_covariance.pose.position.y = 0.0; + obj.classification.resize(1); + obj.classification[0].label = ObjectClassification::UNKNOWN; + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(1.0).y(1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(1.0).y(-1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(-1.0).y(-1.0).z(0.0)); + obj.shape.footprint.points.push_back( + geometry_msgs::build().x(-1.0).y(1.0).z(0.0)); + input_objects.objects.push_back(obj); + } + + // Publish the DetectedObjects + test_manager->test_pub_msg(test_target_node, input_object_topic, input_objects); + + EXPECT_EQ(latest_msg.objects.size(), 1) + << "Expected one object to pass through (or receive something)."; + + rclcpp::shutdown(); +} + +/// @brief Test with an empty object list +TEST(DetectedObjectValidationTest, testObjectLaneletFilterEmptyObjects) +{ + rclcpp::init(0, nullptr); + + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + // Create and add a TF broadcaster node + auto tf_node = createStaticTfBroadcasterNode( + "map", "base_link", geometry_msgs::build().x(0.0).y(0.0).z(0.0), + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0), + "my_test_tf_broadcaster"); + + // Publish simple LaneletMapBin + publishLaneletMapBin(test_manager, test_target_node); + + // Subscribe to the output topic + const std::string output_topic = "output/object"; + int callback_counter = 0; + auto output_callback = [&callback_counter](const DetectedObjects::ConstSharedPtr msg) { + (void)msg; + ++callback_counter; + }; + test_manager->set_subscriber(output_topic, output_callback); + + // Publish empty objects (not publishing map) + DetectedObjects input_objects; + input_objects.header.frame_id = "base_link"; + + test_manager->test_pub_msg(test_target_node, "input/object", input_objects); + + // The callback should still be called at least once + EXPECT_GE(callback_counter, 1); + + rclcpp::shutdown(); +} + +TEST(DetectedObjectValidationTest, testObjectLaneletFilterHeightThreshold) +{ + rclcpp::init(0, nullptr); + + // 1) Setup test manager and node with a custom param override (height filter enabled) + auto test_manager = generateTestManager(); + + auto node_options = rclcpp::NodeOptions{}; + const auto detected_object_validation_dir = + ament_index_cpp::get_package_share_directory("autoware_detected_object_validation"); + node_options.arguments( + {"--ros-args", "--params-file", + detected_object_validation_dir + "/config/object_lanelet_filter.param.yaml"}); + node_options.append_parameter_override("filter_settings.use_height_threshold", true); + node_options.append_parameter_override("filter_settings.max_height_threshold", 2.0); + node_options.append_parameter_override("filter_settings.min_height_threshold", 0.0); + + auto test_target_node = std::make_shared(node_options); + + // 2) Create a TF broadcaster node (map->base_link at z=0.0) + auto tf_node = createStaticTfBroadcasterNode( + "map", "base_link", geometry_msgs::build().x(0.0).y(0.0).z(0.0), + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0), + "my_test_tf_broadcaster_1"); + + // Subscribe to the output topic + const std::string output_topic = "output/object"; + DetectedObjects latest_msg; + auto output_callback = [&latest_msg](const DetectedObjects::ConstSharedPtr msg) { + latest_msg = *msg; + }; + test_manager->set_subscriber(output_topic, output_callback); + + // 3) Publish a simple LaneletMapBin (50m straight lane) + publishLaneletMapBin(test_manager, test_target_node); + + // 4) Publish DetectedObjects + const std::string input_object_topic = "input/object"; + DetectedObjects input_objects; + input_objects.header.frame_id = "base_link"; + + // (A) Object in-lane, height=1.5 => expected to remain + { + DetectedObject obj; + obj.classification.resize(1); + obj.classification[0].label = ObjectClassification::UNKNOWN; + obj.kinematics.pose_with_covariance.pose.position.x = 0.0; + obj.kinematics.pose_with_covariance.pose.position.y = 0.0; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + obj.shape.dimensions.z = 1.5; + input_objects.objects.push_back(obj); + } + + // (B) Object in-lane, height=3.0 => expected to be filtered out + { + DetectedObject obj; + obj.classification.resize(1); + obj.classification[0].label = ObjectClassification::UNKNOWN; + obj.kinematics.pose_with_covariance.pose.position.x = 5.0; + obj.kinematics.pose_with_covariance.pose.position.y = 0.0; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions.x = 2.0; + obj.shape.dimensions.y = 2.0; + obj.shape.dimensions.z = 3.0; + input_objects.objects.push_back(obj); + } + + // Publish the objects (Round 1) + test_manager->test_pub_msg(test_target_node, input_object_topic, input_objects); + + // 5) Check result => only the first object remains + EXPECT_EQ(latest_msg.objects.size(), 1U) + << "Height filter is enabled => only the shorter object (1.5m) should remain."; + + // 6) Second scenario: place ego at z=1.3, effectively lowering object's relative height + auto tf_node_after = createStaticTfBroadcasterNode( + "map", "base_link", geometry_msgs::build().x(0.0).y(0.0).z(1.3), + geometry_msgs::build().x(0.0).y(0.0).z(0.0).w(1.0), + "my_test_tf_broadcaster_2"); + + // Publish the same objects (Round 2) + test_manager->test_pub_msg(test_target_node, input_object_topic, input_objects); + + // 7) Check result => now both objects remain because each one's height above base_link is < 2.0 + EXPECT_EQ(latest_msg.objects.size(), 2U) + << "With ego at z=1.3, the 3.0m object is effectively only ~1.7m above ego => remain."; + + rclcpp::shutdown(); +} diff --git a/perception/autoware_detected_object_validation/test/test_lanelet_utils.hpp b/perception/autoware_detected_object_validation/test/test_lanelet_utils.hpp new file mode 100644 index 0000000000000..3e9ff5372044a --- /dev/null +++ b/perception/autoware_detected_object_validation/test/test_lanelet_utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2025 TIER 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 TEST_LANELET_UTILS_HPP_ +#define TEST_LANELET_UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include + +/** + * @brief Create a simple LaneletMapBin message containing a single 50[m] straight lanelet. + * + * The lanelet has left boundary at y=+1.5 and right boundary at y=-1.5, + * forming a lane of width 3.0[m] and length 50.0[m]. + * + * @return autoware_map_msgs::msg::LaneletMapBin The generated map message. + */ +inline autoware_map_msgs::msg::LaneletMapBin createSimpleLaneletMapMsg() +{ + // 1) Create left boundary (y = +1.5). + // This is a straight line from (0, +1.5, 0) to (50, +1.5, 0). + lanelet::Point3d p1_left(lanelet::utils::getId(), 0.0, 1.5, 0.0); + lanelet::Point3d p2_left(lanelet::utils::getId(), 50.0, 1.5, 0.0); + lanelet::LineString3d ls_left(lanelet::utils::getId(), {p1_left, p2_left}); + ls_left.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::RoadBorder; + + // 2) Create right boundary (y = -1.5). + // This is a straight line from (0, -1.5, 0) to (50, -1.5, 0). + lanelet::Point3d p1_right(lanelet::utils::getId(), 0.0, -1.5, 0.0); + lanelet::Point3d p2_right(lanelet::utils::getId(), 50.0, -1.5, 0.0); + lanelet::LineString3d ls_right(lanelet::utils::getId(), {p1_right, p2_right}); + ls_right.attributes()[lanelet::AttributeName::Type] = lanelet::AttributeValueString::RoadBorder; + + // 3) Create a lanelet by specifying the left and right boundaries, + // then set the subtype to "Road". + lanelet::Lanelet lanelet_section(lanelet::utils::getId(), ls_left, ls_right); + lanelet_section.attributes()[lanelet::AttributeName::Subtype] = + lanelet::AttributeValueString::Road; + + // 4) Create a LaneletMap and add the lanelet to it. + auto lanelet_map = std::make_shared(); + lanelet_map->add(lanelet_section); + + // 5) Convert the LaneletMap to a LaneletMapBin message. + autoware_map_msgs::msg::LaneletMapBin map_bin_msg; + lanelet::utils::conversion::toBinMsg(lanelet_map, &map_bin_msg); + + // 6) Set the frame_id in the header. + map_bin_msg.header.frame_id = "map"; + + return map_bin_msg; +} + +#endif // TEST_LANELET_UTILS_HPP_ diff --git a/perception/autoware_detection_by_tracker/CHANGELOG.rst b/perception/autoware_detection_by_tracker/CHANGELOG.rst index dd8a44828a5ff..78e72f5a9cdff 100644 --- a/perception/autoware_detection_by_tracker/CHANGELOG.rst +++ b/perception/autoware_detection_by_tracker/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package autoware_detection_by_tracker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9880 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_detection_by_tracker + Co-authored-by: Taekjin LEE +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/perception/autoware_detection_by_tracker/package.xml b/perception/autoware_detection_by_tracker/package.xml index b9ee4fa2aac71..fccd8f32bbebc 100644 --- a/perception/autoware_detection_by_tracker/package.xml +++ b/perception/autoware_detection_by_tracker/package.xml @@ -2,7 +2,7 @@ autoware_detection_by_tracker - 0.40.0 + 0.41.0 The ROS 2 autoware_detection_by_tracker package Yukihiro Saito Yoshi Ri diff --git a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp index 39dd3db10cfa9..8bd1deba35ccb 100644 --- a/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_detection_by_tracker/src/debugger/debugger.hpp @@ -89,9 +89,9 @@ class Debugger void startMeasureProcessingTime() { stop_watch_ptr_->tic("processing_time"); } void publishProcessingTime() { - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } diff --git a/perception/autoware_elevation_map_loader/CHANGELOG.rst b/perception/autoware_elevation_map_loader/CHANGELOG.rst index b53c69f07a8a9..c967ebe6326cd 100644 --- a/perception/autoware_elevation_map_loader/CHANGELOG.rst +++ b/perception/autoware_elevation_map_loader/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_elevation_map_loader ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_elevation_map_loader/package.xml b/perception/autoware_elevation_map_loader/package.xml index 6839cd60e2990..a24bf2e0b8062 100644 --- a/perception/autoware_elevation_map_loader/package.xml +++ b/perception/autoware_elevation_map_loader/package.xml @@ -2,7 +2,7 @@ autoware_elevation_map_loader - 0.40.0 + 0.41.0 The autoware_elevation_map_loader package Kosuke Takeuchi Taichi Higashide diff --git a/perception/autoware_euclidean_cluster/CHANGELOG.rst b/perception/autoware_euclidean_cluster/CHANGELOG.rst index b7ce92ddd6510..ef7b29edf7f0a 100644 --- a/perception/autoware_euclidean_cluster/CHANGELOG.rst +++ b/perception/autoware_euclidean_cluster/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package autoware_euclidean_cluster ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_euclidean_cluster)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_euclidean_cluster (`#9873 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_euclidean_cluster + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_euclidean_cluster/package.xml b/perception/autoware_euclidean_cluster/package.xml index f4568baaea19b..8bac386a5bbb6 100644 --- a/perception/autoware_euclidean_cluster/package.xml +++ b/perception/autoware_euclidean_cluster/package.xml @@ -2,7 +2,7 @@ autoware_euclidean_cluster - 0.40.0 + 0.41.0 The autoware_euclidean_cluster package Yukihiro Saito Dai Nguyen diff --git a/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp index cccb05160942c..1a72a813d581b 100644 --- a/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp +++ b/perception/autoware_euclidean_cluster/src/euclidean_cluster_node.cpp @@ -78,11 +78,11 @@ void EuclideanClusterNode::onPointCloud( std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index 019001c54ba49..18898fae3bfa0 100644 --- a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -81,11 +81,11 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - output.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_ground_segmentation/CHANGELOG.rst b/perception/autoware_ground_segmentation/CHANGELOG.rst index b23cfa4972f1f..36a8adae770fe 100644 --- a/perception/autoware_ground_segmentation/CHANGELOG.rst +++ b/perception/autoware_ground_segmentation/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_ground_segmentation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_ground_segmentation): tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9878 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_ground_segmentation +* fix(autoware_ground_segmentation): fix bugprone-branch-clone (`#9648 `_) + fix: bugprone-branch-clone +* Contributors: Fumiya Watanabe, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_ground_segmentation/package.xml b/perception/autoware_ground_segmentation/package.xml index 1cb179970e1ae..b1ef241dd52cf 100644 --- a/perception/autoware_ground_segmentation/package.xml +++ b/perception/autoware_ground_segmentation/package.xml @@ -2,7 +2,7 @@ autoware_ground_segmentation - 0.40.0 + 0.41.0 The ROS 2 autoware_ground_segmentation package amc-nu Yukihiro Saito diff --git a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp index 488de2da47f91..8fda0c1395caf 100644 --- a/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp +++ b/perception/autoware_ground_segmentation/src/scan_ground_filter/node.cpp @@ -246,7 +246,7 @@ void ScanGroundFilterComponent::classifyPointCloud( float radius_distance_from_gnd = pd.radius - prev_gnd_radius; float height_from_gnd = point_curr.z - prev_gnd_point.z; float height_from_obj = point_curr.z - non_ground_cluster.getAverageHeight(); - bool calculate_slope = false; + bool calculate_slope = true; bool is_point_close_to_prev = (points_distance < (pd.radius * radial_divider_angle_rad_ + split_points_distance_tolerance_)); @@ -264,8 +264,6 @@ void ScanGroundFilterComponent::classifyPointCloud( // close to the previous point, set point follow label point_label_curr = PointLabel::POINT_FOLLOW; calculate_slope = false; - } else { - calculate_slope = true; } if (is_point_close_to_prev) { height_from_gnd = point_curr.z - ground_cluster.getAverageHeight(); @@ -371,9 +369,9 @@ void ScanGroundFilterComponent::faster_filter( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_image_projection_based_fusion/CHANGELOG.rst b/perception/autoware_image_projection_based_fusion/CHANGELOG.rst index f0a39ef78c708..2f146bdf94bf0 100644 --- a/perception/autoware_image_projection_based_fusion/CHANGELOG.rst +++ b/perception/autoware_image_projection_based_fusion/CHANGELOG.rst @@ -2,6 +2,133 @@ Changelog for package autoware_image_projection_based_fusion ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_image_projection_based_fusion)!: tier4_debug-msgs changed to autoware_internal_debug_msgs in autoware_image_projection_based_fusion (`#9877 `_) +* fix(image_projection_based_fusion): revise message publishers (`#9865 `_) + * refactor: fix condition for publishing painted pointcloud message + * fix: publish output revised + * feat: fix condition for publishing painted pointcloud message + * feat: roi-pointclout fusion - publish empty image even when there is no target roi + * fix: remap output topic for clusters in roi_pointcloud_fusion + * style(pre-commit): autofix + * feat: fix condition for publishing painted pointcloud message + * feat: Add debug publisher for internal debugging + * feat: remove !! pointer to bool conversion + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* fix(image_projection_based_fusion): remove mutex (`#9862 `_) + refactor: Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability +* refactor(autoware_image_projection_based_fusion): organize 2d-detection related members (`#9789 `_) + * chore: input_camera_topics\_ is only for debug + * feat: fuse main message with cached roi messages in fusion_node.cpp + * chore: add comments on each process step, organize methods + * feat: Export process method in fusion_node.cpp + Export the `exportProcess()` method in `fusion_node.cpp` to handle the post-processing and publishing of the fused messages. This method cancels the timer, performs the necessary post-processing steps, publishes the output message, and resets the flags. It also adds processing time for debugging purposes. This change improves the organization and readability of the code. + * feat: Refactor fusion_node.hpp and fusion_node.cpp + Refactor the `fusion_node.hpp` and `fusion_node.cpp` files to improve code organization and readability. This includes exporting the `exportProcess()` method in `fusion_node.cpp` to handle post-processing and publishing of fused messages, adding comments on each process step, organizing methods, and fusing the main message with cached ROI messages. These changes enhance the overall quality of the codebase. + * Refactor fusion_node.cpp and fusion_node.hpp for improved code organization and readability + * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability + * feat: Refactor fusion_node.cpp for improved code organization and readability + * Refactor fusion_node.cpp for improved code organization and readability + * feat: implement mutex per 2d detection process + * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability + * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability + * revise template, inputs first and output at the last + * explicit in and out types 1 + * clarify pointcloud message type + * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability + * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability + * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability + * Refactor publisher types in fusion_node.hpp and node.hpp + * fix: resolve cppcheck issue shadowVariable + * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability + * chore: rename Det2dManager to Det2dStatus + * revert mutex related changes + * refactor: review member and method's access + * fix: resolve shadowVariable of 'det2d' + * fix missing line + * refactor message postprocess and publish methods + * publish the main message is common + * fix: replace pointcloud message type by the typename + * review member access + * Refactor fusion_node.hpp and fusion_node.cpp for improved code organization and readability + * refactor: fix condition for publishing painted pointcloud message + * fix: remove unused variable + --------- +* feat(lidar_centerpoint, pointpainting): add diag publisher for max voxel size (`#9720 `_) +* feat(pointpainting_fusion): enable cloud display on image (`#9813 `_) +* feat(image_projection_based_fusion): add cache for camera projection (`#9635 `_) + * add camera_projection class and projection cache + * style(pre-commit): autofix + * fix FOV filtering + * style(pre-commit): autofix + * remove unused includes + * update schema + * fix cpplint error + * apply suggestion: more simple and effcient computation + * correct terminology + * style(pre-commit): autofix + * fix comments + * fix var name + Co-authored-by: Taekjin LEE + * fix variable names + Co-authored-by: Taekjin LEE + * fix variable names + Co-authored-by: Taekjin LEE + * fix variable names + Co-authored-by: Taekjin LEE + * fix variable names + Co-authored-by: Taekjin LEE + * fix variable names + * fix initialization + Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> + * add verification to point_project_to_unrectified_image when loading + Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> + * chore: add option description to project points to unrectified image + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Taekjin LEE + Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> + Co-authored-by: Taekjin LEE +* feat(image_projection_based_fusion): add timekeeper (`#9632 `_) + * add timekeeper + * chore: refactor time-keeper position + * chore: bring back a missing comment + * chore: remove redundant timekeepers + --------- + Co-authored-by: Taekjin LEE +* Contributors: Amadeusz Szymko, Fumiya Watanabe, Masaki Baba, Taekjin LEE, Vishal Chauhan, Yi-Hsiang Fang (Vivid), kminoda + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_image_projection_based_fusion/CMakeLists.txt b/perception/autoware_image_projection_based_fusion/CMakeLists.txt index 73d305d2ab2a8..7c8160d6fe1b7 100644 --- a/perception/autoware_image_projection_based_fusion/CMakeLists.txt +++ b/perception/autoware_image_projection_based_fusion/CMakeLists.txt @@ -16,6 +16,7 @@ endif() # Build non-CUDA dependent nodes ament_auto_add_library(${PROJECT_NAME} SHARED + src/camera_projection.cpp src/fusion_node.cpp src/debugger.cpp src/utils/geometry.cpp diff --git a/perception/autoware_image_projection_based_fusion/README.md b/perception/autoware_image_projection_based_fusion/README.md index c976697b0396d..dcf35e45bbd9d 100644 --- a/perception/autoware_image_projection_based_fusion/README.md +++ b/perception/autoware_image_projection_based_fusion/README.md @@ -66,6 +66,12 @@ ros2 launch autoware_image_projection_based_fusion pointpainting_fusion.launch.x The rclcpp::TimerBase timer could not break a for loop, therefore even if time is out when fusing a roi msg at the middle, the program will run until all msgs are fused. +### Approximate camera projection + +For algorithms like `pointpainting_fusion`, the computation required to project points onto an unrectified (raw) image can be substantial. To address this, an option is provided to reduce the computational load. Set the [`approximate_camera_projection parameter`](config/fusion_common.param.yaml) to `true` for each camera (ROIs). If the corresponding `point_project_to_unrectified_image` parameter is also set to true, the projections will be pre-cached. + +The cached projections are calculated using a grid, with the grid size specified by the `approximation_grid_width_size` and `approximation_grid_height_size` parameters in the [configuration file](config/fusion_common.param.yaml). The centers of the grid are used as the projected points. + ### Detail description of each fusion's algorithm is in the following links | Fusion Name | Description | Detail | diff --git a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml index 662362c4fd7c5..86db3bad4f8f8 100644 --- a/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml +++ b/perception/autoware_image_projection_based_fusion/config/fusion_common.param.yaml @@ -4,7 +4,8 @@ timeout_ms: 70.0 match_threshold_ms: 50.0 image_buffer_size: 15 - point_project_to_unrectified_image: false + # projection setting for each ROI whether unrectify image + point_project_to_unrectified_image: [false, false, false, false, false, false] debug_mode: false filter_scope_min_x: -100.0 filter_scope_min_y: -100.0 @@ -12,3 +13,12 @@ filter_scope_max_x: 100.0 filter_scope_max_y: 100.0 filter_scope_max_z: 100.0 + + # camera cache setting for each ROI + approximate_camera_projection: [true, true, true, true, true, true] + # grid size in pixels + approximation_grid_cell_width: 1.0 + approximation_grid_cell_height: 1.0 + + # debug parameters + publish_processing_time_detail: false diff --git a/perception/autoware_image_projection_based_fusion/docs/pointpainting-fusion.md b/perception/autoware_image_projection_based_fusion/docs/pointpainting-fusion.md index 92f62443918c0..21c2140075f51 100644 --- a/perception/autoware_image_projection_based_fusion/docs/pointpainting-fusion.md +++ b/perception/autoware_image_projection_based_fusion/docs/pointpainting-fusion.md @@ -16,18 +16,18 @@ The lidar points are projected onto the output of an image-only 2d object detect | Name | Type | Description | | ------------------------ | -------------------------------------------------------- | --------------------------------------------------------- | -| `input` | `sensor_msgs::msg::PointCloud2` | pointcloud | +| `input/pointcloud` | `sensor_msgs::msg::PointCloud2` | pointcloud | | `input/camera_info[0-7]` | `sensor_msgs::msg::CameraInfo` | camera information to project 3d points onto image planes | | `input/rois[0-7]` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | ROIs from each image | | `input/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | ### Output -| Name | Type | Description | -| ------------------------ | ------------------------------------------------ | ------------------------ | -| `output` | `sensor_msgs::msg::PointCloud2` | painted pointcloud | -| `~/output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | -| `~/debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | +| Name | Type | Description | +| -------------------------- | ------------------------------------------------ | ------------------------ | +| `output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | +| `debug/painted_pointcloud` | `sensor_msgs::msg::PointCloud2` | painted pointcloud | +| `debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | ## Parameters diff --git a/perception/autoware_image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/autoware_image_projection_based_fusion/docs/roi-cluster-fusion.md index 86d3a2fa070b2..e5c5b1d05e9c8 100644 --- a/perception/autoware_image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/autoware_image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -23,10 +23,10 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ### Output -| Name | Type | Description | -| ------------------------ | -------------------------------------------------------- | -------------------------- | -| `output` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | labeled cluster pointcloud | -| `~/debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | +| Name | Type | Description | +| ---------------------- | -------------------------------------------------------- | -------------------------- | +| `output` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | labeled cluster pointcloud | +| `debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization | ## Parameters diff --git a/perception/autoware_image_projection_based_fusion/docs/roi-detected-object-fusion.md b/perception/autoware_image_projection_based_fusion/docs/roi-detected-object-fusion.md index 4dadae5531aa9..1a4652fc814af 100644 --- a/perception/autoware_image_projection_based_fusion/docs/roi-detected-object-fusion.md +++ b/perception/autoware_image_projection_based_fusion/docs/roi-detected-object-fusion.md @@ -30,12 +30,12 @@ The DetectedObject has three possible shape choices/implementations, where the p ### Output -| Name | Type | Description | -| ------------------------- | ------------------------------------------------ | -------------------------- | -| `output` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | -| `~/debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization, | -| `~/debug/fused_objects` | `autoware_perception_msgs::msg::DetectedObjects` | fused detected objects | -| `~/debug/ignored_objects` | `autoware_perception_msgs::msg::DetectedObjects` | not fused detected objects | +| Name | Type | Description | +| ----------------------- | ------------------------------------------------ | -------------------------- | +| `output` | `autoware_perception_msgs::msg::DetectedObjects` | detected objects | +| `debug/image_raw[0-7]` | `sensor_msgs::msg::Image` | images for visualization, | +| `debug/fused_objects` | `autoware_perception_msgs::msg::DetectedObjects` | fused detected objects | +| `debug/ignored_objects` | `autoware_perception_msgs::msg::DetectedObjects` | not fused detected objects | ## Parameters diff --git a/perception/autoware_image_projection_based_fusion/docs/roi-pointcloud-fusion.md b/perception/autoware_image_projection_based_fusion/docs/roi-pointcloud-fusion.md index f2ce7662da976..7727d80003f79 100644 --- a/perception/autoware_image_projection_based_fusion/docs/roi-pointcloud-fusion.md +++ b/perception/autoware_image_projection_based_fusion/docs/roi-pointcloud-fusion.md @@ -24,11 +24,10 @@ The node `roi_pointcloud_fusion` is to cluster the pointcloud based on Region Of ### Output -| Name | Type | Description | -| ----------------- | -------------------------------------------------------- | -------------------------------------------- | -| `output` | `sensor_msgs::msg::PointCloud2` | output pointcloud as default of interface | -| `output_clusters` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters | -| `debug/clusters` | `sensor_msgs/msg/PointCloud2` | colored cluster pointcloud for visualization | +| Name | Type | Description | +| ---------------- | -------------------------------------------------------- | -------------------------------------------- | +| `output` | `tier4_perception_msgs::msg::DetectedObjectsWithFeature` | output clusters | +| `debug/clusters` | `sensor_msgs/msg/PointCloud2` | colored cluster pointcloud for visualization | ## Parameters diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp new file mode 100644 index 0000000000000..f9b3158b1672a --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/camera_projection.hpp @@ -0,0 +1,81 @@ +// 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__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_ +#define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_ + +#define EIGEN_MPL2_ONLY + +#include +#include + +#include + +#include + +#include + +namespace autoware::image_projection_based_fusion +{ +struct PixelPos +{ + float x; + float y; +}; + +class CameraProjection +{ +public: + explicit CameraProjection( + const sensor_msgs::msg::CameraInfo & camera_info, const float grid_cell_width, + const float grid_cell_height, const bool unrectify, const bool use_approximation); + CameraProjection() : cell_width_(1.0), cell_height_(1.0), unrectify_(false) {} + void initialize(); + std::function calcImageProjectedPoint; + sensor_msgs::msg::CameraInfo getCameraInfo(); + bool isOutsideHorizontalView(const float px, const float pz); + bool isOutsideVerticalView(const float py, const float pz); + bool isOutsideFOV(const float px, const float py, const float pz); + +protected: + bool calcRectifiedImageProjectedPoint( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point); + bool calcRawImageProjectedPoint(const cv::Point3d & point3d, Eigen::Vector2d & projected_point); + bool calcRawImageProjectedPointWithApproximation( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point); + void initializeCache(); + + sensor_msgs::msg::CameraInfo camera_info_; + uint32_t image_height_, image_width_; + double tan_h_x_, tan_h_y_; + double fov_left_, fov_right_, fov_top_, fov_bottom_; + + uint32_t cache_size_; + float cell_width_; + float cell_height_; + float inv_cell_width_; + float inv_cell_height_; + int grid_width_; + int grid_height_; + + bool unrectify_; + bool use_approximation_; + + std::unique_ptr projection_cache_; + image_geometry::PinholeCameraModel camera_model_; +}; + +} // namespace autoware::image_projection_based_fusion + +#endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__CAMERA_PROJECTION_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp index c9b4f1f7b903f..964974b96e73d 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/fusion_node.hpp @@ -15,9 +15,11 @@ #ifndef AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ #define AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__FUSION_NODE_HPP_ +#include #include #include #include +#include #include #include @@ -38,7 +40,6 @@ #include #include -#include #include #include #include @@ -50,13 +51,33 @@ using autoware_perception_msgs::msg::DetectedObject; using autoware_perception_msgs::msg::DetectedObjects; using sensor_msgs::msg::CameraInfo; using sensor_msgs::msg::Image; -using sensor_msgs::msg::PointCloud2; -using tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using PointCloudMsgType = sensor_msgs::msg::PointCloud2; +using RoiMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterMsgType = tier4_perception_msgs::msg::DetectedObjectsWithFeature; +using ClusterObjType = tier4_perception_msgs::msg::DetectedObjectWithFeature; using tier4_perception_msgs::msg::DetectedObjectWithFeature; using PointCloud = pcl::PointCloud; +using autoware::image_projection_based_fusion::CameraProjection; using autoware_perception_msgs::msg::ObjectClassification; -template +template +struct Det2dStatus +{ + // camera index + std::size_t id = 0; + // camera projection + std::unique_ptr camera_projector_ptr; + bool project_to_unrectified_image = false; + bool approximate_camera_projection = false; + // process flags + bool is_fused = false; + // timing + double input_offset_ms = 0.0; + // cache + std::map cached_det2d_msgs; +}; + +template class FusionNode : public rclcpp::Node { public: @@ -68,70 +89,71 @@ class FusionNode : public rclcpp::Node explicit FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options, int queue_size); -protected: +private: + // Common process methods void cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id); - virtual void preprocess(TargetMsg3D & output_msg); - - // callback for Msg subscription - virtual void subCallback(const typename TargetMsg3D::ConstSharedPtr input_msg); - - // callback for roi subscription - - virtual void roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); - - virtual void fuseOnSingleImage( - const TargetMsg3D & input_msg, const std::size_t image_id, const Msg2D & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, TargetMsg3D & output_msg) = 0; - - // set args if you need - virtual void postprocess(TargetMsg3D & output_msg); - - virtual void publish(const TargetMsg3D & output_msg); - + // callback for timer void timer_callback(); void setPeriod(const int64_t new_period); - - std::size_t rois_number_{1}; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; - - bool point_project_to_unrectified_image_{false}; - - // camera_info - std::map camera_info_map_; - std::vector::SharedPtr> camera_info_subs_; - + void exportProcess(); + + // 2d detection management methods + bool checkAllDet2dFused() + { + for (const auto & det2d : det2d_list_) { + if (!det2d.is_fused) { + return false; + } + } + return true; + } + + // camera projection + float approx_grid_cell_w_size_; + float approx_grid_cell_h_size_; + + // timer rclcpp::TimerBase::SharedPtr timer_; double timeout_ms_{}; double match_threshold_ms_{}; - std::vector input_rois_topics_; - std::vector input_camera_info_topics_; - std::vector input_camera_topics_; - /** \brief A vector of subscriber. */ - typename rclcpp::Subscription::SharedPtr sub_; std::vector::SharedPtr> rois_subs_; - - // offsets between cameras and the lidars - std::vector input_offset_ms_; + std::vector::SharedPtr> camera_info_subs_; // cache for fusion - std::vector is_fused_; - std::pair - cached_msg_; // first element is the timestamp in nanoseconds, second element is the message - std::vector> cached_roi_msgs_; - std::mutex mutex_cached_msgs_; + int64_t cached_det3d_msg_timestamp_; + typename Msg3D::SharedPtr cached_det3d_msg_ptr_; - // output publisher - typename rclcpp::Publisher::SharedPtr pub_ptr_; +protected: + void setDet2DStatus(std::size_t rois_number); - // debugger - std::shared_ptr debugger_; - virtual bool out_of_scope(const ObjType & obj) = 0; + // callback for main subscription + void subCallback(const typename Msg3D::ConstSharedPtr input_msg); + // callback for roi subscription + void roiCallback(const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i); + + // Custom process methods + virtual void preprocess(Msg3D & output_msg); + virtual void fuseOnSingleImage( + const Msg3D & input_msg, const Det2dStatus & det2d, const Msg2D & input_roi_msg, + Msg3D & output_msg) = 0; + virtual void postprocess(const Msg3D & processing_msg, ExportObj & output_msg); + virtual void publish(const ExportObj & output_msg); + + // Members + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + + // 2d detection management + std::vector> det2d_list_; + + // 3d detection subscription + typename rclcpp::Subscription::SharedPtr det3d_sub_; + + // parameters for out_of_scope filter float filter_scope_min_x_; float filter_scope_max_x_; float filter_scope_min_y_; @@ -139,9 +161,21 @@ class FusionNode : public rclcpp::Node float filter_scope_min_z_; float filter_scope_max_z_; + // output publisher + typename rclcpp::Publisher::SharedPtr pub_ptr_; + + // debugger + std::shared_ptr debugger_; + std::unique_ptr debug_internal_pub_; + /** \brief processing time publisher. **/ std::unique_ptr> stop_watch_ptr_; std::unique_ptr debug_publisher_; + + // timekeeper + rclcpp::Publisher::SharedPtr + detailed_processing_time_publisher_; + std::shared_ptr time_keeper_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp index 8c0e2ed78fc6c..26b4512e345f7 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/node.hpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -41,29 +42,25 @@ inline bool isInsideBbox( proj_y >= roi.y_offset * zc && proj_y <= (roi.y_offset + roi.height) * zc; } -class PointPaintingFusionNode -: public FusionNode +class PointPaintingFusionNode : public FusionNode { public: explicit PointPaintingFusionNode(const rclcpp::NodeOptions & options); -protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; +private: + void preprocess(PointCloudMsgType & pointcloud_msg) override; void fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & painted_pointcloud_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) override; + void postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) override; - rclcpp::Publisher::SharedPtr obj_pub_ptr_; - - std::vector tan_h_; // horizontal field of view + rclcpp::Publisher::SharedPtr painted_point_pub_ptr_; + std::unique_ptr diagnostics_interface_ptr_; int omp_num_threads_{1}; - float score_threshold_{0.0}; std::vector class_names_; std::map class_index_; std::map> isClassTable_; @@ -75,8 +72,6 @@ class PointPaintingFusionNode autoware::lidar_centerpoint::DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; - - bool out_of_scope(const DetectedObjects & obj) override; }; } // namespace autoware::image_projection_based_fusion #endif // AUTOWARE__IMAGE_PROJECTION_BASED_FUSION__POINTPAINTING_FUSION__NODE_HPP_ diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index a89ee3d02cd8b..a1d12bcb56384 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -29,8 +30,8 @@ class PointPaintingTRT : public autoware::lidar_centerpoint::CenterPointTRT using autoware::lidar_centerpoint::CenterPointTRT::CenterPointTRT; explicit PointPaintingTRT( - const autoware::lidar_centerpoint::NetworkParam & encoder_param, - const autoware::lidar_centerpoint::NetworkParam & head_param, + const autoware::tensorrt_common::TrtCommonConfig & encoder_param, + const autoware::tensorrt_common::TrtCommonConfig & head_param, const autoware::lidar_centerpoint::DensificationParam & densification_param, const autoware::lidar_centerpoint::CenterPointConfig & config); diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 2c0283a190023..b7bf8738b68a4 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -24,22 +24,19 @@ namespace autoware::image_projection_based_fusion { const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; -class RoiClusterFusionNode -: public FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature> +class RoiClusterFusionNode : public FusionNode { public: explicit RoiClusterFusionNode(const rclcpp::NodeOptions & options); -protected: - void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override; - void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override; +private: + void preprocess(ClusterMsgType & output_cluster_msg) override; void fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, - DetectedObjectsWithFeature & output_cluster_msg) override; + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) override; + + void postprocess(const ClusterMsgType & output_cluster_msg, ClusterMsgType & output_msg) override; std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; @@ -53,12 +50,12 @@ class RoiClusterFusionNode double fusion_distance_; double trust_object_distance_; std::string non_trust_object_iou_mode_{"iou_x"}; - bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + + bool is_far_enough(const ClusterObjType & obj, const double distance_threshold); + bool out_of_scope(const ClusterObjType & obj); double cal_iou_by_mode( const sensor_msgs::msg::RegionOfInterest & roi_1, const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); - // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp index 7103537ec852d..ba197126277a0 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_detected_object_fusion/node.hpp @@ -31,35 +31,31 @@ namespace autoware::image_projection_based_fusion using sensor_msgs::msg::RegionOfInterest; -class RoiDetectedObjectFusionNode -: public FusionNode +class RoiDetectedObjectFusionNode : public FusionNode { public: explicit RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options); -protected: +private: void preprocess(DetectedObjects & output_msg) override; void fuseOnSingleImage( - const DetectedObjects & input_object_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjects & output_object_msg) override; + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg) override; std::map generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, - const image_geometry::PinholeCameraModel & pinhole_camera_model); + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const Eigen::Affine3d & object2camera_affine); void fuseObjectsOnImage( const DetectedObjects & input_object_msg, const std::vector & image_rois, const std::map & object_roi_map); - void publish(const DetectedObjects & output_msg) override; + void postprocess(const DetectedObjects & processing_msg, DetectedObjects & output_msg) override; - bool out_of_scope(const DetectedObject & obj) override; + bool out_of_scope(const DetectedObject & obj); -private: struct { std::vector passthrough_lower_bound_probability_thresholds{}; diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp index 4de49df61a071..77a1745faa7e5 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/roi_pointcloud_fusion/node.hpp @@ -19,37 +19,35 @@ #include +#include #include #include + namespace autoware::image_projection_based_fusion { -class RoiPointCloudFusionNode -: public FusionNode +class RoiPointCloudFusionNode : public FusionNode { +public: + explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + private: - int min_cluster_size_{1}; - int max_cluster_size_{20}; - bool fuse_unknown_only_{true}; - double cluster_2d_tolerance_; + rclcpp::Publisher::SharedPtr point_pub_ptr_; + rclcpp::Publisher::SharedPtr cluster_debug_pub_; - rclcpp::Publisher::SharedPtr pub_objects_ptr_; - std::vector output_fused_objects_; - rclcpp::Publisher::SharedPtr cluster_debug_pub_; + void fuseOnSingleImage( + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, PointCloudMsgType & output_pointcloud_msg) override; - /* data */ -public: - explicit RoiPointCloudFusionNode(const rclcpp::NodeOptions & options); + void postprocess(const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) override; -protected: - void preprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + void publish(const ClusterMsgType & output_msg) override; - void postprocess(sensor_msgs::msg::PointCloud2 & pointcloud_msg) override; + int min_cluster_size_{1}; + int max_cluster_size_{20}; + bool fuse_unknown_only_{true}; + double cluster_2d_tolerance_; - void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; - bool out_of_scope(const DetectedObjectWithFeature & obj) override; + std::vector output_fused_objects_; }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index b09e9521bdcdb..5414f98e142cd 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -33,10 +34,32 @@ namespace autoware::image_projection_based_fusion { -class SegmentPointCloudFusionNode : public FusionNode +class SegmentPointCloudFusionNode : public FusionNode { +public: + explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); + private: - rclcpp::Publisher::SharedPtr pub_pointcloud_ptr_; + void preprocess(PointCloudMsgType & pointcloud_msg) override; + + void fuseOnSingleImage( + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const Image & input_mask, PointCloudMsgType & output_pointcloud_msg) override; + + inline void copyPointCloud( + const PointCloudMsgType & input, const int point_step, const size_t global_offset, + PointCloudMsgType & output, size_t & output_pointcloud_size) + { + std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); + output_pointcloud_size += point_step; + } + + void postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) override; + + // debug + image_transport::Publisher pub_debug_mask_ptr_; + std::vector filter_semantic_label_target_; float filter_distance_threshold_; // declare list of semantic label target, depend on trained data of yolox segmentation model @@ -46,30 +69,8 @@ class SegmentPointCloudFusionNode : public FusionNode filter_global_offset_set_; - -public: - explicit SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options); - -protected: - void preprocess(PointCloud2 & pointcloud_msg) override; - - void postprocess(PointCloud2 & pointcloud_msg) override; - - void fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, const std::size_t image_id, const Image & input_mask, - const CameraInfo & camera_info, PointCloud2 & output_pointcloud_msg) override; - - bool out_of_scope(const PointCloud2 & filtered_cloud) override; - inline void copyPointCloud( - const PointCloud2 & input, const int point_step, const size_t global_offset, - PointCloud2 & output, size_t & output_pointcloud_size) - { - std::memcpy(&output.data[output_pointcloud_size], &input.data[global_offset], point_step); - output_pointcloud_size += point_step; - } }; } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp index 44679f7509687..044a71ab57533 100644 --- a/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp +++ b/perception/autoware_image_projection_based_fusion/include/autoware/image_projection_based_fusion/utils/utils.hpp @@ -56,7 +56,7 @@ namespace autoware::image_projection_based_fusion { using PointCloud = pcl::PointCloud; -using PointCloud2 = sensor_msgs::msg::PointCloud2; + struct PointData { float distance; @@ -65,10 +65,6 @@ struct PointData bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info); -Eigen::Vector2d calcRawImageProjectedPoint( - const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d, - const bool & unrectify = false); - std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time); @@ -76,17 +72,17 @@ std::optional getTransformStamped( Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t); void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster); + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster); void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects); -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud); +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud); } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml b/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml index c062081fb691c..a221af7fe44d3 100644 --- a/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml +++ b/perception/autoware_image_projection_based_fusion/launch/roi_pointcloud_fusion.launch.xml @@ -17,8 +17,7 @@ - - + @@ -39,8 +38,7 @@ - - + diff --git a/perception/autoware_image_projection_based_fusion/package.xml b/perception/autoware_image_projection_based_fusion/package.xml index 025ed5377fa72..a15642728e1d9 100644 --- a/perception/autoware_image_projection_based_fusion/package.xml +++ b/perception/autoware_image_projection_based_fusion/package.xml @@ -2,7 +2,7 @@ autoware_image_projection_based_fusion - 0.40.0 + 0.41.0 The autoware_image_projection_based_fusion package Yukihiro Saito Shunsuke Miura @@ -17,6 +17,7 @@ autoware_cmake autoware_euclidean_cluster + autoware_internal_debug_msgs autoware_lidar_centerpoint autoware_object_recognition_utils autoware_perception_msgs diff --git a/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json index 73ee1661adaea..8077f3e2f3e30 100644 --- a/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json +++ b/perception/autoware_image_projection_based_fusion/schema/fusion_common.schema.json @@ -31,6 +31,11 @@ "default": 15, "minimum": 1 }, + "point_project_to_unrectified_image": { + "type": "array", + "description": "An array of options indicating whether to project point to unrectified image or not.", + "default": [false, false, false, false, false, false] + }, "debug_mode": { "type": "boolean", "description": "Whether to debug or not.", @@ -65,6 +70,21 @@ "type": "number", "description": "Maximum z position [m].", "default": 100.0 + }, + "approximate_camera_projection": { + "type": "array", + "description": "An array of options indicating whether to use approximated projection for each camera.", + "default": [true, true, true, true, true, true] + }, + "approximation_grid_cell_width": { + "type": "number", + "description": "The width of grid cell used in approximated projection [pixel].", + "default": 1.0 + }, + "approximation_grid_cell_height": { + "type": "number", + "description": "The height of grid cell used in approximated projection [pixel].", + "default": 1.0 } } } diff --git a/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp b/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp new file mode 100644 index 0000000000000..c0a820609a0a7 --- /dev/null +++ b/perception/autoware_image_projection_based_fusion/src/camera_projection.cpp @@ -0,0 +1,252 @@ +// 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/image_projection_based_fusion/camera_projection.hpp" + +#include + +namespace autoware::image_projection_based_fusion +{ +CameraProjection::CameraProjection( + const sensor_msgs::msg::CameraInfo & camera_info, const float grid_cell_width, + const float grid_cell_height, const bool unrectify, const bool use_approximation = false) +: camera_info_(camera_info), + cell_width_(grid_cell_width), + cell_height_(grid_cell_height), + unrectify_(unrectify), + use_approximation_(use_approximation) +{ + if (grid_cell_width <= 0.0 || grid_cell_height <= 0.0) { + throw std::runtime_error("Both grid_cell_width and grid_cell_height must be > 0.0"); + } + + image_width_ = camera_info.width; + image_height_ = camera_info.height; + + // prepare camera model + camera_model_.fromCameraInfo(camera_info); + + // cache settings + inv_cell_width_ = 1 / cell_width_; + inv_cell_height_ = 1 / cell_height_; + grid_width_ = static_cast(std::ceil(image_width_ / cell_width_)); + grid_height_ = static_cast(std::ceil(image_height_ / cell_height_)); + cache_size_ = grid_width_ * grid_height_; + + // compute 3D rays for the image corners and pixels related to optical center + // to find the actual FOV size + // optical centers + const double ocx = static_cast(camera_info.k.at(2)); + const double ocy = static_cast(camera_info.k.at(5)); + // for checking pincushion shape case + const cv::Point3d ray_top_left = camera_model_.projectPixelTo3dRay(cv::Point(0, 0)); + const cv::Point3d ray_top_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, 0)); + const cv::Point3d ray_bottom_left = + camera_model_.projectPixelTo3dRay(cv::Point(0, image_height_ - 1)); + const cv::Point3d ray_bottom_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, image_height_ - 1)); + // for checking barrel shape case + const cv::Point3d ray_mid_top = camera_model_.projectPixelTo3dRay(cv::Point(ocx, 0)); + const cv::Point3d ray_mid_left = camera_model_.projectPixelTo3dRay(cv::Point(0, ocy)); + const cv::Point3d ray_mid_right = + camera_model_.projectPixelTo3dRay(cv::Point(image_width_ - 1, ocy)); + const cv::Point3d ray_mid_bottom = + camera_model_.projectPixelTo3dRay(cv::Point(ocx, image_height_ - 1)); + + cv::Point3d x_left = ray_top_left; + cv::Point3d x_right = ray_top_right; + cv::Point3d y_top = ray_top_left; + cv::Point3d y_bottom = ray_bottom_left; + + // check each side of the view + if (ray_bottom_left.x < x_left.x) x_left = ray_bottom_left; + if (ray_mid_left.x < x_left.x) x_left = ray_mid_left; + + if (x_right.x < ray_bottom_right.x) x_right = ray_bottom_right; + if (x_right.x < ray_mid_right.x) x_right = ray_mid_right; + + if (y_top.y < ray_top_right.y) y_top = ray_top_right; + if (y_top.y < ray_mid_top.y) y_top = ray_mid_top; + + if (ray_bottom_left.y < y_bottom.y) y_bottom = ray_bottom_left; + if (ray_mid_bottom.y < y_bottom.y) y_bottom = ray_mid_bottom; + + // set FOV at z = 1.0 + fov_left_ = x_left.x / x_left.z; + fov_right_ = x_right.x / x_right.z; + fov_top_ = y_top.y / y_top.z; + fov_bottom_ = y_bottom.y / y_bottom.z; +} + +void CameraProjection::initialize() +{ + if (unrectify_) { + if (use_approximation_) { + // create the cache with size of grid center + // store only xy position in float to reduce memory consumption + projection_cache_ = std::make_unique(cache_size_); + initializeCache(); + + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRawImageProjectedPointWithApproximation(point3d, projected_point); + }; + } else { + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRawImageProjectedPoint(point3d, projected_point); + }; + } + } else { + calcImageProjectedPoint = [this]( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) { + return this->calcRectifiedImageProjectedPoint(point3d, projected_point); + }; + } +} + +void CameraProjection::initializeCache() +{ + // sample grid cell centers till the camera height, width to precompute the projections + // + // grid cell size + // / + // v + // |---| w + // 0-----------------> + // 0 | . | . | . | + // |___|___|___| + // | . | . | . | + // | ^ + // h | | + // v grid cell center + // + // each pixel will be rounded in these grid cell centers + // edge pixels in right and bottom side in the image will be assign to these centers + // that is the outside of the image + + for (int idx_y = 0; idx_y < grid_height_; idx_y++) { + for (int idx_x = 0; idx_x < grid_width_; idx_x++) { + const uint32_t grid_index = idx_y * grid_width_ + idx_x; + const float px = (idx_x + 0.5f) * cell_width_; + const float py = (idx_y + 0.5f) * cell_height_; + + // precompute projected point + cv::Point2d raw_image_point = camera_model_.unrectifyPoint(cv::Point2d(px, py)); + projection_cache_[grid_index] = + PixelPos{static_cast(raw_image_point.x), static_cast(raw_image_point.y)}; + } + } +} + +/** + * @brief Calculate a projection of 3D point to rectified image plane 2D point. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRectifiedImageProjectedPoint( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + + if ( + rectified_image_point.x < 0.0 || rectified_image_point.x >= image_width_ || + rectified_image_point.y < 0.0 || rectified_image_point.y >= image_height_) { + return false; + } else { + projected_point << rectified_image_point.x, rectified_image_point.y; + return true; + } +} + +/** + * @brief Calculate a projection of 3D point to raw image plane 2D point. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRawImageProjectedPoint( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + const cv::Point2d raw_image_point = camera_model_.unrectifyPoint(rectified_image_point); + + if ( + rectified_image_point.x < 0.0 || rectified_image_point.x >= image_width_ || + rectified_image_point.y < 0.0 || rectified_image_point.y >= image_height_) { + return false; + } else { + projected_point << raw_image_point.x, raw_image_point.y; + return true; + } +} + +/** + * @brief Calculate a projection of 3D point to raw image plane 2D point with approximation. + * @return Return a boolean indicating whether the projected point is on the image plane. + */ +bool CameraProjection::calcRawImageProjectedPointWithApproximation( + const cv::Point3d & point3d, Eigen::Vector2d & projected_point) +{ + const cv::Point2d rectified_image_point = camera_model_.project3dToPixel(point3d); + + // round to a near grid cell + const int grid_idx_x = static_cast(std::floor(rectified_image_point.x * inv_cell_width_)); + const int grid_idx_y = static_cast(std::floor(rectified_image_point.y * inv_cell_height_)); + + if (grid_idx_x < 0.0 || grid_idx_x >= grid_width_) return false; + if (grid_idx_y < 0.0 || grid_idx_y >= grid_height_) return false; + + const uint32_t grid_index = grid_idx_y * grid_width_ + grid_idx_x; + projected_point << projection_cache_[grid_index].x, projection_cache_[grid_index].y; + + return true; +} + +sensor_msgs::msg::CameraInfo CameraProjection::getCameraInfo() +{ + return camera_info_; +} + +bool CameraProjection::isOutsideHorizontalView(const float px, const float pz) +{ + // assuming the points' origin is centered on the camera + if (pz <= 0.0) return true; + if (px < fov_left_ * pz) return true; + if (px > fov_right_ * pz) return true; + + // inside of the horizontal view + return false; +} + +bool CameraProjection::isOutsideVerticalView(const float py, const float pz) +{ + // assuming the points' origin is centered on the camera + if (pz <= 0.0) return true; + // up in the camera coordinate is negative + if (py < fov_top_ * pz) return true; + if (py > fov_bottom_ * pz) return true; + + // inside of the vertical view + return false; +} + +bool CameraProjection::isOutsideFOV(const float px, const float py, const float pz) +{ + if (isOutsideHorizontalView(px, pz)) return true; + if (isOutsideVerticalView(py, pz)) return true; + + // inside of the FOV + return false; +} + +} // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp index 6c575752b52e5..53b18e20883bc 100644 --- a/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/fusion_node.cpp @@ -18,7 +18,9 @@ #include #include +#include +#include #include #include @@ -27,6 +29,7 @@ #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -42,80 +45,69 @@ static double processing_time_ms = 0; namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; -template -FusionNode::FusionNode( +template +FusionNode::FusionNode( const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { // set rois_number - rois_number_ = static_cast(declare_parameter("rois_number")); - if (rois_number_ < 1) { - RCLCPP_WARN( - this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number_); - rois_number_ = 1; + const std::size_t rois_number = + static_cast(declare_parameter("rois_number")); + if (rois_number < 1) { + RCLCPP_ERROR( + this->get_logger(), "minimum rois_number is 1. current rois_number is %zu", rois_number); } - if (rois_number_ > 8) { + if (rois_number > 8) { RCLCPP_WARN( this->get_logger(), - "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number_); + "Current rois_number is %zu. Large rois number may cause performance issue.", rois_number); } // Set parameters match_threshold_ms_ = declare_parameter("match_threshold_ms"); timeout_ms_ = declare_parameter("timeout_ms"); - input_rois_topics_.resize(rois_number_); - input_camera_topics_.resize(rois_number_); - input_camera_info_topics_.resize(rois_number_); + std::vector input_rois_topics; + std::vector input_camera_info_topics; + + input_rois_topics.resize(rois_number); + input_camera_info_topics.resize(rois_number); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - input_rois_topics_.at(roi_i) = declare_parameter( + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_rois_topics.at(roi_i) = declare_parameter( "input/rois" + std::to_string(roi_i), "/perception/object_recognition/detection/rois" + std::to_string(roi_i)); - input_camera_info_topics_.at(roi_i) = declare_parameter( + input_camera_info_topics.at(roi_i) = declare_parameter( "input/camera_info" + std::to_string(roi_i), "/sensing/camera/camera" + std::to_string(roi_i) + "/camera_info"); - - input_camera_topics_.at(roi_i) = declare_parameter( - "input/image" + std::to_string(roi_i), - "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); - } - - input_offset_ms_ = declare_parameter>("input_offset_ms"); - if (!input_offset_ms_.empty() && rois_number_ > input_offset_ms_.size()) { - throw std::runtime_error("The number of offsets does not match the number of topics."); } - // sub camera info - camera_info_subs_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // subscribe camera info + camera_info_subs_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function fnc = std::bind(&FusionNode::cameraInfoCallback, this, std::placeholders::_1, roi_i); camera_info_subs_.at(roi_i) = this->create_subscription( - input_camera_info_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); + input_camera_info_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), fnc); } - // sub rois - rois_subs_.resize(rois_number_); - cached_roi_msgs_.resize(rois_number_); - is_fused_.resize(rois_number_, false); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { + // subscribe rois + rois_subs_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { std::function roi_callback = std::bind(&FusionNode::roiCallback, this, std::placeholders::_1, roi_i); rois_subs_.at(roi_i) = this->create_subscription( - input_rois_topics_.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); + input_rois_topics.at(roi_i), rclcpp::QoS{1}.best_effort(), roi_callback); } - // subscribers - std::function sub_callback = + // subscribe 3d detection + std::function sub_callback = std::bind(&FusionNode::subCallback, this, std::placeholders::_1); - sub_ = - this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); - - // publisher - pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + det3d_sub_ = + this->create_subscription("input", rclcpp::QoS(1).best_effort(), sub_callback); // Set timer const auto period_ns = std::chrono::duration_cast( @@ -123,82 +115,177 @@ FusionNode::FusionNode( timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&FusionNode::timer_callback, this)); + // initialization on each 2d detections + setDet2DStatus(rois_number); + + // parameters for approximation grid + approx_grid_cell_w_size_ = declare_parameter("approximation_grid_cell_width"); + approx_grid_cell_h_size_ = declare_parameter("approximation_grid_cell_height"); + + // parameters for out_of_scope filter + filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); + filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); + filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); + filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); + filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); + filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); + // debugger if (declare_parameter("debug_mode", false)) { + std::vector input_camera_topics; + input_camera_topics.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + input_camera_topics.at(roi_i) = declare_parameter( + "input/image" + std::to_string(roi_i), + "/sensing/camera/camera" + std::to_string(roi_i) + "/image_rect_color"); + } std::size_t image_buffer_size = static_cast(declare_parameter("image_buffer_size")); debugger_ = - std::make_shared(this, rois_number_, image_buffer_size, input_camera_topics_); + std::make_shared(this, rois_number, image_buffer_size, input_camera_topics); + + // input topic timing publisher + debug_internal_pub_ = + std::make_unique(this, get_name()); + } + + // time keeper + bool use_time_keeper = declare_parameter("publish_processing_time_detail"); + if (use_time_keeper) { + detailed_processing_time_publisher_ = + this->create_publisher( + "~/debug/processing_time_detail_ms", 1); + auto time_keeper = autoware::universe_utils::TimeKeeper(detailed_processing_time_publisher_); + time_keeper_ = std::make_shared(time_keeper); } - point_project_to_unrectified_image_ = - declare_parameter("point_project_to_unrectified_image"); // initialize debug tool { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, get_name()); + stop_watch_ptr_ = + std::make_unique>(); + debug_publisher_ = std::make_unique(this, get_name()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } +} + +template +void FusionNode::setDet2DStatus(std::size_t rois_number) +{ + // camera offset settings + std::vector input_offset_ms = declare_parameter>("input_offset_ms"); + if (!input_offset_ms.empty() && rois_number > input_offset_ms.size()) { + throw std::runtime_error("The number of offsets does not match the number of topics."); + } - filter_scope_min_x_ = declare_parameter("filter_scope_min_x"); - filter_scope_max_x_ = declare_parameter("filter_scope_max_x"); - filter_scope_min_y_ = declare_parameter("filter_scope_min_y"); - filter_scope_max_y_ = declare_parameter("filter_scope_max_y"); - filter_scope_min_z_ = declare_parameter("filter_scope_min_z"); - filter_scope_max_z_ = declare_parameter("filter_scope_max_z"); + // camera projection settings + std::vector point_project_to_unrectified_image = + declare_parameter>("point_project_to_unrectified_image"); + if (rois_number > point_project_to_unrectified_image.size()) { + throw std::runtime_error( + "The number of point_project_to_unrectified_image does not match the number of rois " + "topics."); + } + std::vector approx_camera_projection = + declare_parameter>("approximate_camera_projection"); + if (rois_number != approx_camera_projection.size()) { + const std::size_t current_size = approx_camera_projection.size(); + RCLCPP_WARN( + get_logger(), + "The number of elements in approximate_camera_projection should be the same as in " + "rois_number. " + "It has %zu elements.", + current_size); + if (current_size < rois_number) { + approx_camera_projection.resize(rois_number); + for (std::size_t i = current_size; i < rois_number; i++) { + approx_camera_projection.at(i) = true; + } + } + } + + // 2d detection status initialization + det2d_list_.resize(rois_number); + for (std::size_t roi_i = 0; roi_i < rois_number; ++roi_i) { + det2d_list_.at(roi_i).id = roi_i; + det2d_list_.at(roi_i).project_to_unrectified_image = + point_project_to_unrectified_image.at(roi_i); + det2d_list_.at(roi_i).approximate_camera_projection = approx_camera_projection.at(roi_i); + det2d_list_.at(roi_i).input_offset_ms = input_offset_ms.at(roi_i); + } } -template -void FusionNode::cameraInfoCallback( +template +void FusionNode::cameraInfoCallback( const sensor_msgs::msg::CameraInfo::ConstSharedPtr input_camera_info_msg, const std::size_t camera_id) { - camera_info_map_[camera_id] = *input_camera_info_msg; + // create the CameraProjection when the camera info arrives for the first time + // assuming the camera info does not change while the node is running + auto & det2d = det2d_list_.at(camera_id); + if (!det2d.camera_projector_ptr && checkCameraInfo(*input_camera_info_msg)) { + det2d.camera_projector_ptr = std::make_unique( + *input_camera_info_msg, approx_grid_cell_w_size_, approx_grid_cell_h_size_, + det2d.project_to_unrectified_image, det2d.approximate_camera_projection); + det2d.camera_projector_ptr->initialize(); + } } -template -void FusionNode::preprocess(TargetMsg3D & ouput_msg - __attribute__((unused))) +template +void FusionNode::preprocess(Msg3D & ouput_msg __attribute__((unused))) { // do nothing by default } -template -void FusionNode::subCallback( - const typename TargetMsg3D::ConstSharedPtr input_msg) +template +void FusionNode::exportProcess() +{ + timer_->cancel(); + + ExportObj output_msg; + postprocess(*(cached_det3d_msg_ptr_), output_msg); + publish(output_msg); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - cached_det3d_msg_ptr_->header.stamp).nanoseconds())) + .count(); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", + processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); + debug_publisher_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + processing_time_ms = 0; + } + cached_det3d_msg_ptr_ = nullptr; +} + +template +void FusionNode::subCallback( + const typename Msg3D::ConstSharedPtr det3d_msg) { - if (cached_msg_.second != nullptr) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + if (cached_det3d_msg_ptr_ != nullptr) { + // PROCESS: if the main message is remained (and roi is not collected all) publish the main + // message may processed partially with arrived 2d rois stop_watch_ptr_->toc("processing_time", true); - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - cached_msg_.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - debug_publisher_->publish( - "debug/pipeline_latency_ms", pipeline_latency_ms); - processing_time_ms = 0; - } + exportProcess(); - cached_msg_.second = nullptr; + // reset flags + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; + } } - std::lock_guard lock(mutex_cached_msgs_); + // TIMING: reset timer to the timeout time auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); try { @@ -210,154 +297,141 @@ void FusionNode::subCallback( stop_watch_ptr_->toc("processing_time", true); - typename TargetMsg3D::SharedPtr output_msg = std::make_shared(*input_msg); - + // PROCESS: preprocess the main message + typename Msg3D::SharedPtr output_msg = std::make_shared(*det3d_msg); preprocess(*output_msg); + // PROCESS: fuse the main message with the cached roi messages + // (please ask maintainers before parallelize this loop because debugger is not thread safe) int64_t timestamp_nsec = (*output_msg).header.stamp.sec * static_cast(1e9) + (*output_msg).header.stamp.nanosec; + // for loop for each roi + for (auto & det2d : det2d_list_) { + const auto roi_i = det2d.id; - // if matching rois exist, fuseOnSingle - // please ask maintainers before parallelize this loop because debugger is not thread safe - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + // check camera info + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); continue; } - - if ((cached_roi_msgs_.at(roi_i)).size() > 0) { - int64_t min_interval = 1e9; - int64_t matched_stamp = -1; - std::list outdate_stamps; - - for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { - int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * static_cast(1e6); - int64_t interval = abs(static_cast(k) - new_stamp); - - if ( - interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { - min_interval = interval; - matched_stamp = k; - } else if ( - static_cast(k) < new_stamp && - interval > match_threshold_ms_ * static_cast(1e6)) { - outdate_stamps.push_back(static_cast(k)); - } + auto & det2d_msgs = det2d.cached_det2d_msgs; + + // check if the roi is collected + if (det2d_msgs.size() == 0) continue; + + // MATCH: get the closest roi message, and remove outdated messages + int64_t min_interval = 1e9; + int64_t matched_stamp = -1; + std::list outdate_stamps; + for (const auto & [roi_stamp, value] : det2d_msgs) { + int64_t new_stamp = timestamp_nsec + det2d.input_offset_ms * static_cast(1e6); + int64_t interval = abs(static_cast(roi_stamp) - new_stamp); + + if (interval <= min_interval && interval <= match_threshold_ms_ * static_cast(1e6)) { + min_interval = interval; + matched_stamp = roi_stamp; + } else if ( + static_cast(roi_stamp) < new_stamp && + interval > match_threshold_ms_ * static_cast(1e6)) { + outdate_stamps.push_back(static_cast(roi_stamp)); } + } + for (auto stamp : outdate_stamps) { + det2d_msgs.erase(stamp); + } - // remove outdated stamps - for (auto stamp : outdate_stamps) { - (cached_roi_msgs_.at(roi_i)).erase(stamp); + // PROCESS: if matched, fuse the main message with the roi message + if (matched_stamp != -1) { + if (debugger_) { + debugger_->clear(); } - // fuseOnSingle - if (matched_stamp != -1) { - if (debugger_) { - debugger_->clear(); - } + fuseOnSingleImage(*det3d_msg, det2d, *(det2d_msgs[matched_stamp]), *output_msg); + det2d_msgs.erase(matched_stamp); + det2d.is_fused = true; - fuseOnSingleImage( - *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), - camera_info_map_.at(roi_i), *output_msg); - (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); - is_fused_.at(roi_i) = true; - - // add timestamp interval for debug - if (debug_publisher_) { - double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_publisher_->publish( - "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); - } + // add timestamp interval for debug + if (debug_internal_pub_) { + double timestamp_interval_ms = (matched_stamp - timestamp_nsec) / 1e6; + debug_internal_pub_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); + debug_internal_pub_->publish( + "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", + timestamp_interval_ms - det2d.input_offset_ms); } } } - // if all camera fused, postprocess; else, publish the old Msg(if exists) and cache the current - // Msg - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*output_msg); - publish(*output_msg); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - processing_time_ms = 0; + // PROCESS: check if the fused message is ready to publish + cached_det3d_msg_timestamp_ = timestamp_nsec; + cached_det3d_msg_ptr_ = output_msg; + if (checkAllDet2dFused()) { + // if all camera fused, postprocess and publish the main message + exportProcess(); + + // reset flags + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } } else { - cached_msg_.first = timestamp_nsec; - cached_msg_.second = output_msg; + // if all of rois are not collected, publish the old Msg(if exists) and cache the + // current Msg processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } } -template -void FusionNode::roiCallback( - const typename Msg2D::ConstSharedPtr input_roi_msg, const std::size_t roi_i) +template +void FusionNode::roiCallback( + const typename Msg2D::ConstSharedPtr det2d_msg, const std::size_t roi_i) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + stop_watch_ptr_->toc("processing_time", true); - int64_t timestamp_nsec = (*input_roi_msg).header.stamp.sec * static_cast(1e9) + - (*input_roi_msg).header.stamp.nanosec; + auto & det2d = det2d_list_.at(roi_i); + int64_t timestamp_nsec = + (*det2d_msg).header.stamp.sec * static_cast(1e9) + (*det2d_msg).header.stamp.nanosec; // if cached Msg exist, try to match - if (cached_msg_.second != nullptr) { - int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * static_cast(1e6); + if (cached_det3d_msg_ptr_ != nullptr) { + int64_t new_stamp = + cached_det3d_msg_timestamp_ + det2d.input_offset_ms * static_cast(1e6); int64_t interval = abs(timestamp_nsec - new_stamp); - if ( - interval < match_threshold_ms_ * static_cast(1e6) && is_fused_.at(roi_i) == false) { - if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { + // PROCESS: if matched, fuse the main message with the roi message + if (interval < match_threshold_ms_ * static_cast(1e6) && det2d.is_fused == false) { + // check camera info + if (det2d.camera_projector_ptr == nullptr) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; return; } + if (debugger_) { debugger_->clear(); } + // PROCESS: fuse the main message with the roi message + fuseOnSingleImage(*(cached_det3d_msg_ptr_), det2d, *det2d_msg, *(cached_det3d_msg_ptr_)); + det2d.is_fused = true; - fuseOnSingleImage( - *(cached_msg_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), - *(cached_msg_.second)); - is_fused_.at(roi_i) = true; - - if (debug_publisher_) { - double timestamp_interval_ms = (timestamp_nsec - cached_msg_.first) / 1e6; - debug_publisher_->publish( + if (debug_internal_pub_) { + double timestamp_interval_ms = (timestamp_nsec - cached_det3d_msg_timestamp_) / 1e6; + debug_internal_pub_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); - debug_publisher_->publish( + debug_internal_pub_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_offset_ms", - timestamp_interval_ms - input_offset_ms_.at(roi_i)); + timestamp_interval_ms - det2d.input_offset_ms); } - if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { - timer_->cancel(); - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; + // PROCESS: if all camera fused, postprocess and publish the main message + if (checkAllDet2dFused()) { + exportProcess(); + // reset flags + for (auto & status : det2d_list_) { + status.is_fused = false; } } processing_time_ms = processing_time_ms + stop_watch_ptr_->toc("processing_time", true); @@ -365,57 +439,32 @@ void FusionNode::roiCallback( } } // store roi msg if not matched - (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; + det2d.cached_det2d_msgs[timestamp_nsec] = det2d_msg; } -template -void FusionNode::postprocess(TargetMsg3D & output_msg - __attribute__((unused))) +template +void FusionNode::timer_callback() { - // do nothing by default -} + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); -template -void FusionNode::timer_callback() -{ using std::chrono_literals::operator""ms; timer_->cancel(); - if (mutex_cached_msgs_.try_lock()) { - // timeout, postprocess cached msg - if (cached_msg_.second != nullptr) { - stop_watch_ptr_->toc("processing_time", true); - - postprocess(*(cached_msg_.second)); - publish(*(cached_msg_.second)); - - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", - processing_time_ms + stop_watch_ptr_->toc("processing_time", true)); - processing_time_ms = 0; - } - } - std::fill(is_fused_.begin(), is_fused_.end(), false); - cached_msg_.second = nullptr; - mutex_cached_msgs_.unlock(); - } else { - try { - std::chrono::nanoseconds period = 10ms; - setPeriod(period.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - } - timer_->reset(); + // PROCESS: if timeout, postprocess cached msg + if (cached_det3d_msg_ptr_ != nullptr) { + stop_watch_ptr_->toc("processing_time", true); + exportProcess(); + } + + // reset flags whether the message is fused or not + for (auto & det2d : det2d_list_) { + det2d.is_fused = false; } } -template -void FusionNode::setPeriod(const int64_t new_period) +template +void FusionNode::setPeriod(const int64_t new_period) { if (!timer_) { return; @@ -431,8 +480,16 @@ void FusionNode::setPeriod(const int64_t new_period) } } -template -void FusionNode::publish(const TargetMsg3D & output_msg) +template +void FusionNode::postprocess( + const Msg3D & processing_msg __attribute__((unused)), + ExportObj & output_msg __attribute__((unused))) +{ + // do nothing by default +} + +template +void FusionNode::publish(const ExportObj & output_msg) { if (pub_ptr_->get_subscription_count() < 1) { return; @@ -440,10 +497,21 @@ void FusionNode::publish(const TargetMsg3D & output_msg pub_ptr_->publish(output_msg); } -template class FusionNode; -template class FusionNode< - DetectedObjectsWithFeature, DetectedObjectWithFeature, DetectedObjectsWithFeature>; -template class FusionNode; -template class FusionNode; -template class FusionNode; +// Explicit instantiation for the supported types + +// pointpainting fusion +template class FusionNode; + +// roi cluster fusion +template class FusionNode; + +// roi detected-object fusion +template class FusionNode; + +// roi pointcloud fusion +template class FusionNode; + +// segment pointcloud fusion +template class FusionNode; + } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp index d4601e26dda46..91142612f6e3b 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -35,6 +36,7 @@ namespace { +using autoware::universe_utils::ScopedTimeTrack; Eigen::Affine3f _transformToEigen(const geometry_msgs::msg::Transform & t) { @@ -89,8 +91,7 @@ inline bool isUnknown(int label2d) } PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "pointpainting_fusion", options) +: FusionNode("pointpainting_fusion", options) { omp_num_threads_ = this->declare_parameter("omp_params.num_threads"); const float score_threshold = @@ -154,17 +155,16 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt const auto min_area_matrix = this->declare_parameter>("min_area_matrix"); const auto max_area_matrix = this->declare_parameter>("max_area_matrix"); - std::function sub_callback = + // subscriber + std::function sub_callback = std::bind(&PointPaintingFusionNode::subCallback, this, std::placeholders::_1); - sub_ = this->create_subscription( + det3d_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS().keep_last(3), sub_callback); - tan_h_.resize(rois_number_); - for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { - auto fx = camera_info_map_[roi_i].k.at(0); - auto x0 = camera_info_map_[roi_i].k.at(2); - tan_h_[roi_i] = x0 / fx; - } + // publisher + pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + painted_point_pub_ptr_ = + this->create_publisher("~/debug/painted_pointcloud", rclcpp::QoS{1}); detection_class_remapper_.setParameters( allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); @@ -177,10 +177,10 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt iou_bev_nms_.setParameters(p); } - autoware::lidar_centerpoint::NetworkParam encoder_param( - encoder_onnx_path, encoder_engine_path, trt_precision); - autoware::lidar_centerpoint::NetworkParam head_param( - head_onnx_path, head_engine_path, trt_precision); + autoware::tensorrt_common::TrtCommonConfig encoder_param( + encoder_onnx_path, trt_precision, encoder_engine_path); + autoware::tensorrt_common::TrtCommonConfig head_param( + head_onnx_path, trt_precision, head_engine_path); autoware::lidar_centerpoint::DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); autoware::lidar_centerpoint::CenterPointConfig config( @@ -191,8 +191,8 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt // create detector detector_ptr_ = std::make_unique( encoder_param, head_param, densification_param, config); - - obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + diagnostics_interface_ptr_ = + std::make_unique(this, "pointpainting_trt"); if (this->declare_parameter("build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); @@ -200,8 +200,11 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt } } -void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::preprocess(PointCloudMsgType & painted_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); @@ -254,11 +257,9 @@ void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted } void PointPaintingFusionNode::fuseOnSingleImage( - __attribute__((unused)) const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - __attribute__((unused)) const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, - sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) + __attribute__((unused)) const PointCloudMsgType & input_pointcloud_msg, + const Det2dStatus & det2d, const RoiMsgType & input_roi_msg, + PointCloudMsgType & painted_pointcloud_msg) { if (painted_pointcloud_msg.data.empty() || painted_pointcloud_msg.fields.empty()) { RCLCPP_WARN_STREAM_THROTTLE( @@ -271,7 +272,8 @@ void PointPaintingFusionNode::fuseOnSingleImage( return; } - if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); std::vector debug_image_rois; std::vector debug_image_points; @@ -289,10 +291,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( lidar2cam_affine = _transformToEigen(transform_stamped_optional.value().transform); } - // transform - // sensor_msgs::msg::PointCloud2 transformed_pointcloud; - // tf2::doTransform(painted_pointcloud_msg, transformed_pointcloud, transform_stamped); - const auto x_offset = painted_pointcloud_msg.fields .at(static_cast(autoware::point_types::PointXYZIRCIndex::X)) .offset; @@ -304,9 +302,6 @@ void PointPaintingFusionNode::fuseOnSingleImage( .offset; const auto class_offset = painted_pointcloud_msg.fields.at(4).offset; const auto p_step = painted_pointcloud_msg.point_step; - // projection matrix - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); Eigen::Vector3f point_lidar, point_camera; /** dc : don't care @@ -322,66 +317,87 @@ dc | dc dc dc dc ||zc| // iterate points // Requires 'OMP_NUM_THREADS=N' omp_set_num_threads(omp_num_threads_); -#pragma omp parallel for - for (int i = 0; i < iterations; i++) { - int stride = p_step * i; - unsigned char * data = &painted_pointcloud_msg.data[0]; - unsigned char * output = &painted_pointcloud_msg.data[0]; - // cppcheck-suppress-begin invalidPointerCast - float p_x = *reinterpret_cast(&data[stride + x_offset]); - float p_y = *reinterpret_cast(&data[stride + y_offset]); - float p_z = *reinterpret_cast(&data[stride + z_offset]); - // cppcheck-suppress-end invalidPointerCast - point_lidar << p_x, p_y, p_z; - point_camera = lidar2cam_affine * point_lidar; - p_x = point_camera.x(); - p_y = point_camera.y(); - p_z = point_camera.z(); - - if (p_z <= 0.0 || p_x > (tan_h_.at(image_id) * p_z) || p_x < (-tan_h_.at(image_id) * p_z)) { - continue; - } - // project - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(p_x, p_y, p_z), point_project_to_unrectified_image_); - - // iterate 2d bbox - for (const auto & feature_object : objects) { - sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; - // paint current point if it is inside bbox - int label2d = feature_object.object.classification.front().label; - if (!isUnknown(label2d) && isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { - // cppcheck-suppress invalidPointerCast - auto p_class = reinterpret_cast(&output[stride + class_offset]); - for (const auto & cls : isClassTable_) { - // add up the class values if the point belongs to multiple classes - *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; - } + std::vector> local_vectors(omp_num_threads_); +#pragma omp parallel + { +#pragma omp for + for (int i = 0; i < iterations; i++) { + int stride = p_step * i; + unsigned char * data = &painted_pointcloud_msg.data[0]; + unsigned char * output = &painted_pointcloud_msg.data[0]; + // cppcheck-suppress-begin invalidPointerCast + float p_x = *reinterpret_cast(&data[stride + x_offset]); + float p_y = *reinterpret_cast(&data[stride + y_offset]); + float p_z = *reinterpret_cast(&data[stride + z_offset]); + // cppcheck-suppress-end invalidPointerCast + point_lidar << p_x, p_y, p_z; + point_camera = lidar2cam_affine * point_lidar; + p_x = point_camera.x(); + p_y = point_camera.y(); + p_z = point_camera.z(); + + if (det2d.camera_projector_ptr->isOutsideHorizontalView(p_x, p_z)) { + continue; } -#if 0 - // Parallelizing loop don't support push_back - if (debugger_) { - debug_image_points.push_back(projected_point); + + // project + Eigen::Vector2d projected_point; + if (det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(p_x, p_y, p_z), projected_point)) { + // iterate 2d bbox + for (const auto & feature_object : objects) { + sensor_msgs::msg::RegionOfInterest roi = feature_object.feature.roi; + // paint current point if it is inside bbox + int label2d = feature_object.object.classification.front().label; + if ( + !isUnknown(label2d) && + isInsideBbox(projected_point.x(), projected_point.y(), roi, 1.0)) { + // cppcheck-suppress invalidPointerCast + auto p_class = reinterpret_cast(&output[stride + class_offset]); + for (const auto & cls : isClassTable_) { + // add up the class values if the point belongs to multiple classes + *p_class = cls.second(label2d) ? (class_index_[cls.first] + *p_class) : *p_class; + } + } + if (debugger_) { + int thread_id = omp_get_thread_num(); + local_vectors[thread_id].push_back(projected_point); + } + } } -#endif } - } + if (debugger_) { + std::unique_ptr inner_st_ptr; + if (time_keeper_) + inner_st_ptr = std::make_unique("publish debug message", *time_keeper_); - for (const auto & feature_object : input_roi_msg.feature_objects) { - debug_image_rois.push_back(feature_object.feature.roi); - } + for (const auto & feature_object : input_roi_msg.feature_objects) { + debug_image_rois.push_back(feature_object.feature.roi); + } + + for (const auto & local_vec : local_vectors) { + debug_image_points.insert(debug_image_points.end(), local_vec.begin(), local_vec.end()); + } - if (debugger_) { - debugger_->image_rois_ = debug_image_rois; - debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->image_rois_ = debug_image_rois; + debugger_->obstacle_points_ = debug_image_points; + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); + } } } -void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) +void PointPaintingFusionNode::postprocess( + const PointCloudMsgType & painted_pointcloud_msg, DetectedObjects & output_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + diagnostics_interface_ptr_->clear(); + + output_msg.header = painted_pointcloud_msg.header; + output_msg.objects.clear(); + const auto objects_sub_count = - obj_pub_ptr_->get_subscription_count() + obj_pub_ptr_->get_intra_process_subscription_count(); + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); if (objects_sub_count < 1) { return; } @@ -393,10 +409,21 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte } std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(painted_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_num_pillars_within_range = true; + bool is_success = detector_ptr_->detect( + painted_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } + diagnostics_interface_ptr_->add_key_value( + "is_num_pillars_within_range", is_num_pillars_within_range); + if (!is_num_pillars_within_range) { + std::stringstream message; + message << "PointPaintingTRT::detect: The actual number of pillars exceeds its maximum value, " + << "which may limit the detection performance."; + diagnostics_interface_ptr_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); @@ -406,21 +433,18 @@ void PointPaintingFusionNode::postprocess(sensor_msgs::msg::PointCloud2 & painte raw_objects.emplace_back(obj); } - autoware_perception_msgs::msg::DetectedObjects output_msg; - output_msg.header = painted_pointcloud_msg.header; + // prepare output message output_msg.objects = iou_bev_nms_.apply(raw_objects); detection_class_remapper_.mapClasses(output_msg); - if (objects_sub_count > 0) { - obj_pub_ptr_->publish(output_msg); + // publish debug message: painted pointcloud + if (debugger_ && painted_point_pub_ptr_->get_subscription_count() > 0) { + painted_point_pub_ptr_->publish(painted_pointcloud_msg); } + diagnostics_interface_ptr_->publish(painted_pointcloud_msg.header.stamp); } -bool PointPaintingFusionNode::out_of_scope(__attribute__((unused)) const DetectedObjects & obj) -{ - return false; -} } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index cc3eb17cc32ab..1fe860e68a967 100644 --- a/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/autoware_image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -27,8 +27,8 @@ namespace autoware::image_projection_based_fusion { PointPaintingTRT::PointPaintingTRT( - const autoware::lidar_centerpoint::NetworkParam & encoder_param, - const autoware::lidar_centerpoint::NetworkParam & head_param, + const autoware::tensorrt_common::TrtCommonConfig & encoder_param, + const autoware::tensorrt_common::TrtCommonConfig & head_param, const autoware::lidar_centerpoint::DensificationParam & densification_param, const autoware::lidar_centerpoint::CenterPointConfig & config) : autoware::lidar_centerpoint::CenterPointTRT( diff --git a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9a94613a2b788..0a9d9e3391882 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -16,12 +16,14 @@ #include #include +#include #include #include #include #include +#include #include #include #include @@ -36,10 +38,10 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_cluster_fusion", options) +: FusionNode("roi_cluster_fusion", options) { trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); @@ -51,10 +53,16 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) remove_unknown_ = declare_parameter("remove_unknown"); fusion_distance_ = declare_parameter("fusion_distance"); trust_object_distance_ = declare_parameter("trust_object_distance"); + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) +void RoiClusterFusionNode::preprocess(ClusterMsgType & output_cluster_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + // reset cluster semantic type if (!use_cluster_semantic_type_) { for (auto & feature_object : output_cluster_msg.feature_objects) { @@ -65,34 +73,14 @@ void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluste } } -void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_cluster_msg) -{ - if (!remove_unknown_) { - return; - } - DetectedObjectsWithFeature known_objects; - known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); - for (auto & feature_object : output_cluster_msg.feature_objects) { - bool is_roi_label_known = feature_object.object.classification.front().label != - autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; - if ( - is_roi_label_known || - feature_object.object.existence_probability >= min_roi_existence_prob_) { - known_objects.feature_objects.push_back(feature_object); - } - } - output_cluster_msg.feature_objects = known_objects.feature_objects; -} - void RoiClusterFusionNode::fuseOnSingleImage( - const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) + const ClusterMsgType & input_cluster_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, ClusterMsgType & output_cluster_msg) { - if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); // get transform from cluster frame id to camera optical frame id geometry_msgs::msg::TransformStamped transform_stamped; @@ -110,6 +98,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( } std::map m_cluster_roi; + for (std::size_t i = 0; i < input_cluster_msg.feature_objects.size(); ++i) { if (input_cluster_msg.feature_objects.at(i).feature.cluster.data.empty()) { continue; @@ -139,18 +128,17 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(*iter_x, *iter_y, *iter_z), - point_project_to_unrectified_image_); - if ( - 0 <= static_cast(projected_point.x()) && - static_cast(projected_point.x()) <= static_cast(camera_info.width) - 1 && - 0 <= static_cast(projected_point.y()) && - static_cast(projected_point.y()) <= static_cast(camera_info.height) - 1) { - min_x = std::min(static_cast(projected_point.x()), min_x); - min_y = std::min(static_cast(projected_point.y()), min_y); - max_x = std::max(static_cast(projected_point.x()), max_x); - max_y = std::max(static_cast(projected_point.y()), max_y); + Eigen::Vector2d projected_point; + if (det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(*iter_x, *iter_y, *iter_z), projected_point)) { + const int px = static_cast(projected_point.x()); + const int py = static_cast(projected_point.y()); + + min_x = std::min(px, min_x); + min_y = std::min(py, min_y); + max_x = std::max(px, max_x); + max_y = std::max(py, max_y); + projected_points.push_back(projected_point); if (debugger_) debugger_->obstacle_points_.push_back(projected_point); } @@ -160,7 +148,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( } sensor_msgs::msg::RegionOfInterest roi; - // roi.do_rectify = m_camera_info_.at(id).do_rectify; roi.x_offset = min_x; roi.y_offset = min_y; roi.width = max_x - min_x; @@ -223,7 +210,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( // note: debug objects are safely cleared in fusion_node.cpp if (debugger_) { - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } @@ -283,6 +270,28 @@ double RoiClusterFusionNode::cal_iou_by_mode( } } +void RoiClusterFusionNode::postprocess( + const ClusterMsgType & processing_msg, ClusterMsgType & output_msg) +{ + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg = processing_msg; + + if (remove_unknown_) { + // filter by object classification and existence probability + output_msg.feature_objects.clear(); + for (const auto & feature_object : processing_msg.feature_objects) { + if ( + feature_object.object.classification.front().label != + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN || + feature_object.object.existence_probability >= min_roi_existence_prob_) { + output_msg.feature_objects.push_back(feature_object); + } + } + } +} + } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp index 3ef3af8168f53..8b6a4fd2b7d10 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_detected_object_fusion/node.cpp @@ -18,18 +18,20 @@ #include #include +#include #include #include +#include #include #include namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_detected_object_fusion", options) +: FusionNode("roi_detected_object_fusion", options) { fusion_params_.passthrough_lower_bound_probability_thresholds = declare_parameter>("passthrough_lower_bound_probability_thresholds"); @@ -45,10 +47,16 @@ RoiDetectedObjectFusionNode::RoiDetectedObjectFusionNode(const rclcpp::NodeOptio can_assign_vector.data(), label_num, label_num); fusion_params_.can_assign_matrix = can_assign_matrix_tmp.transpose(); } + + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::vector passthrough_object_flags, fused_object_flags, ignored_object_flags; passthrough_object_flags.resize(output_msg.objects.size()); fused_object_flags.resize(output_msg.objects.size()); @@ -76,12 +84,11 @@ void RoiDetectedObjectFusionNode::preprocess(DetectedObjects & output_msg) } void RoiDetectedObjectFusionNode::fuseOnSingleImage( - const DetectedObjects & input_object_msg, const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, - DetectedObjects & output_object_msg __attribute__((unused))) + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, DetectedObjects & output_object_msg __attribute__((unused))) { - if (!checkCameraInfo(camera_info)) return; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); Eigen::Affine3d object2camera_affine; { @@ -94,12 +101,8 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( object2camera_affine = transformToEigen(transform_stamped_optional.value().transform); } - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); - - const auto object_roi_map = generateDetectedObjectRoIs( - input_object_msg, static_cast(camera_info.width), - static_cast(camera_info.height), object2camera_affine, pinhole_camera_model); + const auto object_roi_map = + generateDetectedObjectRoIs(input_object_msg, det2d, object2camera_affine); fuseObjectsOnImage(input_object_msg, input_roi_msg.feature_objects, object_roi_map); if (debugger_) { @@ -107,16 +110,18 @@ void RoiDetectedObjectFusionNode::fuseOnSingleImage( for (std::size_t roi_i = 0; roi_i < input_roi_msg.feature_objects.size(); ++roi_i) { debugger_->image_rois_.push_back(input_roi_msg.feature_objects.at(roi_i).feature.roi); } - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } std::map RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( - const DetectedObjects & input_object_msg, const double image_width, const double image_height, - const Eigen::Affine3d & object2camera_affine, - const image_geometry::PinholeCameraModel & pinhole_camera_model) + const DetectedObjects & input_object_msg, const Det2dStatus & det2d, + const Eigen::Affine3d & object2camera_affine) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + std::map object_roi_map; int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + input_object_msg.header.stamp.nanosec; @@ -127,6 +132,10 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( return object_roi_map; } const auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); + const double image_width = static_cast(camera_info.width); + const double image_height = static_cast(camera_info.height); + for (std::size_t obj_i = 0; obj_i < input_object_msg.objects.size(); ++obj_i) { std::vector vertices_camera_coord; const auto & object = input_object_msg.objects.at(obj_i); @@ -153,18 +162,17 @@ RoiDetectedObjectFusionNode::generateDetectedObjectRoIs( continue; } - Eigen::Vector2d proj_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(point.x(), point.y(), point.z()), - point_project_to_unrectified_image_); + Eigen::Vector2d proj_point; + if (det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(point.x(), point.y(), point.z()), proj_point)) { + const double px = proj_point.x(); + const double py = proj_point.y(); - min_x = std::min(proj_point.x(), min_x); - min_y = std::min(proj_point.y(), min_y); - max_x = std::max(proj_point.x(), max_x); - max_y = std::max(proj_point.y(), max_y); + min_x = std::min(px, min_x); + min_y = std::min(py, min_y); + max_x = std::max(px, max_x); + max_y = std::max(py, max_y); - if ( - proj_point.x() >= 0 && proj_point.x() < image_width && proj_point.y() >= 0 && - proj_point.y() < image_height) { point_on_image_cnt++; if (debugger_) { @@ -202,6 +210,9 @@ void RoiDetectedObjectFusionNode::fuseObjectsOnImage( const std::vector & image_rois, const std::map & object_roi_map) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + int64_t timestamp_nsec = input_object_msg.header.stamp.sec * static_cast(1e9) + input_object_msg.header.stamp.nanosec; if (fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { @@ -279,14 +290,15 @@ bool RoiDetectedObjectFusionNode::out_of_scope(const DetectedObject & obj) return is_out; } -void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) +void RoiDetectedObjectFusionNode::postprocess( + const DetectedObjects & processing_msg, DetectedObjects & output_msg) { - if (pub_ptr_->get_subscription_count() < 1) { - return; - } + output_msg.header = processing_msg.header; + output_msg.objects.clear(); - int64_t timestamp_nsec = - output_msg.header.stamp.sec * static_cast(1e9) + output_msg.header.stamp.nanosec; + // filter out ignored objects + int64_t timestamp_nsec = processing_msg.header.stamp.sec * static_cast(1e9) + + processing_msg.header.stamp.nanosec; if ( passthrough_object_flags_map_.size() == 0 || fused_object_flags_map_.size() == 0 || ignored_object_flags_map_.size() == 0) { @@ -298,33 +310,37 @@ void RoiDetectedObjectFusionNode::publish(const DetectedObjects & output_msg) ignored_object_flags_map_.count(timestamp_nsec) == 0) { return; } + auto & passthrough_object_flags = passthrough_object_flags_map_.at(timestamp_nsec); auto & fused_object_flags = fused_object_flags_map_.at(timestamp_nsec); - auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); - - DetectedObjects output_objects_msg, debug_fused_objects_msg, debug_ignored_objects_msg; - output_objects_msg.header = output_msg.header; - debug_fused_objects_msg.header = output_msg.header; - debug_ignored_objects_msg.header = output_msg.header; - for (std::size_t obj_i = 0; obj_i < output_msg.objects.size(); ++obj_i) { - const auto & obj = output_msg.objects.at(obj_i); - if (passthrough_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); + if (passthrough_object_flags.at(obj_i) || fused_object_flags.at(obj_i)) { + output_msg.objects.emplace_back(obj); } + } + + // debug messages + auto & ignored_object_flags = ignored_object_flags_map_.at(timestamp_nsec); + DetectedObjects debug_fused_objects_msg, debug_ignored_objects_msg; + debug_fused_objects_msg.header = processing_msg.header; + debug_ignored_objects_msg.header = processing_msg.header; + for (std::size_t obj_i = 0; obj_i < processing_msg.objects.size(); ++obj_i) { + const auto & obj = processing_msg.objects.at(obj_i); if (fused_object_flags.at(obj_i)) { - output_objects_msg.objects.emplace_back(obj); debug_fused_objects_msg.objects.emplace_back(obj); } if (ignored_object_flags.at(obj_i)) { debug_ignored_objects_msg.objects.emplace_back(obj); } } + if (debug_internal_pub_) { + debug_internal_pub_->publish("debug/fused_objects", debug_fused_objects_msg); + debug_internal_pub_->publish( + "debug/ignored_objects", debug_ignored_objects_msg); + } - pub_ptr_->publish(output_objects_msg); - - debug_publisher_->publish("debug/fused_objects", debug_fused_objects_msg); - debug_publisher_->publish("debug/ignored_objects", debug_ignored_objects_msg); - + // clear flags passthrough_object_flags_map_.erase(timestamp_nsec); fused_object_flags_map_.erase(timestamp_nsec); ignored_object_flags_map_.erase(timestamp_nsec); diff --git a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp index f9eb4ef909282..ed2a0cbc5d68f 100644 --- a/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/roi_pointcloud_fusion/node.cpp @@ -17,6 +17,9 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include + +#include #include #ifdef ROS_DISTRO_GALACTIC @@ -31,60 +34,32 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; + RoiPointCloudFusionNode::RoiPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode( - "roi_pointcloud_fusion", options) +: FusionNode("roi_pointcloud_fusion", options) { fuse_unknown_only_ = declare_parameter("fuse_unknown_only"); min_cluster_size_ = declare_parameter("min_cluster_size"); max_cluster_size_ = declare_parameter("max_cluster_size"); cluster_2d_tolerance_ = declare_parameter("cluster_2d_tolerance"); - pub_objects_ptr_ = - this->create_publisher("output_clusters", rclcpp::QoS{1}); - cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); -} -void RoiPointCloudFusionNode::preprocess( - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) -{ - return; + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); + cluster_debug_pub_ = this->create_publisher("debug/clusters", 1); } -void RoiPointCloudFusionNode::postprocess( - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & pointcloud_msg) -{ - const auto objects_sub_count = pub_objects_ptr_->get_subscription_count() + - pub_objects_ptr_->get_intra_process_subscription_count(); - if (objects_sub_count < 1) { - return; - } - - DetectedObjectsWithFeature output_msg; - output_msg.header = pointcloud_msg.header; - output_msg.feature_objects = output_fused_objects_; - - if (objects_sub_count > 0) { - pub_objects_ptr_->publish(output_msg); - } - output_fused_objects_.clear(); - // publish debug cluster - if (cluster_debug_pub_->get_subscription_count() > 0) { - sensor_msgs::msg::PointCloud2 debug_cluster_msg; - autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); - cluster_debug_pub_->publish(debug_cluster_msg); - } -} void RoiPointCloudFusionNode::fuseOnSingleImage( - const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, - __attribute__((unused)) const std::size_t image_id, - const DetectedObjectsWithFeature & input_roi_msg, - const sensor_msgs::msg::CameraInfo & camera_info, - __attribute__((unused)) sensor_msgs::msg::PointCloud2 & output_pointcloud_msg) + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + const RoiMsgType & input_roi_msg, + __attribute__((unused)) PointCloudMsgType & output_pointcloud_msg) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (input_pointcloud_msg.data.empty()) { return; } - if (!checkCameraInfo(camera_info)) return; std::vector output_objs; std::vector debug_image_rois; @@ -104,14 +79,18 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( debug_image_rois.push_back(feature_obj.feature.roi); } } + + // check if there is no object to fuse if (output_objs.empty()) { + // publish debug image, which is empty + if (debugger_) { + debugger_->image_rois_ = debug_image_rois; + debugger_->obstacle_points_ = debug_image_points; + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); + } return; } - // transform pointcloud to camera optical frame id - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); - geometry_msgs::msg::TransformStamped transform_stamped; { const auto transform_stamped_optional = getTransformStamped( @@ -122,15 +101,18 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( } transform_stamped = transform_stamped_optional.value(); } - 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; - - sensor_msgs::msg::PointCloud2 transformed_cloud; + const int point_step = input_pointcloud_msg.point_step; + const int x_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset; + const int y_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; + const int z_offset = + input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; + + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); - std::vector clusters; + std::vector clusters; std::vector clusters_data_size; clusters.resize(output_objs.size()); for (auto & cluster : clusters) { @@ -150,33 +132,36 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( if (transformed_z <= 0.0) { continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z), - point_project_to_unrectified_image_); - for (std::size_t i = 0; i < output_objs.size(); ++i) { - auto & feature_obj = output_objs.at(i); - const auto & check_roi = feature_obj.feature.roi; - auto & cluster = clusters.at(i); - - if ( - clusters_data_size.at(i) >= - static_cast(max_cluster_size_) * static_cast(point_step)) { - continue; - } - if ( - check_roi.x_offset <= projected_point.x() && check_roi.y_offset <= projected_point.y() && - check_roi.x_offset + check_roi.width >= projected_point.x() && - check_roi.y_offset + check_roi.height >= projected_point.y()) { - std::memcpy( - &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], point_step); - clusters_data_size.at(i) += point_step; + + Eigen::Vector2d projected_point; + if (det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { + for (std::size_t i = 0; i < output_objs.size(); ++i) { + auto & feature_obj = output_objs.at(i); + const auto & check_roi = feature_obj.feature.roi; + auto & cluster = clusters.at(i); + + const double px = projected_point.x(); + const double py = projected_point.y(); + + if ( + clusters_data_size.at(i) >= + static_cast(max_cluster_size_) * static_cast(point_step)) { + continue; + } + if ( + check_roi.x_offset <= px && check_roi.y_offset <= py && + check_roi.x_offset + check_roi.width >= px && + check_roi.y_offset + check_roi.height >= py) { + std::memcpy( + &cluster.data[clusters_data_size.at(i)], &input_pointcloud_msg.data[offset], + point_step); + clusters_data_size.at(i) += point_step; + } } - } - if (debugger_) { - // add all points inside image to debug - if ( - projected_point.x() > 0 && projected_point.x() < camera_info.width && - projected_point.y() > 0 && projected_point.y() < camera_info.height) { + + if (debugger_) { + // add all points inside image to debug debug_image_points.push_back(projected_point); } } @@ -186,17 +171,42 @@ void RoiPointCloudFusionNode::fuseOnSingleImage( updateOutputFusedObjects( output_objs, clusters, clusters_data_size, input_pointcloud_msg, input_roi_msg.header, tf_buffer_, min_cluster_size_, max_cluster_size_, cluster_2d_tolerance_, output_fused_objects_); + + // publish debug image if (debugger_) { debugger_->image_rois_ = debug_image_rois; debugger_->obstacle_points_ = debug_image_points; - debugger_->publishImage(image_id, input_roi_msg.header.stamp); + debugger_->publishImage(det2d.id, input_roi_msg.header.stamp); } } -bool RoiPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const DetectedObjectWithFeature & obj) +void RoiPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, ClusterMsgType & output_msg) { - return false; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.feature_objects = output_fused_objects_; + + output_fused_objects_.clear(); + + // publish debug cluster + if (cluster_debug_pub_->get_subscription_count() > 0) { + PointCloudMsgType debug_cluster_msg; + autoware::euclidean_cluster::convertObjectMsg2SensorMsg(output_msg, debug_cluster_msg); + cluster_debug_pub_->publish(debug_cluster_msg); + } +} + +void RoiPointCloudFusionNode::publish(const ClusterMsgType & output_msg) +{ + const auto objects_sub_count = + pub_ptr_->get_subscription_count() + pub_ptr_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + pub_ptr_->publish(output_msg); } } // namespace autoware::image_projection_based_fusion diff --git a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp index e678a956bc64e..7d63cac7273c6 100644 --- a/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp +++ b/perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion/node.cpp @@ -17,6 +17,7 @@ #include "autoware/image_projection_based_fusion/utils/geometry.hpp" #include "autoware/image_projection_based_fusion/utils/utils.hpp" +#include #include #include @@ -32,8 +33,10 @@ namespace autoware::image_projection_based_fusion { +using autoware::universe_utils::ScopedTimeTrack; + SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptions & options) -: FusionNode("segmentation_pointcloud_fusion", options) +: FusionNode("segmentation_pointcloud_fusion", options) { filter_distance_threshold_ = declare_parameter("filter_distance_threshold"); for (auto & item : filter_semantic_label_target_list_) { @@ -45,50 +48,36 @@ SegmentPointCloudFusionNode::SegmentPointCloudFusionNode(const rclcpp::NodeOptio } is_publish_debug_mask_ = declare_parameter("is_publish_debug_mask"); pub_debug_mask_ptr_ = image_transport::create_publisher(this, "~/debug/mask"); -} -void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) PointCloud2 & pointcloud_msg) -{ - return; + // publisher + pub_ptr_ = this->create_publisher("output", rclcpp::QoS{1}); } -void SegmentPointCloudFusionNode::postprocess(PointCloud2 & pointcloud_msg) +void SegmentPointCloudFusionNode::preprocess(__attribute__((unused)) + PointCloudMsgType & pointcloud_msg) { - auto original_cloud = std::make_shared(pointcloud_msg); - - int point_step = original_cloud->point_step; - size_t output_pointcloud_size = 0; - pointcloud_msg.data.clear(); - pointcloud_msg.data.resize(original_cloud->data.size()); - - for (size_t global_offset = 0; global_offset < original_cloud->data.size(); - global_offset += point_step) { - if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { - copyPointCloud( - *original_cloud, point_step, global_offset, pointcloud_msg, output_pointcloud_size); - } - } + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); - pointcloud_msg.data.resize(output_pointcloud_size); - pointcloud_msg.row_step = output_pointcloud_size / pointcloud_msg.height; - pointcloud_msg.width = output_pointcloud_size / pointcloud_msg.point_step / pointcloud_msg.height; - - filter_global_offset_set_.clear(); return; } void SegmentPointCloudFusionNode::fuseOnSingleImage( - const PointCloud2 & input_pointcloud_msg, __attribute__((unused)) const std::size_t image_id, - [[maybe_unused]] const Image & input_mask, __attribute__((unused)) const CameraInfo & camera_info, - __attribute__((unused)) PointCloud2 & output_cloud) + const PointCloudMsgType & input_pointcloud_msg, const Det2dStatus & det2d, + [[maybe_unused]] const Image & input_mask, + __attribute__((unused)) PointCloudMsgType & output_cloud) { + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + if (input_pointcloud_msg.data.empty()) { return; } - if (!checkCameraInfo(camera_info)) return; if (input_mask.height == 0 || input_mask.width == 0) { return; } + + const sensor_msgs::msg::CameraInfo & camera_info = det2d.camera_projector_ptr->getCameraInfo(); std::vector mask_data(input_mask.data.begin(), input_mask.data.end()); cv::Mat mask = perception_utils::runLengthDecoder(mask_data, input_mask.height, input_mask.width); @@ -103,8 +92,6 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( const int orig_height = camera_info.height; // resize mask to the same size as the camera image cv::resize(mask, mask, cv::Size(orig_width, orig_height), 0, 0, cv::INTER_NEAREST); - image_geometry::PinholeCameraModel pinhole_camera_model; - pinhole_camera_model.fromCameraInfo(camera_info); geometry_msgs::msg::TransformStamped transform_stamped; // transform pointcloud from frame id to camera optical frame id @@ -118,7 +105,7 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( transform_stamped = transform_stamped_optional.value(); } - PointCloud2 transformed_cloud; + PointCloudMsgType transformed_cloud; tf2::doTransform(input_pointcloud_msg, transformed_cloud, transform_stamped); int point_step = input_pointcloud_msg.point_step; @@ -139,13 +126,9 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( continue; } - Eigen::Vector2d projected_point = calcRawImageProjectedPoint( - pinhole_camera_model, cv::Point3d(transformed_x, transformed_y, transformed_z), - point_project_to_unrectified_image_); - - bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width && - projected_point.y() > 0 && projected_point.y() < camera_info.height; - if (!is_inside_image) { + Eigen::Vector2d projected_point; + if (!det2d.camera_projector_ptr->calcImageProjectedPoint( + cv::Point3d(transformed_x, transformed_y, transformed_z), projected_point)) { continue; } @@ -162,11 +145,33 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( } } -bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) - const PointCloud2 & filtered_cloud) +void SegmentPointCloudFusionNode::postprocess( + const PointCloudMsgType & pointcloud_msg, PointCloudMsgType & output_msg) { - return false; + std::unique_ptr st_ptr; + if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); + + output_msg.header = pointcloud_msg.header; + output_msg.data.clear(); + output_msg.data.resize(pointcloud_msg.data.size()); + const int point_step = pointcloud_msg.point_step; + + size_t output_pointcloud_size = 0; + for (size_t global_offset = 0; global_offset < pointcloud_msg.data.size(); + global_offset += point_step) { + if (filter_global_offset_set_.find(global_offset) == filter_global_offset_set_.end()) { + copyPointCloud(pointcloud_msg, point_step, global_offset, output_msg, output_pointcloud_size); + } + } + + output_msg.data.resize(output_pointcloud_size); + output_msg.row_step = output_pointcloud_size / output_msg.height; + output_msg.width = output_pointcloud_size / output_msg.point_step / output_msg.height; + + filter_global_offset_set_.clear(); + return; } + } // namespace autoware::image_projection_based_fusion #include diff --git a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp index 338a5605e5a79..81424d4c23a34 100644 --- a/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp +++ b/perception/autoware_image_projection_based_fusion/src/utils/utils.cpp @@ -44,20 +44,6 @@ bool checkCameraInfo(const sensor_msgs::msg::CameraInfo & camera_info) return true; } -Eigen::Vector2d calcRawImageProjectedPoint( - const image_geometry::PinholeCameraModel & pinhole_camera_model, const cv::Point3d & point3d, - const bool & unrectify) -{ - const cv::Point2d rectified_image_point = pinhole_camera_model.project3dToPixel(point3d); - - if (!unrectify) { - return Eigen::Vector2d(rectified_image_point.x, rectified_image_point.y); - } - const cv::Point2d raw_image_point = pinhole_camera_model.unrectifyPoint(rectified_image_point); - - return Eigen::Vector2d(raw_image_point.x, raw_image_point.y); -} - std::optional getTransformStamped( const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, const std::string & source_frame_id, const rclcpp::Time & time) @@ -81,8 +67,8 @@ Eigen::Affine3d transformToEigen(const geometry_msgs::msg::Transform & t) } void closest_cluster( - const PointCloud2 & cluster, const double cluster_2d_tolerance, const int min_cluster_size, - const pcl::PointXYZ & center, PointCloud2 & out_cluster) + const PointCloudMsgType & cluster, const double cluster_2d_tolerance, const int min_cluster_size, + const pcl::PointXYZ & center, PointCloudMsgType & out_cluster) { // sort point by distance to camera origin @@ -137,8 +123,8 @@ void closest_cluster( } void updateOutputFusedObjects( - std::vector & output_objs, std::vector & clusters, - const std::vector & clusters_data_size, const PointCloud2 & in_cloud, + std::vector & output_objs, std::vector & clusters, + const std::vector & clusters_data_size, const PointCloudMsgType & in_cloud, const std_msgs::msg::Header & in_roi_header, const tf2_ros::Buffer & tf_buffer, const int min_cluster_size, const int max_cluster_size, const float cluster_2d_tolerance, std::vector & output_fused_objects) @@ -176,7 +162,7 @@ void updateOutputFusedObjects( // TODO(badai-nguyen): change to interface to select refine criteria like closest, largest // to output refine cluster and centroid - sensor_msgs::msg::PointCloud2 refine_cluster; + PointCloudMsgType refine_cluster; closest_cluster( cluster, cluster_2d_tolerance, min_cluster_size, camera_orig_point_frame, refine_cluster); if ( @@ -198,7 +184,7 @@ void updateOutputFusedObjects( } } -geometry_msgs::msg::Point getCentroid(const sensor_msgs::msg::PointCloud2 & pointcloud) +geometry_msgs::msg::Point getCentroid(const PointCloudMsgType & pointcloud) { geometry_msgs::msg::Point centroid; centroid.x = 0.0f; diff --git a/perception/autoware_lidar_apollo_instance_segmentation/CHANGELOG.rst b/perception/autoware_lidar_apollo_instance_segmentation/CHANGELOG.rst index 5841ae3059503..dd8864ef5a0ef 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/CHANGELOG.rst +++ b/perception/autoware_lidar_apollo_instance_segmentation/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package autoware_lidar_apollo_instance_segmentation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_lidar_apollo_instance_segmentation): tier4_debug_msgs to autoware_internal_debug_msgs in files perce… (`#9876 `_) + feat: tier4_debug_msgs to autoware_internal_debug_msgs in files perception/autoware_lidar_apollo_instance_segmentation + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* Contributors: Amadeusz Szymko, Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt index 16ab7ecccae60..0555cedc235d9 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/autoware_lidar_apollo_instance_segmentation/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(tf2_eigen REQUIRED) find_package(autoware_universe_utils REQUIRED) -find_package(tier4_debug_msgs REQUIRED) +find_package(autoware_internal_debug_msgs REQUIRED) find_package(tier4_perception_msgs REQUIRED) find_package(autoware_tensorrt_common) @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} rclcpp_components::component tf2_eigen::tf2_eigen ${autoware_tensorrt_common_TARGETS} - ${tier4_debug_msgs_TARGETS} + ${autoware_internal_debug_msgs_TARGETS} ${tier4_perception_msgs_TARGETS} ) diff --git a/perception/autoware_lidar_apollo_instance_segmentation/package.xml b/perception/autoware_lidar_apollo_instance_segmentation/package.xml index 8a3d5a7ea6f76..7e09587aef8a1 100755 --- a/perception/autoware_lidar_apollo_instance_segmentation/package.xml +++ b/perception/autoware_lidar_apollo_instance_segmentation/package.xml @@ -2,7 +2,7 @@ autoware_lidar_apollo_instance_segmentation - 0.40.0 + 0.41.0 autoware_lidar_apollo_instance_segmentation Yoshi Ri Yukihiro Saito @@ -14,6 +14,7 @@ ament_cmake autoware_cuda_utils + autoware_internal_debug_msgs autoware_perception_msgs autoware_tensorrt_common autoware_universe_utils @@ -22,7 +23,6 @@ rclcpp rclcpp_components tf2_eigen - tier4_debug_msgs tier4_perception_msgs ament_lint_auto diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp index 60aa09c8e16d7..2e3a7a2243399 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/detector.cpp @@ -18,7 +18,6 @@ #include -#include #include #include @@ -51,12 +50,9 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * const auto precision = node_->declare_parameter("precision", "fp32"); trt_common_ = std::make_unique( - onnx_file, precision, nullptr, autoware::tensorrt_common::BatchConfig{1, 1, 1}, 1 << 30); - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - RCLCPP_ERROR_STREAM(node_->get_logger(), "failed to create tensorrt engine file."); - return; + tensorrt_common::TrtCommonConfig(onnx_file, precision)); + if (!trt_common_->setup()) { + throw std::runtime_error("Failed to setup TensorRT"); } if (node_->declare_parameter("build_only", false)) { @@ -65,11 +61,11 @@ LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); input_d_ = autoware::cuda_utils::make_unique(input_size); - const auto output_dims = trt_common_->getBindingDimensions(1); + const auto output_dims = trt_common_->getTensorShape(1); output_size_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); output_d_ = autoware::cuda_utils::make_unique(output_size_); @@ -127,10 +123,6 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( const sensor_msgs::msg::PointCloud2 & input, tier4_perception_msgs::msg::DetectedObjectsWithFeature & output) { - if (!trt_common_->isInitialized()) { - return false; - } - // move up pointcloud z_offset in z axis sensor_msgs::msg::PointCloud2 transformed_cloud; transformCloud(input, transformed_cloud, z_offset_); @@ -169,7 +161,10 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( std::vector buffers = {input_d_.get(), output_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( output_h_.get(), output_d_.get(), sizeof(float) * output_size_, cudaMemcpyDeviceToHost, diff --git a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp index 9a3e13b81120f..c535fc6762e1f 100644 --- a/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp +++ b/perception/autoware_lidar_apollo_instance_segmentation/src/node.cpp @@ -62,9 +62,9 @@ void LidarInstanceSegmentationNode::pointCloudCallback( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_lidar_centerpoint/CHANGELOG.rst b/perception/autoware_lidar_centerpoint/CHANGELOG.rst index 46f68e7462377..3978a514305a8 100644 --- a/perception/autoware_lidar_centerpoint/CHANGELOG.rst +++ b/perception/autoware_lidar_centerpoint/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package autoware_lidar_centerpoint ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* feat(lidar_centerpoint, pointpainting): add diag publisher for max voxel size (`#9720 `_) +* fix(autoware_lidar_centerpoint): fixed rounding errors that caused illegal memory access (`#9795 `_) + fix: fixed rounding errors that caused illegal memory address +* feat(autoware_lidar_centerpoint): process front voxels first (`#9608 `_) + * feat: optimize voxel indexing in preprocess_kernel.cu + * fix: remove redundant index check + * fix: modify voxel index for better memory access + --------- +* Contributors: Amadeusz Szymko, Fumiya Watanabe, Kenzo Lobos Tsunekawa, Taekjin LEE, kminoda + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_lidar_centerpoint/CMakeLists.txt b/perception/autoware_lidar_centerpoint/CMakeLists.txt index 88244b3153349..322907c67c6ae 100644 --- a/perception/autoware_lidar_centerpoint/CMakeLists.txt +++ b/perception/autoware_lidar_centerpoint/CMakeLists.txt @@ -84,8 +84,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) lib/detection_class_remapper.cpp lib/utils.cpp lib/ros_utils.cpp - lib/network/network_trt.cpp - lib/network/tensorrt_wrapper.cpp lib/postprocess/non_maximum_suppression.cpp lib/preprocess/pointcloud_densification.cpp lib/preprocess/voxel_generator.cpp diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp index ce5ec3ea0abfe..4b531e34877d6 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/centerpoint_trt.hpp @@ -16,12 +16,13 @@ #define AUTOWARE__LIDAR_CENTERPOINT__CENTERPOINT_TRT_HPP_ #include "autoware/lidar_centerpoint/cuda_utils.hpp" -#include "autoware/lidar_centerpoint/network/network_trt.hpp" #include "autoware/lidar_centerpoint/postprocess/postprocess_kernel.hpp" #include "autoware/lidar_centerpoint/preprocess/voxel_generator.hpp" #include "pcl/point_cloud.h" #include "pcl/point_types.h" +#include + #include "sensor_msgs/msg/point_cloud2.hpp" #include @@ -31,41 +32,25 @@ namespace autoware::lidar_centerpoint { -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_; -}; +using autoware::tensorrt_common::ProfileDims; +using autoware::tensorrt_common::TrtCommonConfig; class CenterPointTRT { public: explicit CenterPointTRT( - const NetworkParam & encoder_param, const NetworkParam & head_param, + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param, const DensificationParam & densification_param, const CenterPointConfig & config); virtual ~CenterPointTRT(); bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d); + std::vector & det_boxes3d, bool & is_num_pillars_within_range); protected: void initPtr(); + void initTrt(const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param); virtual bool preprocess( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); @@ -75,8 +60,8 @@ class CenterPointTRT void postProcess(std::vector & det_boxes3d); std::unique_ptr vg_ptr_{nullptr}; - std::unique_ptr encoder_trt_ptr_{nullptr}; - std::unique_ptr head_trt_ptr_{nullptr}; + std::unique_ptr encoder_trt_ptr_{nullptr}; + std::unique_ptr head_trt_ptr_{nullptr}; std::unique_ptr post_proc_ptr_{nullptr}; cudaStream_t stream_{nullptr}; diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp deleted file mode 100644 index 17778d77ed798..0000000000000 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/network_trt.hpp +++ /dev/null @@ -1,53 +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. - -#ifndef AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ -#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ - -#include "autoware/lidar_centerpoint/centerpoint_config.hpp" -#include "autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp" - -#include - -namespace autoware::lidar_centerpoint -{ -class VoxelEncoderTRT : public TensorRTWrapper -{ -public: - using TensorRTWrapper::TensorRTWrapper; - -protected: - bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) override; -}; - -class HeadTRT : public TensorRTWrapper -{ -public: - using TensorRTWrapper::TensorRTWrapper; - - HeadTRT(const std::vector & out_channel_sizes, const CenterPointConfig & config); - -protected: - bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) override; - - std::vector out_channel_sizes_; -}; - -} // namespace autoware::lidar_centerpoint - -#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp deleted file mode 100644 index d56ccc699add3..0000000000000 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp +++ /dev/null @@ -1,67 +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. - -#ifndef AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ -#define AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ - -#include "NvInfer.h" -#include "autoware/lidar_centerpoint/centerpoint_config.hpp" -#include "autoware/tensorrt_common/tensorrt_common.hpp" - -#include -#include -#include - -namespace autoware::lidar_centerpoint -{ - -class TensorRTWrapper -{ -public: - explicit TensorRTWrapper(const CenterPointConfig & config); - - virtual ~TensorRTWrapper(); - - bool init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision); - - autoware::tensorrt_common::TrtUniquePtr context_{nullptr}; - -protected: - virtual bool setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) = 0; - - CenterPointConfig config_; - autoware::tensorrt_common::Logger logger_; - -private: - bool parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - std::size_t workspace_size = (1ULL << 30)); - - bool saveEngine(const std::string & engine_path); - - bool loadEngine(const std::string & engine_path); - - bool createContext(); - - autoware::tensorrt_common::TrtUniquePtr runtime_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr plan_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr engine_{nullptr}; -}; - -} // namespace autoware::lidar_centerpoint - -#endif // AUTOWARE__LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ diff --git a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp index a49451fde9d0d..1748db0e48392 100644 --- a/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp +++ b/perception/autoware_lidar_centerpoint/include/autoware/lidar_centerpoint/node.hpp @@ -20,6 +20,7 @@ #include "autoware/lidar_centerpoint/postprocess/non_maximum_suppression.hpp" #include +#include #include #include #include @@ -59,6 +60,7 @@ class LidarCenterPointNode : public rclcpp::Node DetectionClassRemapper detection_class_remapper_; std::unique_ptr detector_ptr_{nullptr}; + std::unique_ptr diagnostics_interface_ptr_; // debugger std::unique_ptr> stop_watch_ptr_{ diff --git a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp index 6ee2b3733bdb0..c586eeb961716 100644 --- a/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/autoware_lidar_centerpoint/lib/centerpoint_trt.cpp @@ -19,6 +19,7 @@ #include "autoware/lidar_centerpoint/preprocess/preprocess_kernel.hpp" #include +#include #include #include @@ -27,39 +28,22 @@ #include #include #include +#include +#include #include namespace autoware::lidar_centerpoint { CenterPointTRT::CenterPointTRT( - const NetworkParam & encoder_param, const NetworkParam & head_param, + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param, const DensificationParam & densification_param, const CenterPointConfig & config) : config_(config) { vg_ptr_ = std::make_unique(densification_param, config_); post_proc_ptr_ = std::make_unique(config_); - // encoder - encoder_trt_ptr_ = std::make_unique(config_); - encoder_trt_ptr_->init( - encoder_param.onnx_path(), encoder_param.engine_path(), encoder_param.trt_precision()); - encoder_trt_ptr_->context_->setBindingDimensions( - 0, - nvinfer1::Dims3( - config_.max_voxel_size_, config_.max_point_in_voxel_size_, config_.encoder_in_feature_size_)); - - // head - std::vector out_channel_sizes = { - config_.class_size_, config_.head_out_offset_size_, config_.head_out_z_size_, - config_.head_out_dim_size_, config_.head_out_rot_size_, config_.head_out_vel_size_}; - head_trt_ptr_ = std::make_unique(out_channel_sizes, config_); - head_trt_ptr_->init(head_param.onnx_path(), head_param.engine_path(), head_param.trt_precision()); - head_trt_ptr_->context_->setBindingDimensions( - 0, nvinfer1::Dims4( - config_.batch_size_, config_.encoder_out_feature_size_, config_.grid_size_y_, - config_.grid_size_x_)); - initPtr(); + initTrt(encoder_param, head_param); cudaStreamCreate(&stream_); } @@ -125,10 +109,72 @@ void CenterPointTRT::initPtr() cudaMemcpyHostToDevice, stream_)); } +void CenterPointTRT::initTrt( + const TrtCommonConfig & encoder_param, const TrtCommonConfig & head_param) +{ + // encoder input profile + auto enc_in_dims = nvinfer1::Dims{ + 3, + {static_cast(config_.max_voxel_size_), + static_cast(config_.max_point_in_voxel_size_), + static_cast(config_.encoder_in_feature_size_)}}; + std::vector encoder_profile_dims{ + tensorrt_common::ProfileDims(0, enc_in_dims, enc_in_dims, enc_in_dims)}; + auto encoder_profile_dims_ptr = + std::make_unique>(encoder_profile_dims); + + // head input profile + auto head_in_dims = nvinfer1::Dims{ + 4, + {static_cast(config_.batch_size_), + static_cast(config_.encoder_out_feature_size_), + static_cast(config_.grid_size_y_), static_cast(config_.grid_size_x_)}}; + std::vector head_profile_dims{ + tensorrt_common::ProfileDims(0, head_in_dims, head_in_dims, head_in_dims)}; + std::unordered_map out_channel_map = { + {1, static_cast(config_.class_size_)}, + {2, static_cast(config_.head_out_offset_size_)}, + {3, static_cast(config_.head_out_z_size_)}, + {4, static_cast(config_.head_out_dim_size_)}, + {5, static_cast(config_.head_out_rot_size_)}, + {6, static_cast(config_.head_out_vel_size_)}}; + for (const auto & [tensor_name, channel_size] : out_channel_map) { + auto dims = nvinfer1::Dims{ + 4, + {static_cast(config_.batch_size_), channel_size, + static_cast(config_.down_grid_size_y_), + static_cast(config_.down_grid_size_x_)}}; + head_profile_dims.emplace_back(tensor_name, dims, dims, dims); + } + auto head_profile_dims_ptr = + std::make_unique>(head_profile_dims); + + // initialize trt wrappers + encoder_trt_ptr_ = std::make_unique(encoder_param); + + head_trt_ptr_ = std::make_unique(head_param); + + // setup trt engines + if ( + !encoder_trt_ptr_->setup(std::move(encoder_profile_dims_ptr)) || + !head_trt_ptr_->setup(std::move(head_profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TRT engine."); + } + + // set input shapes + if ( + !encoder_trt_ptr_->setInputShape(0, enc_in_dims) || + !head_trt_ptr_->setInputShape(0, head_in_dims)) { + throw std::runtime_error("Failed to set input shape."); + } +} + bool CenterPointTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, - std::vector & det_boxes3d) + std::vector & det_boxes3d, bool & is_num_pillars_within_range) { + is_num_pillars_within_range = true; + CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); CHECK_CUDA_ERROR( @@ -155,6 +201,7 @@ bool CenterPointTRT::detect( "The actual number of pillars (%u) exceeds its maximum value (%zu). " "Please considering increasing it since it may limit the detection performance.", num_pillars, config_.max_voxel_size_); + is_num_pillars_within_range = false; } return true; @@ -206,13 +253,10 @@ bool CenterPointTRT::preprocess( void CenterPointTRT::inference() { - if (!encoder_trt_ptr_->context_ || !head_trt_ptr_->context_) { - throw std::runtime_error("Failed to create tensorrt context."); - } - // pillar encoder network - std::vector encoder_buffers{encoder_in_features_d_.get(), pillar_features_d_.get()}; - encoder_trt_ptr_->context_->enqueueV2(encoder_buffers.data(), stream_, nullptr); + std::vector encoder_tensors = {encoder_in_features_d_.get(), pillar_features_d_.get()}; + encoder_trt_ptr_->setTensorsAddresses(encoder_tensors); + encoder_trt_ptr_->enqueueV3(stream_); // scatter CHECK_CUDA_ERROR(scatterFeatures_launch( @@ -221,11 +265,13 @@ void CenterPointTRT::inference() spatial_features_d_.get(), stream_)); // head network - std::vector head_buffers = {spatial_features_d_.get(), head_out_heatmap_d_.get(), + std::vector head_tensors = {spatial_features_d_.get(), head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), head_out_rot_d_.get(), head_out_vel_d_.get()}; - head_trt_ptr_->context_->enqueueV2(head_buffers.data(), stream_, nullptr); + head_trt_ptr_->setTensorsAddresses(head_tensors); + + head_trt_ptr_->enqueueV3(stream_); } void CenterPointTRT::postProcess(std::vector & det_boxes3d) diff --git a/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp b/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp deleted file mode 100644 index 7767c0b5cbb02..0000000000000 --- a/perception/autoware_lidar_centerpoint/lib/network/network_trt.cpp +++ /dev/null @@ -1,85 +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 "autoware/lidar_centerpoint/network/network_trt.hpp" - -#include -#include - -namespace autoware::lidar_centerpoint -{ -bool VoxelEncoderTRT::setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) -{ - auto profile = builder.createOptimizationProfile(); - auto in_name = network.getInput(0)->getName(); - auto in_dims = nvinfer1::Dims3( - config_.max_voxel_size_, config_.max_point_in_voxel_size_, config_.encoder_in_feature_size_); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMIN, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kOPT, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMAX, in_dims); - - auto out_name = network.getOutput(0)->getName(); - auto out_dims = nvinfer1::Dims2(config_.max_voxel_size_, config_.encoder_out_feature_size_); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMIN, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kOPT, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMAX, out_dims); - config.addOptimizationProfile(profile); - - return true; -} - -HeadTRT::HeadTRT( - const std::vector & out_channel_sizes, const CenterPointConfig & config) -: TensorRTWrapper(config), out_channel_sizes_(out_channel_sizes) -{ -} - -bool HeadTRT::setProfile( - nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, - nvinfer1::IBuilderConfig & config) -{ - auto profile = builder.createOptimizationProfile(); - auto in_name = network.getInput(0)->getName(); - auto in_dims = nvinfer1::Dims4( - config_.batch_size_, config_.encoder_out_feature_size_, config_.grid_size_y_, - config_.grid_size_x_); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMIN, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kOPT, in_dims); - profile->setDimensions(in_name, nvinfer1::OptProfileSelector::kMAX, in_dims); - - for (std::size_t ci = 0; ci < out_channel_sizes_.size(); ci++) { - auto out_name = network.getOutput(ci)->getName(); - - if ( - out_name == std::string("heatmap") && - network.getOutput(ci)->getDimensions().d[1] != static_cast(out_channel_sizes_[ci])) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Expected and actual number of classes do not match" << std::endl; - return false; - } - auto out_dims = nvinfer1::Dims4( - config_.batch_size_, out_channel_sizes_[ci], config_.down_grid_size_y_, - config_.down_grid_size_x_); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMIN, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kOPT, out_dims); - profile->setDimensions(out_name, nvinfer1::OptProfileSelector::kMAX, out_dims); - } - config.addOptimizationProfile(profile); - - return true; -} - -} // namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp deleted file mode 100644 index 6d4c2e053b34f..0000000000000 --- a/perception/autoware_lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ /dev/null @@ -1,169 +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 "autoware/lidar_centerpoint/network/tensorrt_wrapper.hpp" - -#include "NvOnnxParser.h" - -#include -#include -#include - -namespace autoware::lidar_centerpoint -{ -TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config) : config_(config) -{ -} - -TensorRTWrapper::~TensorRTWrapper() -{ - context_.reset(); - runtime_.reset(); - plan_.reset(); - engine_.reset(); -} - -bool TensorRTWrapper::init( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision) -{ - runtime_ = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferRuntime(logger_)); - if (!runtime_) { - autoware::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 TensorRTWrapper::createContext() -{ - if (!engine_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create context: Engine was not created" << std::endl; - return false; - } - - context_ = autoware::tensorrt_common::TrtUniquePtr( - engine_->createExecutionContext()); - if (!context_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; - return false; - } - - return true; -} - -bool TensorRTWrapper::parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - const std::size_t workspace_size) -{ - auto builder = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferBuilder(logger_)); - if (!builder) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; - return false; - } - - auto config = autoware::tensorrt_common::TrtUniquePtr( - builder->createBuilderConfig()); - if (!config) { - autoware::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()) { - autoware::tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; - config->setFlag(nvinfer1::BuilderFlag::kFP16); - } else { - autoware::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 = autoware::tensorrt_common::TrtUniquePtr( - builder->createNetworkV2(flag)); - if (!network) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; - return false; - } - - auto parser = autoware::tensorrt_common::TrtUniquePtr( - nvonnxparser::createParser(*network, logger_)); - parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); - - if (!setProfile(*builder, *network, *config)) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; - return false; - } - - plan_ = autoware::tensorrt_common::TrtUniquePtr( - builder->buildSerializedNetwork(*network, *config)); - if (!plan_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create serialized network" << std::endl; - return false; - } - engine_ = autoware::tensorrt_common::TrtUniquePtr( - runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); - if (!engine_) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; - return false; - } - - return saveEngine(engine_path); -} - -bool TensorRTWrapper::saveEngine(const std::string & engine_path) -{ - autoware::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 true; -} - -bool TensorRTWrapper::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_ = - autoware::tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - autoware::tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; - return true; -} - -} // namespace autoware::lidar_centerpoint diff --git a/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 502ad04223ce9..95ac7b4353e90 100644 --- a/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -148,7 +148,9 @@ __global__ void generateVoxels_random_kernel( int voxel_idx = floorf((point.x - min_x_range) / pillar_x_size); int voxel_idy = floorf((point.y - min_y_range) / pillar_y_size); - unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx; + voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy; + unsigned int voxel_index = (grid_x_size - 1 - voxel_idx) * grid_y_size + voxel_idy; unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); @@ -185,12 +187,14 @@ __global__ void generateBaseFeatures_kernel( unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, int max_voxel_size, unsigned int * pillar_num, float * voxel_features, float * voxel_num, 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; + // exchange x and y to process in a row-major order + // flip x axis direction to process front to back + unsigned int voxel_idx_inverted = blockIdx.y * blockDim.y + threadIdx.y; + unsigned int voxel_idy = blockIdx.x * blockDim.x + threadIdx.x; + if (voxel_idx_inverted >= grid_x_size || voxel_idy >= grid_y_size) return; + unsigned int voxel_idx = grid_x_size - 1 - voxel_idx_inverted; + + unsigned int voxel_index = voxel_idx_inverted * grid_y_size + voxel_idy; unsigned int count = mask[voxel_index]; if (!(count > 0)) return; count = count < MAX_POINT_IN_VOXEL_SIZE ? count : MAX_POINT_IN_VOXEL_SIZE; @@ -220,9 +224,10 @@ cudaError_t generateBaseFeatures_launch( unsigned int * pillar_num, float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream) { + // exchange x and y to process in a row-major order dim3 threads = {32, 32}; dim3 blocks = { - (grid_x_size + threads.x - 1) / threads.x, (grid_y_size + threads.y - 1) / threads.y}; + (grid_y_size + threads.x - 1) / threads.x, (grid_x_size + threads.y - 1) / threads.y}; generateBaseFeatures_kernel<<>>( mask, voxels, grid_y_size, grid_x_size, max_voxel_size, pillar_num, voxel_features, voxel_num, diff --git a/perception/autoware_lidar_centerpoint/package.xml b/perception/autoware_lidar_centerpoint/package.xml index 78e900f8198f6..a4eb5c6758f61 100644 --- a/perception/autoware_lidar_centerpoint/package.xml +++ b/perception/autoware_lidar_centerpoint/package.xml @@ -2,7 +2,7 @@ autoware_lidar_centerpoint - 0.40.0 + 0.41.0 The autoware_lidar_centerpoint package Kenzo Lobos-Tsunekawa Koji Minoda diff --git a/perception/autoware_lidar_centerpoint/src/node.cpp b/perception/autoware_lidar_centerpoint/src/node.cpp index 59acceeac54d6..19d1475db9051 100644 --- a/perception/autoware_lidar_centerpoint/src/node.cpp +++ b/perception/autoware_lidar_centerpoint/src/node.cpp @@ -86,8 +86,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti iou_bev_nms_.setParameters(p); } - NetworkParam encoder_param(encoder_onnx_path, encoder_engine_path, trt_precision); - NetworkParam head_param(head_onnx_path, head_engine_path, trt_precision); + TrtCommonConfig encoder_param(encoder_onnx_path, trt_precision, encoder_engine_path); + TrtCommonConfig head_param(head_onnx_path, trt_precision, head_engine_path); DensificationParam densification_param( densification_world_frame_id, densification_num_past_frames); @@ -107,6 +107,8 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti circle_nms_dist_threshold, yaw_norm_thresholds, has_variance_); detector_ptr_ = std::make_unique(encoder_param, head_param, densification_param, config); + diagnostics_interface_ptr_ = + std::make_unique(this, "centerpoint_trt"); pointcloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), @@ -144,12 +146,24 @@ void LidarCenterPointNode::pointCloudCallback( if (stop_watch_ptr_) { stop_watch_ptr_->toc("processing_time", true); } + diagnostics_interface_ptr_->clear(); std::vector det_boxes3d; - bool is_success = detector_ptr_->detect(*input_pointcloud_msg, tf_buffer_, det_boxes3d); + bool is_num_pillars_within_range = true; + bool is_success = detector_ptr_->detect( + *input_pointcloud_msg, tf_buffer_, det_boxes3d, is_num_pillars_within_range); if (!is_success) { return; } + diagnostics_interface_ptr_->add_key_value( + "is_num_pillars_within_range", is_num_pillars_within_range); + if (!is_num_pillars_within_range) { + std::stringstream message; + message << "CenterPointTRT::detect: The actual number of pillars exceeds its maximum value, " + << "which may limit the detection performance."; + diagnostics_interface_ptr_->update_level_and_message( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } std::vector raw_objects; raw_objects.reserve(det_boxes3d.size()); @@ -169,6 +183,7 @@ void LidarCenterPointNode::pointCloudCallback( objects_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); } + diagnostics_interface_ptr_->publish(input_pointcloud_msg->header.stamp); // add processing time for debug if (debug_publisher_ptr_ && stop_watch_ptr_) { diff --git a/perception/autoware_lidar_transfusion/CHANGELOG.rst b/perception/autoware_lidar_transfusion/CHANGELOG.rst index f397c9e1ddc06..f1f2809947e39 100644 --- a/perception/autoware_lidar_transfusion/CHANGELOG.rst +++ b/perception/autoware_lidar_transfusion/CHANGELOG.rst @@ -2,6 +2,42 @@ Changelog for package autoware_lidar_transfusion ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* fix(autoware_lidar_transfusion): fixed rounding errors that caused illegal memory access (`#9796 `_) + fix: fixed rounding errors that caused illegal memory address + Co-authored-by: Amadeusz Szymko +* Contributors: Amadeusz Szymko, Fumiya Watanabe, Kenzo Lobos Tsunekawa + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_lidar_transfusion/CMakeLists.txt b/perception/autoware_lidar_transfusion/CMakeLists.txt index c3a56f883fbe5..fdb978c64946f 100644 --- a/perception/autoware_lidar_transfusion/CMakeLists.txt +++ b/perception/autoware_lidar_transfusion/CMakeLists.txt @@ -84,7 +84,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_add_library(${PROJECT_NAME}_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 diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp deleted file mode 100644 index 71cd08acdb8c8..0000000000000 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/network/network_trt.hpp +++ /dev/null @@ -1,87 +0,0 @@ -// 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__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ -#define AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ - -#include "autoware/lidar_transfusion/transfusion_config.hpp" -#include "autoware/lidar_transfusion/utils.hpp" - -#include - -#include - -#include -#include -#include -#include -#include -#include - -namespace autoware::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); - - autoware::tensorrt_common::TrtUniquePtr engine{nullptr}; - autoware::tensorrt_common::TrtUniquePtr context{nullptr}; - -private: - bool parseONNX( - const std::string & onnx_path, const std::string & engine_path, const std::string & precision, - std::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); - - autoware::tensorrt_common::TrtUniquePtr runtime_{nullptr}; - autoware::tensorrt_common::TrtUniquePtr plan_{nullptr}; - autoware::tensorrt_common::Logger logger_; - TransfusionConfig config_; - std::vector tensors_names_; - - std::array in_profile_dims_; -}; - -} // namespace autoware::lidar_transfusion - -#endif // AUTOWARE__LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp index 363ee17a0d6e0..e60ee04150b5c 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_config.hpp @@ -35,21 +35,21 @@ class TransfusionConfig 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]; + voxels_num_[0] = static_cast(voxels_num[0]); + voxels_num_[1] = static_cast(voxels_num[1]); + voxels_num_[2] = static_cast(voxels_num[2]); - min_voxel_size_ = voxels_num[0]; - opt_voxel_size_ = voxels_num[1]; - max_voxel_size_ = voxels_num[2]; + min_voxel_size_ = static_cast(voxels_num[0]); + opt_voxel_size_ = static_cast(voxels_num[1]); + max_voxel_size_ = static_cast(voxels_num[2]); - min_points_size_ = voxels_num[0]; - opt_points_size_ = voxels_num[1]; - max_points_size_ = voxels_num[2]; + min_points_size_ = static_cast(voxels_num[0]); + opt_points_size_ = static_cast(voxels_num[1]); + max_points_size_ = static_cast(voxels_num[2]); - min_coors_size_ = voxels_num[0]; - opt_coors_size_ = voxels_num[1]; - max_coors_size_ = voxels_num[2]; + min_coors_size_ = static_cast(voxels_num[0]); + opt_coors_size_ = static_cast(voxels_num[1]); + max_coors_size_ = static_cast(voxels_num[2]); } if (point_cloud_range.size() == 6) { min_x_range_ = static_cast(point_cloud_range[0]); @@ -91,7 +91,7 @@ class TransfusionConfig std::size_t cloud_capacity_{}; ///// KERNEL PARAMETERS ///// const std::size_t threads_for_voxel_{256}; // threads number for a block - const std::size_t points_per_voxel_{20}; + const int32_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 @@ -99,9 +99,9 @@ class TransfusionConfig 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 + const int32_t batch_size_{1}; + const int32_t num_classes_{5}; + const int32_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}; @@ -114,9 +114,9 @@ class TransfusionConfig 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}; - std::size_t num_proposals_{200}; + const int32_t max_num_points_per_pillar_{points_per_voxel_}; + const int32_t num_point_values_{4}; + int32_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 @@ -126,7 +126,7 @@ class TransfusionConfig 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}; + const int32_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}; @@ -136,33 +136,33 @@ class TransfusionConfig std::size_t feature_y_size_{grid_y_size_ / out_size_factor_}; ///// RUNTIME DIMENSIONS ///// - std::vector voxels_num_{5000, 30000, 60000}; + 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]}; + int32_t min_voxel_size_{voxels_num_[0]}; + int32_t opt_voxel_size_{voxels_num_[1]}; + int32_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_}; + int32_t min_point_in_voxel_size_{points_per_voxel_}; + int32_t opt_point_in_voxel_size_{points_per_voxel_}; + int32_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_}; + int32_t min_network_feature_size_{num_point_feature_size_}; + int32_t opt_network_feature_size_{num_point_feature_size_}; + int32_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]}; + int32_t min_points_size_{voxels_num_[0]}; + int32_t opt_points_size_{voxels_num_[1]}; + int32_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]}; + int32_t min_coors_size_{voxels_num_[0]}; + int32_t opt_coors_size_{voxels_num_[1]}; + int32_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_}; + int32_t min_coors_dim_size_{num_point_values_}; + int32_t opt_coors_dim_size_{num_point_values_}; + int32_t max_coors_dim_size_{num_point_values_}; }; } // namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp index 06dd65b4b805f..00bb5e726706c 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/transfusion_trt.hpp @@ -16,7 +16,6 @@ #define AUTOWARE__LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ #include "autoware/lidar_transfusion/cuda_utils.hpp" -#include "autoware/lidar_transfusion/network/network_trt.hpp" #include "autoware/lidar_transfusion/postprocess/postprocess_kernel.hpp" #include "autoware/lidar_transfusion/preprocess/pointcloud_densification.hpp" #include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" @@ -24,6 +23,7 @@ #include "autoware/lidar_transfusion/utils.hpp" #include "autoware/lidar_transfusion/visibility_control.hpp" +#include #include #include @@ -34,38 +34,17 @@ #include #include #include -#include #include namespace autoware::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); + const tensorrt_common::TrtCommonConfig & trt_config, + const DensificationParam & densification_param, const TransfusionConfig config); virtual ~TransfusionTRT(); bool detect( @@ -73,6 +52,8 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT std::vector & det_boxes3d, std::unordered_map & proc_timing); protected: + void initTrt(const tensorrt_common::TrtCommonConfig & trt_config); + void initPtr(); bool preprocess(const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); @@ -81,7 +62,7 @@ class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT bool postprocess(std::vector & det_boxes3d); - std::unique_ptr network_trt_ptr_{nullptr}; + std::unique_ptr network_trt_ptr_{nullptr}; std::unique_ptr vg_ptr_{nullptr}; std::unique_ptr> stop_watch_ptr_{ nullptr}; diff --git a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp b/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp deleted file mode 100644 index 8e3cb8a55ec7e..0000000000000 --- a/perception/autoware_lidar_transfusion/lib/network/network_trt.cpp +++ /dev/null @@ -1,357 +0,0 @@ -// 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/lidar_transfusion/network/network_trt.hpp" - -#include - -#include -#include -#include -#include -#include - -namespace autoware::lidar_transfusion -{ - -inline NetworkIO nameToNetworkIO(const char * name) -{ - static const std::unordered_map name_to_enum = { - {"voxels", NetworkIO::voxels}, {"num_points", NetworkIO::num_points}, - {"coors", NetworkIO::coors}, {"cls_score0", NetworkIO::cls_score}, - {"bbox_pred0", NetworkIO::bbox_pred}, {"dir_cls_pred0", NetworkIO::dir_pred}}; - - auto it = name_to_enum.find(name); - if (it != name_to_enum.end()) { - return it->second; - } - throw std::runtime_error("Invalid input name: " + std::string(name)); -} - -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_ = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferRuntime(logger_)); - if (!runtime_) { - autoware::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) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create context: Engine was not created" << std::endl; - return false; - } - - context = autoware::tensorrt_common::TrtUniquePtr( - engine->createExecutionContext()); - if (!context) { - autoware::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 = autoware::tensorrt_common::TrtUniquePtr( - nvinfer1::createInferBuilder(logger_)); - if (!builder) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; - return false; - } - - auto config = autoware::tensorrt_common::TrtUniquePtr( - builder->createBuilderConfig()); - if (!config) { - autoware::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()) { - autoware::tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; - config->setFlag(nvinfer1::BuilderFlag::kFP16); - } else { - autoware::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 = autoware::tensorrt_common::TrtUniquePtr( - builder->createNetworkV2(flag)); - if (!network) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; - return false; - } - - auto parser = autoware::tensorrt_common::TrtUniquePtr( - nvonnxparser::createParser(*network, logger_)); - parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); - - if (!setProfile(*builder, *network, *config)) { - autoware::tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; - return false; - } - - plan_ = autoware::tensorrt_common::TrtUniquePtr( - builder->buildSerializedNetwork(*network, *config)); - if (!plan_) { - autoware::tensorrt_common::LOG_ERROR(logger_) - << "Failed to create serialized network" << std::endl; - return false; - } - engine = autoware::tensorrt_common::TrtUniquePtr( - runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); - if (!engine) { - autoware::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) -{ - autoware::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 = - autoware::tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - autoware::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) { - autoware::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."); - } - - // Initialize tensors_names_ with null values - tensors_names_.resize(NetworkIO::ENUM_SIZE, nullptr); - - // Loop over the tensor names and place them in the correct positions - for (int i = 0; i < NetworkIO::ENUM_SIZE; ++i) { - const char * name = engine->getIOTensorName(i); - tensors_names_[nameToNetworkIO(name)] = name; - } - - // 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 + " "; }); - autoware::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)}; - - autoware::tensorrt_common::LOG_INFO(logger_) - << "Profile for " << tensors_names_[i] << ": " << engine_dims << std::endl; - - if (engine_dims != in_profile_dims_[i]) { - autoware::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_)}); - autoware::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())) { - autoware::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])) { - autoware::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 autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu index 49131218ff698..a89268e646cfc 100644 --- a/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu +++ b/perception/autoware_lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -176,6 +176,8 @@ __global__ void generateVoxels_random_kernel( int voxel_idx = floorf((x - min_x_range) / pillar_x_size); int voxel_idy = floorf((y - min_y_range) / pillar_y_size); + voxel_idx = voxel_idx < 0 ? 0 : voxel_idx >= grid_x_size ? grid_x_size - 1 : voxel_idx; + voxel_idy = voxel_idy < 0 ? 0 : voxel_idy >= grid_y_size ? grid_y_size - 1 : voxel_idy; unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); diff --git a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp index 4e100f2e794d5..dd075a45cb29c 100644 --- a/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp +++ b/perception/autoware_lidar_transfusion/lib/transfusion_trt.cpp @@ -17,6 +17,7 @@ #include "autoware/lidar_transfusion/preprocess/preprocess_kernel.hpp" #include "autoware/lidar_transfusion/transfusion_config.hpp" +#include #include #include @@ -27,25 +28,23 @@ #include #include #include +#include #include namespace autoware::lidar_transfusion { TransfusionTRT::TransfusionTRT( - const NetworkParam & network_param, const DensificationParam & densification_param, - const TransfusionConfig & config) -: config_(config) + const tensorrt_common::TrtCommonConfig & trt_config, + const DensificationParam & densification_param, TransfusionConfig config) +: config_(std::move(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(); + initTrt(trt_config); CHECK_CUDA_ERROR(cudaStreamCreate(&stream_)); } @@ -99,6 +98,51 @@ void TransfusionTRT::initPtr() post_ptr_ = std::make_unique(config_, stream_); } +void TransfusionTRT::initTrt(const tensorrt_common::TrtCommonConfig & trt_config) +{ + std::vector network_io{ + autoware::tensorrt_common::NetworkIO( + "voxels", {3, {-1, config_.points_per_voxel_, config_.num_point_feature_size_}}), + autoware::tensorrt_common::NetworkIO("num_points", {1, {-1}}), + autoware::tensorrt_common::NetworkIO("coors", {2, {-1, config_.num_point_values_}}), + autoware::tensorrt_common::NetworkIO( + "cls_score0", {3, {config_.batch_size_, config_.num_classes_, config_.num_proposals_}}), + autoware::tensorrt_common::NetworkIO( + "bbox_pred0", {3, {config_.batch_size_, config_.num_box_values_, config_.num_proposals_}}), + autoware::tensorrt_common::NetworkIO( + "dir_cls_pred0", {3, {config_.batch_size_, 2, config_.num_proposals_}})}; + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + "voxels", + {3, + {config_.min_voxel_size_, config_.min_point_in_voxel_size_, + config_.min_network_feature_size_}}, + {3, + {config_.opt_voxel_size_, config_.opt_point_in_voxel_size_, + config_.opt_network_feature_size_}}, + {3, + {config_.max_voxel_size_, config_.max_point_in_voxel_size_, + config_.max_network_feature_size_}}), + autoware::tensorrt_common::ProfileDims( + "num_points", {1, {static_cast(config_.min_points_size_)}}, + {1, {static_cast(config_.opt_points_size_)}}, + {1, {static_cast(config_.max_points_size_)}}), + autoware::tensorrt_common::ProfileDims( + "coors", {2, {config_.min_coors_size_, config_.min_coors_dim_size_}}, + {2, {config_.opt_coors_size_, config_.opt_coors_dim_size_}}, + {2, {config_.max_coors_size_, config_.max_coors_dim_size_}})}; + + auto network_io_ptr = + std::make_unique>(network_io); + auto profile_dims_ptr = + std::make_unique>(profile_dims); + + network_trt_ptr_ = std::make_unique(trt_config); + if (!network_trt_ptr_->setup(std::move(profile_dims_ptr), std::move(network_io_ptr))) + throw std::runtime_error("Failed to setup TRT engine."); +} + bool TransfusionTRT::detect( const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d, std::unordered_map & proc_timing) @@ -166,8 +210,9 @@ bool TransfusionTRT::preprocess( CHECK_CUDA_ERROR(cudaMemcpyAsync( ¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost, stream_)); CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + auto params_input_i32 = static_cast(params_input); - if (params_input < config_.min_voxel_size_) { + if (params_input_i32 < config_.min_voxel_size_) { RCLCPP_WARN_STREAM( rclcpp::get_logger("lidar_transfusion"), "Too few voxels (" << params_input << ") for actual optimization profile (" @@ -185,39 +230,34 @@ bool TransfusionTRT::preprocess( 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), + bool success = true; + + // Inputs + success &= network_trt_ptr_->setTensor( + "voxels", voxel_features_d_.get(), nvinfer1::Dims3{ - static_cast(params_input), static_cast(config_.max_num_points_per_pillar_), + params_input_i32, 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; + success &= network_trt_ptr_->setTensor( + "num_points", voxel_num_d_.get(), nvinfer1::Dims{1, {params_input_i32}}); + success &= network_trt_ptr_->setTensor( + "coors", voxel_idxs_d_.get(), nvinfer1::Dims2{params_input_i32, config_.num_point_values_}); + + // Outputs + success &= network_trt_ptr_->setTensor("cls_score0", cls_output_d_.get()); + success &= network_trt_ptr_->setTensor("bbox_pred0", box_output_d_.get()); + success &= network_trt_ptr_->setTensor("dir_cls_pred0", dir_cls_output_d_.get()); + + return success; } bool TransfusionTRT::inference() { - auto status = network_trt_ptr_->context->enqueueV3(stream_); + auto status = network_trt_ptr_->enqueueV3(stream_); CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); if (!status) { diff --git a/perception/autoware_lidar_transfusion/package.xml b/perception/autoware_lidar_transfusion/package.xml index d9c43bc5a1b20..f9fd376e8be9a 100644 --- a/perception/autoware_lidar_transfusion/package.xml +++ b/perception/autoware_lidar_transfusion/package.xml @@ -2,7 +2,7 @@ autoware_lidar_transfusion - 0.40.0 + 0.41.0 The lidar_transfusion package Amadeusz Szymko Kenzo Lobos-Tsunekawa diff --git a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp index 8bb70a821621f..563abd97698e0 100644 --- a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp @@ -75,7 +75,6 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) 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( @@ -91,7 +90,9 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) 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); + auto trt_config = tensorrt_common::TrtCommonConfig(onnx_path, trt_precision, engine_path); + + detector_ptr_ = std::make_unique(trt_config, densification_param, config); cloud_sub_ = this->create_subscription( "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), @@ -155,7 +156,6 @@ void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::Co 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); diff --git a/perception/autoware_map_based_prediction/CHANGELOG.rst b/perception/autoware_map_based_prediction/CHANGELOG.rst index 55bc24894c230..0a67ffc40d586 100644 --- a/perception/autoware_map_based_prediction/CHANGELOG.rst +++ b/perception/autoware_map_based_prediction/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_map_based_prediction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(map_based_prediction): fix unintentional accumulation of lanelets (`#9950 `_) + add clear before insert +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9875 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_map_based_prediction +* Contributors: Fumiya Watanabe, Masaki Baba, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_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 index 1675f8b1fbfa2..b047cd28114ba 100644 --- a/perception/autoware_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 @@ -71,8 +71,8 @@ struct hash } // namespace std namespace autoware::map_based_prediction { +using autoware_internal_debug_msgs::msg::StringStamped; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_debug_msgs::msg::StringStamped; using TrajectoryPoints = std::vector; class MapBasedPredictionNode : public rclcpp::Node diff --git a/perception/autoware_map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml index 9012590a45b14..fa6a1195b1ed2 100644 --- a/perception/autoware_map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -2,7 +2,7 @@ autoware_map_based_prediction - 0.40.0 + 0.41.0 This package implements a map_based_prediction Tomoya Kimura Yoshi Ri @@ -16,6 +16,7 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -27,7 +28,6 @@ tf2 tf2_geometry_msgs tf2_ros - tier4_debug_msgs unique_identifier_msgs visualization_msgs diff --git a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index 42dbf5b83fa8c..b65020d6b4708 100644 --- a/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -670,9 +670,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt if (stop_watch_ptr_) { const auto processing_time_ms = stop_watch_ptr_->toc("processing_time", true); const auto cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_map_based_prediction/src/predictor_vru.cpp b/perception/autoware_map_based_prediction/src/predictor_vru.cpp index 0eea665cbd8a2..ffbdbd97b50a2 100644 --- a/perception/autoware_map_based_prediction/src/predictor_vru.cpp +++ b/perception/autoware_map_based_prediction/src/predictor_vru.cpp @@ -269,6 +269,7 @@ void PredictorVru::setLaneletMap(std::shared_ptr lanelet_ma const auto all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_); const auto crosswalks = lanelet::utils::query::crosswalkLanelets(all_lanelets); const auto walkways = lanelet::utils::query::walkwayLanelets(all_lanelets); + crosswalks_.clear(); crosswalks_.insert(crosswalks_.end(), crosswalks.begin(), crosswalks.end()); crosswalks_.insert(crosswalks_.end(), walkways.begin(), walkways.end()); diff --git a/perception/autoware_multi_object_tracker/CHANGELOG.rst b/perception/autoware_multi_object_tracker/CHANGELOG.rst index 5fced913f2b9d..b9d8d5f982397 100644 --- a/perception/autoware_multi_object_tracker/CHANGELOG.rst +++ b/perception/autoware_multi_object_tracker/CHANGELOG.rst @@ -2,6 +2,87 @@ Changelog for package autoware_multi_object_tracker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(multi_object_tracker): integrate odometry and transform processes (`#9912 `_) + * feat: Add odometry processor to multi-object tracker + * refactor: Refactor Odometry class for improved code organization and readability + * feat: Refactor Odometry class for improved code organization and readability + * refactor: Transform objects to world coordinate in Odometry class + refactor: Transform objects to world coordinate in Odometry class + refactor: Update Odometry class to get transform from tf with source frame ID + feat: Update Odometry class to get transform from tf with source frame ID + fix: move necessare tr2 header + * Revert "refactor: Transform objects to world coordinate in Odometry class" + This reverts commit efca28a40105f80deb09d57b55cb6f9d83ffda2c. + * refactor: Remove unnecessary tf2 headers from tracker models + * fix: move transform obtainer to odometry class + * refactor: Update Odometry class to get transform from tf with source frame ID + * refactor: Transform objects to world coordinate in Odometry class + * refactor: remove transformObjects from shapes + * refactor: Update Odometry class to use 'updateFromTf' instead of 'setOdometryFromTf' + * refactor: Update Odometry class to use 'updateFromTf' instead of 'setOdometryFromTf' + * refactor: Update InputManager to include Odometry in constructor + * refactor: Move odometry.cpp to lib folder + * move object transform to input stream + * refactor: Add enable_odometry_uncertainty parameter to Odometry constructor + * refactor: Update Odometry class to return optional Odometry from getOdometryFromTf + * refactor: Update Odometry class to use tf_cache\_ for storing and retrieving transforms + * refactor: Update Odometry class to use tf_cache\_ for storing and retrieving transforms + * refactor: bring odometry covariance modeler into odometry class + * refactor: Remove redundant code for updating tf cache in Odometry::updateTfCache + * refactor: Update runProcess parameter name to detected_objects + --------- +* feat: tier4_debug_msgs to autoware_internal_debug_msgs in files perc… (`#9879 `_) + feat: tier4_debug_msgs to autoware_internal_debug_msgs in files perception/autoware_multi_object_tracker +* chore(autoware_multi_object_tracker): fix autoware univserse documentation page (`#9772 `_) + * feat: Add descriptions for confidence thresholds in multi_object_tracker_node schema + * feat: Update multi_object_tracker_node schema with confidence threshold descriptions + --------- +* refactor(autoware_multi_object_tracker): define a new internal object class (`#9706 `_) + * feat: Add dynamic_object.hpp to object_model directory + * chore: Update autoware_perception_msgs include statements in association.hpp and dynamic_object.hpp + * fix: replace object message type to the DynamicObject type + * chore: Update autoware_perception_msgs include statements in association.hpp and dynamic_object.hpp + * chore: add channel index to the DynamicObjects + * Revert "chore: add channel index to the DynamicObjects" + This reverts commit c7e73f08a8d17b5b085dd330dbf187aabbec6879. + * fix: replace trackedobject in the process + * fix: Replace transformObjects with shapes::transformObjects for object transformation + * chore: add channel index to the DynamicObjects + * feat: separate shape related functions + * chore: clean up utils.hpp + * chore: Update function signatures to use DynamicObjectList instead of DynamicObjects + * chore: Add channel index to DynamicObject and DynamicObjectList + * chore: Refactor processor and debugger classes to remove channel_index parameter + * chore: Refactor multiple_vehicle_tracker.cpp and debugger.cpp + * Refactor object tracker classes to remove self_transform parameter + * Refactor object tracker classes to use shapes namespace for shape-related functions + * Refactor object tracker classes to use types.hpp for object model types + * Refactor object tracker classes to remove unused utils.hpp + * Refactor object tracker classes to use types.hpp for object model types + * chore: rename to types.cpp + * rename getDynamicObject to toDynamicObject + * Update perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp + Co-authored-by: Yukihiro Saito + --------- + Co-authored-by: Yukihiro Saito +* fix(autoware_multi_object_tracker): fix bugprone-errors (`#9651 `_) + fix: bugprone-errors +* refactor(autoware_multi_object_tracker): add configurable tracker parameters (`#9621 `_) + * refactor(autoware_multi_object_tracker): add configurable tracker parameters + * style(pre-commit): autofix + * refactor(autoware_multi_object_tracker): remove default values from parameter declarations + * refactor(autoware_multi_object_tracker): update schema file + * style(pre-commit): autofix + * Update perception/autoware_multi_object_tracker/src/processor/processor.cpp + * Update perception/autoware_multi_object_tracker/src/processor/processor.cpp + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Taekjin LEE +* Contributors: Fumiya Watanabe, Taekjin LEE, Vishal Chauhan, jakor97, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_multi_object_tracker/CMakeLists.txt b/perception/autoware_multi_object_tracker/CMakeLists.txt index 298b6605192a5..26895428d4c7f 100644 --- a/perception/autoware_multi_object_tracker/CMakeLists.txt +++ b/perception/autoware_multi_object_tracker/CMakeLists.txt @@ -28,8 +28,11 @@ set(${PROJECT_NAME}_src src/processor/input_manager.cpp ) set(${PROJECT_NAME}_lib + lib/odometry.cpp lib/association/association.cpp lib/association/mu_successive_shortest_path/mu_ssp.cpp + lib/object_model/types.cpp + lib/object_model/shapes.cpp lib/tracker/motion_model/motion_model_base.cpp lib/tracker/motion_model/bicycle_motion_model.cpp # cspell: ignore ctrv diff --git a/perception/autoware_multi_object_tracker/README.md b/perception/autoware_multi_object_tracker/README.md index afb1598645733..3f83817f6509f 100644 --- a/perception/autoware_multi_object_tracker/README.md +++ b/perception/autoware_multi_object_tracker/README.md @@ -69,7 +69,12 @@ Multiple inputs are pre-defined in the input channel parameters (described below ### Core Parameters +- Node + {{ json_to_markdown("perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json") }} + +- Association + {{ json_to_markdown("perception/autoware_multi_object_tracker/schema/data_association_matrix.schema.json") }} #### Simulation parameters diff --git a/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml b/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml index 7dd588ec8eeba..c9d0a3676b526 100644 --- a/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml +++ b/perception/autoware_multi_object_tracker/config/multi_object_tracker_node.param.yaml @@ -15,6 +15,21 @@ enable_delay_compensation: false consider_odometry_uncertainty: false + # tracker parameters + tracker_lifetime: 1.0 # [s] + min_known_object_removal_iou: 0.1 # [ratio] + min_unknown_object_removal_iou: 0.001 # [ratio] + distance_threshold: 5.0 # [m] + confident_count_threshold: + UNKNOWN: 3 + CAR: 3 + TRUCK: 3 + BUS: 3 + TRAILER: 3 + MOTORBIKE: 3 + BICYCLE: 3 + PEDESTRIAN: 3 + # debug parameters publish_processing_time: false publish_tentative_objects: false diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp index 2c12341d0aa67..b92e17987eb5f 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/association/association.hpp @@ -27,7 +27,7 @@ #include #include -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include #include @@ -58,7 +58,7 @@ class DataAssociation const Eigen::MatrixXd & src, std::unordered_map & direct_assignment, std::unordered_map & reverse_assignment); Eigen::MatrixXd calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, + const types::DynamicObjectList & measurements, const std::list> & trackers); virtual ~DataAssociation() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.hpp new file mode 100644 index 0000000000000..84c0eb912610a --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/shapes.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. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ + +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include + +#include + +#include + +namespace autoware::multi_object_tracker +{ +namespace shapes +{ +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area = 0.01); + +bool convertConvexHullToBoundingBox( + const types::DynamicObject & input_object, types::DynamicObject & output_object); + +bool getMeasurementYaw( + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw); + +int getNearestCornerOrSurface( + const double x, const double y, const double yaw, const double width, const double length, + const geometry_msgs::msg::Transform & self_transform); + +void calcAnchorPointOffset( + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset); +} // namespace shapes +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__SHAPES_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp new file mode 100644 index 0000000000000..7dab840aac481 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/object_model/types.hpp @@ -0,0 +1,88 @@ +// 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. +// +// +// Author: v1.0 Taekjin Lee + +#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace autoware::multi_object_tracker +{ +namespace types +{ +enum OrientationAvailability : uint8_t { + UNAVAILABLE = 0, + SIGN_UNKNOWN = 1, + AVAILABLE = 2, +}; + +struct ObjectKinematics +{ + geometry_msgs::msg::PoseWithCovariance pose_with_covariance; + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + bool has_position_covariance = false; + OrientationAvailability orientation_availability; + bool has_twist = false; + bool has_twist_covariance = false; +}; + +struct DynamicObject +{ + unique_identifier_msgs::msg::UUID object_id = unique_identifier_msgs::msg::UUID(); + uint channel_index; + float existence_probability; + std::vector classification; + ObjectKinematics kinematics; + autoware_perception_msgs::msg::Shape shape; +}; + +struct DynamicObjectList +{ + std_msgs::msg::Header header; + uint channel_index; + std::vector objects; +}; + +DynamicObject toDynamicObject( + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index = 0); + +DynamicObjectList toDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index = 0); + +autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object); + +} // namespace types + +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__OBJECT_MODEL__TYPES_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/odometry.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/odometry.hpp new file mode 100644 index 0000000000000..37a11b6eeb1f5 --- /dev/null +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/odometry.hpp @@ -0,0 +1,73 @@ +// Copyright 2025 TIER 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__MULTI_OBJECT_TRACKER__ODOMETRY_HPP_ +#define AUTOWARE__MULTI_OBJECT_TRACKER__ODOMETRY_HPP_ + +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include + +#include +#include + +#include +#include + +#include +#include +#include + +namespace autoware::multi_object_tracker +{ + +class Odometry +{ +public: + Odometry( + rclcpp::Node & node, const std::string & world_frame_id, + bool enable_odometry_uncertainty = false); + + std::optional getTransform( + const std::string & source_frame_id, const rclcpp::Time & time) const; + std::optional getTransform(const rclcpp::Time & time) const; + + std::optional getOdometryFromTf(const rclcpp::Time & time) const; + + std::optional transformObjects( + const types::DynamicObjectList & input_objects) const; + +private: + rclcpp::Node & node_; + // frame id + std::string ego_frame_id_ = "base_link"; // ego vehicle frame + std::string world_frame_id_; // absolute/relative ground frame + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + +public: + bool enable_odometry_uncertainty_ = false; + +private: + void updateTfCache( + const rclcpp::Time & time, const geometry_msgs::msg::Transform & transform) const; + + // cache of tf + mutable std::map tf_cache_; +}; + +} // namespace autoware::multi_object_tracker + +#endif // AUTOWARE__MULTI_OBJECT_TRACKER__ODOMETRY_HPP_ diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp index 8501c68b0cf23..ad3667eb240c0 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp @@ -19,18 +19,20 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__BICYCLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class BicycleTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::bicycle; @@ -50,23 +52,19 @@ class BicycleTracker : public Tracker public: BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp index 8b4fa1babf652..b9e026bf3c009 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/multiple_vehicle_tracker.hpp @@ -19,10 +19,11 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__MULTIPLE_VEHICLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" +#include #include namespace autoware::multi_object_tracker @@ -36,17 +37,13 @@ class MultipleVehicleTracker : public Tracker public: MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~MultipleVehicleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp index 45cd0f31a4e85..6230ba6e3b0f4 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp @@ -19,32 +19,30 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PASS_THROUGH_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "tracker_base.hpp" +#include + namespace autoware::multi_object_tracker { class PassThroughTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; - autoware_perception_msgs::msg::DetectedObject prev_observed_object_; + types::DynamicObject object_; + types::DynamicObject prev_observed_object_; rclcpp::Logger logger_; rclcpp::Time last_update_time_; public: PassThroughTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp index 4287e0f99d5ee..8a4bfc59d37ac 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_and_bicycle_tracker.hpp @@ -19,11 +19,13 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_AND_BICYCLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" +#include + namespace autoware::multi_object_tracker { @@ -35,17 +37,13 @@ class PedestrianAndBicycleTracker : public Tracker public: PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; virtual ~PedestrianAndBicycleTracker() {} }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp index f20b38f73e95f..5a2acc50a8249 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp @@ -19,18 +19,20 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__PEDESTRIAN_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class PedestrianTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; object_model::ObjectModel object_model_ = object_model::pedestrian; @@ -56,23 +58,19 @@ class PedestrianTracker : public Tracker public: PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) const; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp index 0b9ea9c44a6cf..ac5527fca6400 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/tracker_base.hpp @@ -20,14 +20,14 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__TRACKER_BASE_HPP_ #define EIGEN_MPL2_ONLY -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include +#include #include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" +#include +#include #include #include @@ -67,9 +67,8 @@ class Tracker return existence_vector.size() > 0; } bool updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, - const uint & channel_index); + const types::DynamicObject & object, const rclcpp::Time & measurement_time, + const geometry_msgs::msg::Transform & self_transform); bool updateWithoutMeasurement(const rclcpp::Time & now); // classification @@ -108,12 +107,11 @@ class Tracker protected: virtual bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) = 0; public: - virtual bool getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const = 0; + virtual bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const = 0; virtual bool predict(const rclcpp::Time & time) = 0; }; diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp index 9f128c864ad6c..075db6b8a9d69 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp @@ -19,17 +19,19 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__UNKNOWN_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { class UnknownTracker : public Tracker { private: - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; rclcpp::Logger logger_; struct EkfParams @@ -47,22 +49,17 @@ class UnknownTracker : public Tracker public: UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp index f3708fd1ff905..f50d117acc081 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp @@ -19,11 +19,13 @@ #ifndef AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ #define AUTOWARE__MULTI_OBJECT_TRACKER__TRACKER__MODEL__VEHICLE_TRACKER_HPP_ -#include "autoware/kalman_filter/kalman_filter.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" +#include + namespace autoware::multi_object_tracker { @@ -35,7 +37,7 @@ class VehicleTracker : public Tracker double velocity_deviation_threshold_; - autoware_perception_msgs::msg::DetectedObject object_; + types::DynamicObject object_; double z_; struct BoundingBox @@ -53,24 +55,19 @@ class VehicleTracker : public Tracker public: VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index); + const types::DynamicObject & object, const size_t channel_size); bool predict(const rclcpp::Time & time) override; bool measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) override; - bool measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object); - bool measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object); - bool getTrackedObject( - const rclcpp::Time & time, - autoware_perception_msgs::msg::TrackedObject & object) const override; + bool measureWithPose(const types::DynamicObject & object); + bool measureWithShape(const types::DynamicObject & object); + bool getTrackedObject(const rclcpp::Time & time, types::DynamicObject & object) const override; private: - autoware_perception_msgs::msg::DetectedObject getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform); + types::DynamicObject getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform); }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp index 12bd865795b85..44ad012a5f428 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp +++ b/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp @@ -19,6 +19,7 @@ #define AUTOWARE__MULTI_OBJECT_TRACKER__UNCERTAINTY__UNCERTAINTY_PROCESSOR_HPP_ #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include @@ -32,21 +33,19 @@ namespace uncertainty { using autoware::multi_object_tracker::object_model::ObjectModel; -using autoware_perception_msgs::msg::DetectedObject; -using autoware_perception_msgs::msg::DetectedObjects; using autoware_perception_msgs::msg::ObjectClassification; using nav_msgs::msg::Odometry; ObjectModel decodeObjectModel(const ObjectClassification & object_class); -DetectedObjects modelUncertainty(const DetectedObjects & detected_objects); +types::DynamicObjectList modelUncertainty(const types::DynamicObjectList & detected_objects); object_model::StateCovariance covarianceFromObjectClass( - const DetectedObject & detected_object, const ObjectClassification & object_class); + const types::DynamicObject & detected_object, const ObjectClassification & object_class); -void normalizeUncertainty(DetectedObjects & detected_objects); +void normalizeUncertainty(types::DynamicObjectList & detected_objects); -void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects); +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList & detected_objects); } // namespace uncertainty } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/association/association.cpp b/perception/autoware_multi_object_tracker/lib/association/association.cpp index 0c809ee5f086d..d74d87489f8db 100644 --- a/perception/autoware_multi_object_tracker/lib/association/association.cpp +++ b/perception/autoware_multi_object_tracker/lib/association/association.cpp @@ -15,8 +15,10 @@ #include "autoware/multi_object_tracker/association/association.hpp" #include "autoware/multi_object_tracker/association/solver/gnn_solver.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include #include #include @@ -150,7 +152,7 @@ void DataAssociation::assign( } Eigen::MatrixXd DataAssociation::calcScoreMatrix( - const autoware_perception_msgs::msg::DetectedObjects & measurements, + const types::DynamicObjectList & measurements, const std::list> & trackers) { Eigen::MatrixXd score_matrix = @@ -162,14 +164,13 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( for (size_t measurement_idx = 0; measurement_idx < measurements.objects.size(); ++measurement_idx) { - const autoware_perception_msgs::msg::DetectedObject & measurement_object = - measurements.objects.at(measurement_idx); + const types::DynamicObject & measurement_object = measurements.objects.at(measurement_idx); const std::uint8_t measurement_label = autoware::object_recognition_utils::getHighestProbLabel(measurement_object.classification); double score = 0.0; if (can_assign_matrix_(tracker_label, measurement_label)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; + types::DynamicObject tracked_object; (*tracker_itr)->getTrackedObject(measurements.header.stamp, tracked_object); const double max_dist = max_dist_matrix_(tracker_label, measurement_label); @@ -210,8 +211,8 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix( if (passed_gate) { const double min_iou = min_iou_matrix_(tracker_label, measurement_label); const double min_union_iou_area = 1e-2; - const double iou = autoware::object_recognition_utils::get2dIoU( - measurement_object, tracked_object, min_union_iou_area); + const double iou = + shapes::get2dIoU(measurement_object, tracked_object, min_union_iou_area); if (iou < min_iou) passed_gate = false; } diff --git a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp similarity index 70% rename from perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp rename to perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp index 833f768e171f4..2818e6dc53bb9 100644 --- a/perception/autoware_multi_object_tracker/include/autoware/multi_object_tracker/utils/utils.hpp +++ b/perception/autoware_multi_object_tracker/lib/object_model/shapes.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// 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. @@ -13,31 +13,148 @@ // limitations under the License. // // -// Author: v1.0 Yukihiro Saito -// +// Author: v1.0 Yukihiro Saito, Taekjin Lee -#ifndef AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ -#define AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +#include "autoware/multi_object_tracker/object_model/shapes.hpp" -#include #include +#include +#include + +#include +#include -#include "autoware_perception_msgs/msg/detected_object.hpp" -#include "autoware_perception_msgs/msg/shape.hpp" -#include "autoware_perception_msgs/msg/tracked_object.hpp" -#include -#include -#include +#include #include #include -#include -#include +#include #include -namespace utils +namespace autoware::multi_object_tracker +{ +namespace shapes +{ +inline double getSumArea(const std::vector & polygons) +{ + return std::accumulate( + polygons.begin(), polygons.end(), 0.0, [](double acc, autoware::universe_utils::Polygon2d p) { + return acc + boost::geometry::area(p); + }); +} + +inline double getIntersectionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) +{ + std::vector intersection_polygons; + boost::geometry::intersection(source_polygon, target_polygon, intersection_polygons); + return getSumArea(intersection_polygons); +} + +inline double getUnionArea( + const autoware::universe_utils::Polygon2d & source_polygon, + const autoware::universe_utils::Polygon2d & target_polygon) { + std::vector union_polygons; + boost::geometry::union_(source_polygon, target_polygon, union_polygons); + return getSumArea(union_polygons); +} + +double get2dIoU( + const types::DynamicObject & source_object, const types::DynamicObject & target_object, + const double min_union_area) +{ + static const double MIN_AREA = 1e-6; + + const auto source_polygon = autoware::universe_utils::toPolygon2d( + source_object.kinematics.pose_with_covariance.pose, source_object.shape); + if (boost::geometry::area(source_polygon) < MIN_AREA) return 0.0; + const auto target_polygon = autoware::universe_utils::toPolygon2d( + target_object.kinematics.pose_with_covariance.pose, target_object.shape); + if (boost::geometry::area(target_polygon) < MIN_AREA) return 0.0; + + const double intersection_area = getIntersectionArea(source_polygon, target_polygon); + if (intersection_area < MIN_AREA) return 0.0; + const double union_area = getUnionArea(source_polygon, target_polygon); + + const double iou = + union_area < min_union_area ? 0.0 : std::min(1.0, intersection_area / union_area); + return iou; +} + +/** + * @brief convert convex hull shape object to bounding box object + * @param input_object: input convex hull objects + * @param output_object: output bounding box objects + */ +bool convertConvexHullToBoundingBox( + const types::DynamicObject & input_object, types::DynamicObject & output_object) +{ + // check footprint size + if (input_object.shape.footprint.points.size() < 3) { + return false; + } + + // look for bounding box boundary + float max_x = 0; + float max_y = 0; + float min_x = 0; + float min_y = 0; + float max_z = 0; + for (const auto & point : input_object.shape.footprint.points) { + max_x = std::max(max_x, point.x); + max_y = std::max(max_y, point.y); + min_x = std::min(min_x, point.x); + min_y = std::min(min_y, point.y); + max_z = std::max(max_z, point.z); + } + + // calc new center + const Eigen::Vector2d center{ + input_object.kinematics.pose_with_covariance.pose.position.x, + input_object.kinematics.pose_with_covariance.pose.position.y}; + const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); + const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); + const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; + const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; + + // set output parameters + output_object = input_object; + output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); + output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); + + output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + output_object.shape.dimensions.x = max_x - min_x; + output_object.shape.dimensions.y = max_y - min_y; + output_object.shape.dimensions.z = max_z; + + return true; +} + +bool getMeasurementYaw( + const types::DynamicObject & object, const double & predicted_yaw, double & measurement_yaw) +{ + measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + + // check orientation sign is known or not, and fix the limiting delta yaw + double limiting_delta_yaw = M_PI_2; + if (object.kinematics.orientation_availability == types::OrientationAvailability::AVAILABLE) { + limiting_delta_yaw = M_PI; + } + // limiting delta yaw, even the availability is unknown + while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { + if (measurement_yaw < predicted_yaw) { + measurement_yaw += 2 * limiting_delta_yaw; + } else { + measurement_yaw -= 2 * limiting_delta_yaw; + } + } + // return false if the orientation is unknown + return object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; +} + enum BBOX_IDX { FRONT_SURFACE = 0, RIGHT_SURFACE = 1, @@ -51,17 +168,6 @@ enum BBOX_IDX { INVALID = -1 }; -/** - * @brief check if object label belongs to "large vehicle" - * @param label: input object label - * @return True if object label means large vehicle - */ -inline bool isLargeVehicleLabel(const uint8_t label) -{ - using Label = autoware_perception_msgs::msg::ObjectClassification; - return label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER; -} - /** * @brief Determine the Nearest Corner or Surface of detected object observed from ego vehicle * @@ -73,7 +179,7 @@ inline bool isLargeVehicleLabel(const uint8_t label) * @param self_transform: Ego vehicle position in map frame * @return int index */ -inline int getNearestCornerOrSurface( +int getNearestCornerOrSurface( const double x, const double y, const double yaw, const double width, const double length, const geometry_msgs::msg::Transform & self_transform) { @@ -171,10 +277,9 @@ inline Eigen::Vector2d calcOffsetVectorFromShapeChange( * @param offset_object: output tracking measurement to feed ekf * @return nearest corner index(int) */ -inline void calcAnchorPointOffset( - const double w, const double l, const int indx, - const autoware_perception_msgs::msg::DetectedObject & input_object, const double & yaw, - autoware_perception_msgs::msg::DetectedObject & offset_object, Eigen::Vector2d & tracking_offset) +void calcAnchorPointOffset( + const double w, const double l, const int indx, const types::DynamicObject & input_object, + const double & yaw, types::DynamicObject & offset_object, Eigen::Vector2d & tracking_offset) { // copy value offset_object = input_object; @@ -198,82 +303,6 @@ inline void calcAnchorPointOffset( offset_object.kinematics.pose_with_covariance.pose.position.y += rotated_offset.y(); } -/** - * @brief convert convex hull shape object to bounding box object - * @param input_object: input convex hull objects - * @param output_object: output bounding box objects - */ -inline bool convertConvexHullToBoundingBox( - const autoware_perception_msgs::msg::DetectedObject & input_object, - autoware_perception_msgs::msg::DetectedObject & output_object) -{ - // check footprint size - if (input_object.shape.footprint.points.size() < 3) { - return false; - } - - // look for bounding box boundary - float max_x = 0; - float max_y = 0; - float min_x = 0; - float min_y = 0; - float max_z = 0; - for (const auto & point : input_object.shape.footprint.points) { - max_x = std::max(max_x, point.x); - max_y = std::max(max_y, point.y); - min_x = std::min(min_x, point.x); - min_y = std::min(min_y, point.y); - max_z = std::max(max_z, point.z); - } - - // calc new center - const Eigen::Vector2d center{ - input_object.kinematics.pose_with_covariance.pose.position.x, - input_object.kinematics.pose_with_covariance.pose.position.y}; - const auto yaw = tf2::getYaw(input_object.kinematics.pose_with_covariance.pose.orientation); - const Eigen::Matrix2d R_inv = Eigen::Rotation2Dd(-yaw).toRotationMatrix(); - const Eigen::Vector2d new_local_center{(max_x + min_x) / 2.0, (max_y + min_y) / 2.0}; - const Eigen::Vector2d new_center = center + R_inv.transpose() * new_local_center; - - // set output parameters - output_object = input_object; - output_object.kinematics.pose_with_covariance.pose.position.x = new_center.x(); - output_object.kinematics.pose_with_covariance.pose.position.y = new_center.y(); - - output_object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; - output_object.shape.dimensions.x = max_x - min_x; - output_object.shape.dimensions.y = max_y - min_y; - output_object.shape.dimensions.z = max_z; - - return true; -} - -inline bool getMeasurementYaw( - const autoware_perception_msgs::msg::DetectedObject & object, const double & predicted_yaw, - double & measurement_yaw) -{ - measurement_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - - // check orientation sign is known or not, and fix the limiting delta yaw - double limiting_delta_yaw = M_PI_2; - if ( - object.kinematics.orientation_availability == - autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { - limiting_delta_yaw = M_PI; - } - // limiting delta yaw, even the availability is unknown - while (std::fabs(predicted_yaw - measurement_yaw) > limiting_delta_yaw) { - if (measurement_yaw < predicted_yaw) { - measurement_yaw += 2 * limiting_delta_yaw; - } else { - measurement_yaw -= 2 * limiting_delta_yaw; - } - } - // return false if the orientation is unknown - return object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; -} - -} // namespace utils +} // namespace shapes -#endif // AUTOWARE__MULTI_OBJECT_TRACKER__UTILS__UTILS_HPP_ +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/object_model/types.cpp b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp new file mode 100644 index 0000000000000..671d5313d0ff8 --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/object_model/types.cpp @@ -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. +// +// +// Author: v1.0 Taekjin Lee + +#include "autoware/multi_object_tracker/object_model/types.hpp" + +#include + +namespace autoware::multi_object_tracker +{ + +namespace types +{ + +DynamicObject toDynamicObject( + const autoware_perception_msgs::msg::DetectedObject & det_object, const uint channel_index) +{ + DynamicObject dynamic_object; + dynamic_object.channel_index = channel_index; + dynamic_object.existence_probability = det_object.existence_probability; + dynamic_object.classification = det_object.classification; + + dynamic_object.kinematics.pose_with_covariance = det_object.kinematics.pose_with_covariance; + dynamic_object.kinematics.twist_with_covariance = det_object.kinematics.twist_with_covariance; + dynamic_object.kinematics.has_position_covariance = det_object.kinematics.has_position_covariance; + if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::UNAVAILABLE; + } else if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::SIGN_UNKNOWN; + } else if ( + det_object.kinematics.orientation_availability == + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE) { + dynamic_object.kinematics.orientation_availability = OrientationAvailability::AVAILABLE; + } + dynamic_object.kinematics.has_twist = det_object.kinematics.has_twist; + dynamic_object.kinematics.has_twist_covariance = det_object.kinematics.has_twist_covariance; + + dynamic_object.shape = det_object.shape; + + return dynamic_object; +} + +DynamicObjectList toDynamicObjectList( + const autoware_perception_msgs::msg::DetectedObjects & det_objects, const uint channel_index) +{ + DynamicObjectList dynamic_objects; + dynamic_objects.header = det_objects.header; + dynamic_objects.channel_index = channel_index; + dynamic_objects.objects.reserve(det_objects.objects.size()); + for (const auto & det_object : det_objects.objects) { + dynamic_objects.objects.emplace_back(toDynamicObject(det_object, channel_index)); + } + return dynamic_objects; +} + +autoware_perception_msgs::msg::TrackedObject toTrackedObjectMsg(const DynamicObject & dyn_object) +{ + autoware_perception_msgs::msg::TrackedObject tracked_object; + tracked_object.object_id = dyn_object.object_id; + tracked_object.existence_probability = dyn_object.existence_probability; + tracked_object.classification = dyn_object.classification; + + tracked_object.kinematics.pose_with_covariance = dyn_object.kinematics.pose_with_covariance; + tracked_object.kinematics.twist_with_covariance = dyn_object.kinematics.twist_with_covariance; + if (dyn_object.kinematics.orientation_availability == OrientationAvailability::UNAVAILABLE) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; + } else if ( + dyn_object.kinematics.orientation_availability == OrientationAvailability::SIGN_UNKNOWN) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::SIGN_UNKNOWN; + } else if (dyn_object.kinematics.orientation_availability == OrientationAvailability::AVAILABLE) { + tracked_object.kinematics.orientation_availability = + autoware_perception_msgs::msg::TrackedObjectKinematics::AVAILABLE; + } + tracked_object.kinematics.is_stationary = false; + + tracked_object.shape = dyn_object.shape; + + return tracked_object; +} +} // namespace types + +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/odometry.cpp b/perception/autoware_multi_object_tracker/lib/odometry.cpp new file mode 100644 index 0000000000000..2d31fe876c329 --- /dev/null +++ b/perception/autoware_multi_object_tracker/lib/odometry.cpp @@ -0,0 +1,181 @@ +// Copyright 2025 TIER 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/multi_object_tracker/odometry.hpp" + +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" + +#include + +#include + +#include +#include + +namespace autoware::multi_object_tracker +{ + +Odometry::Odometry( + rclcpp::Node & node, const std::string & world_frame_id, bool enable_odometry_uncertainty) +: node_(node), + world_frame_id_(world_frame_id), + tf_buffer_(node_.get_clock()), + tf_listener_(tf_buffer_), + enable_odometry_uncertainty_(enable_odometry_uncertainty) +{ + // Create tf timer + auto cti = std::make_shared( + node_.get_node_base_interface(), node_.get_node_timers_interface()); + tf_buffer_.setCreateTimerInterface(cti); +} + +void Odometry::updateTfCache( + const rclcpp::Time & time, const geometry_msgs::msg::Transform & tf) const +{ + // update the tf buffer + tf_cache_.emplace(time, tf); + + // remove too old tf + const auto max_tf_age = rclcpp::Duration::from_seconds(1.0); + for (auto it = tf_cache_.begin(); it != tf_cache_.end();) { + if (time - it->first > max_tf_age) { + it = tf_cache_.erase(it); + } else { + ++it; + } + } +} + +std::optional Odometry::getTransform(const rclcpp::Time & time) const +{ + // check buffer and return if the transform is found + for (const auto & tf : tf_cache_) { + if (tf.first == time) { + return std::optional(tf.second); + } + } + // if not found, get the transform from tf + return getTransform(ego_frame_id_, time); +} + +std::optional Odometry::getTransform( + const std::string & source_frame_id, const rclcpp::Time & time) const +{ + try { + // Check if the frames are ready + std::string errstr; // This argument prevents error msg from being displayed in the terminal. + if (!tf_buffer_.canTransform( + world_frame_id_, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { + return std::nullopt; + } + + // Lookup the transform + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer_.lookupTransform( + world_frame_id_, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + + // update the cache + if (source_frame_id == ego_frame_id_) { + updateTfCache(time, self_transform_stamped.transform); + } + + return std::optional(self_transform_stamped.transform); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); + return std::nullopt; + } +} + +std::optional Odometry::getOdometryFromTf(const rclcpp::Time & time) const +{ + const auto self_transform = getTransform(time); + if (!self_transform) { + return std::nullopt; + } + const auto current_transform = self_transform.value(); + + nav_msgs::msg::Odometry odometry; + { + odometry.header.stamp = time + rclcpp::Duration::from_seconds(0.00001); + + // set the odometry pose + auto & odom_pose = odometry.pose.pose; + odom_pose.position.x = current_transform.translation.x; + odom_pose.position.y = current_transform.translation.y; + odom_pose.position.z = current_transform.translation.z; + odom_pose.orientation = current_transform.rotation; + + // set odometry twist + auto & odom_twist = odometry.twist.twist; + odom_twist.linear.x = 10.0; // m/s + odom_twist.linear.y = 0.1; // m/s + odom_twist.angular.z = 0.1; // rad/s + + // model the uncertainty + auto & odom_pose_cov = odometry.pose.covariance; + odom_pose_cov[0] = 0.1; // x-x + odom_pose_cov[7] = 0.1; // y-y + odom_pose_cov[35] = 0.0001; // yaw-yaw + + auto & odom_twist_cov = odometry.twist.covariance; + odom_twist_cov[0] = 2.0; // x-x [m^2/s^2] + odom_twist_cov[7] = 0.2; // y-y [m^2/s^2] + odom_twist_cov[35] = 0.001; // yaw-yaw [rad^2/s^2] + } + + return std::optional(odometry); +} + +std::optional Odometry::transformObjects( + const types::DynamicObjectList & input_objects) const +{ + types::DynamicObjectList output_objects = input_objects; + + // transform to world coordinate + if (input_objects.header.frame_id != world_frame_id_) { + output_objects.header.frame_id = world_frame_id_; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(input_objects.header.frame_id, input_objects.header.stamp); + if (!ros_target2objects_world) { + return std::nullopt; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_objects.objects) { + auto & pose_with_cov = object.kinematics.pose_with_covariance; + tf2::fromMsg(pose_with_cov.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + // transform pose, frame difference and object pose + tf2::toMsg(tf_target2objects, pose_with_cov.pose); + // transform covariance, only the frame difference + pose_with_cov.covariance = + tf2::transformCovariance(pose_with_cov.covariance, tf_target2objects_world); + } + } + // Add the odometry uncertainty to the object uncertainty + if (enable_odometry_uncertainty_) { + // Create a modeled odometry message + const auto odometry = getOdometryFromTf(input_objects.header.stamp); + // Add the odometry uncertainty to the object uncertainty + uncertainty::addOdometryUncertainty(odometry.value(), output_objects); + } + + return std::optional(output_objects); +} + +} // namespace autoware::multi_object_tracker diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp index 098ad39dd3df9..32caf16b17a2d 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/bicycle_tracker.cpp @@ -19,19 +19,17 @@ #include "autoware/multi_object_tracker/tracker/model/bicycle_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include #include +#include +#include +#include +#include +#include #include -#include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -46,9 +44,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; BicycleTracker::BicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("BicycleTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -56,7 +52,7 @@ BicycleTracker::BicycleTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // OBJECT SHAPE MODEL if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -148,16 +144,16 @@ bool BicycleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, +types::DynamicObject BicycleTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!utils::convertConvexHullToBoundingBox(object, updating_object)) { + if (!shapes::convertConvexHullToBoundingBox(object, updating_object)) { updating_object = object; } } @@ -165,12 +161,12 @@ autoware_perception_msgs::msg::DetectedObject BicycleTracker::getUpdatingObject( return updating_object; } -bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool BicycleTracker::measureWithPose(const types::DynamicObject & object) { // get measurement yaw angle to update const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); double measurement_yaw = 0.0; - bool is_yaw_available = utils::getMeasurementYaw(object, tracked_yaw, measurement_yaw); + bool is_yaw_available = shapes::getMeasurementYaw(object, tracked_yaw, measurement_yaw); // update bool is_updated = false; @@ -196,7 +192,7 @@ bool BicycleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect return is_updated; } -bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) +bool BicycleTracker::measureWithShape(const types::DynamicObject & object) { if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box @@ -208,11 +204,8 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec constexpr double size_min = 0.1; // [m] if ( object.shape.dimensions.x > size_max || object.shape.dimensions.y > size_max || - object.shape.dimensions.z > size_max) { - return false; - } else if ( - object.shape.dimensions.x < size_min || object.shape.dimensions.y < size_min || - object.shape.dimensions.z < size_min) { + object.shape.dimensions.z > size_max || object.shape.dimensions.x < size_min || + object.shape.dimensions.y < size_min || object.shape.dimensions.z < size_min) { return false; } @@ -238,7 +231,7 @@ bool BicycleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec } bool BicycleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -263,8 +256,7 @@ bool BicycleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -272,9 +264,9 @@ bool BicycleTracker::measure( } bool BicycleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp index ff06544bd509c..9f249ab3bc7bc 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/multiple_vehicle_tracker.cpp @@ -26,17 +26,13 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; MultipleVehicleTracker::MultipleVehicleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), - normal_vehicle_tracker_( - object_model::normal_vehicle, time, object, self_transform, channel_size, channel_index), - big_vehicle_tracker_( - object_model::big_vehicle, time, object, self_transform, channel_size, channel_index) + normal_vehicle_tracker_(object_model::normal_vehicle, time, object, channel_size), + big_vehicle_tracker_(object_model::big_vehicle, time, object, channel_size) { // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool MultipleVehicleTracker::predict(const rclcpp::Time & time) @@ -47,7 +43,7 @@ bool MultipleVehicleTracker::predict(const rclcpp::Time & time) } bool MultipleVehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { big_vehicle_tracker_.measure(object, time, self_transform); @@ -60,14 +56,14 @@ bool MultipleVehicleTracker::measure( } bool MultipleVehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using Label = autoware_perception_msgs::msg::ObjectClassification; const uint8_t label = getHighestProbLabel(); if (label == Label::CAR) { normal_vehicle_tracker_.getTrackedObject(time, object); - } else if (utils::isLargeVehicleLabel(label)) { + } else if (label == Label::BUS || label == Label::TRUCK || label == Label::TRAILER) { big_vehicle_tracker_.getTrackedObject(time, object); } object.object_id = getUUID(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp index f66e616241ae0..e9852baaa94af 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pass_through_tracker.cpp @@ -18,16 +18,12 @@ #define EIGEN_MPL2_ONLY #include "autoware/multi_object_tracker/tracker/model/pass_through_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - #include #include +#include +#include #include -#include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -40,9 +36,7 @@ namespace autoware::multi_object_tracker { PassThroughTracker::PassThroughTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PassThroughTracker")), last_update_time_(time) @@ -51,7 +45,7 @@ PassThroughTracker::PassThroughTracker( prev_observed_object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool PassThroughTracker::predict(const rclcpp::Time & time) @@ -66,7 +60,7 @@ bool PassThroughTracker::predict(const rclcpp::Time & time) } bool PassThroughTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { prev_observed_object_ = object_; @@ -88,10 +82,10 @@ bool PassThroughTracker::measure( } bool PassThroughTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); object.kinematics.pose_with_covariance.covariance[XYZRPY_COV_IDX::X_X] = 0.0; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp index 1b8018351f5a5..21ce949231062 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_and_bicycle_tracker.cpp @@ -24,15 +24,13 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianAndBicycleTracker::PedestrianAndBicycleTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), - pedestrian_tracker_(time, object, self_transform, channel_size, channel_index), - bicycle_tracker_(time, object, self_transform, channel_size, channel_index) + pedestrian_tracker_(time, object, channel_size), + bicycle_tracker_(time, object, channel_size) { // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); } bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) @@ -43,7 +41,7 @@ bool PedestrianAndBicycleTracker::predict(const rclcpp::Time & time) } bool PedestrianAndBicycleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { pedestrian_tracker_.measure(object, time, self_transform); @@ -56,7 +54,7 @@ bool PedestrianAndBicycleTracker::measure( } bool PedestrianAndBicycleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { using Label = autoware_perception_msgs::msg::ObjectClassification; const uint8_t label = getHighestProbLabel(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp index 2135514df8485..779fba7376857 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/pedestrian_tracker.cpp @@ -19,19 +19,15 @@ #include "autoware/multi_object_tracker/tracker/model/pedestrian_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - #include #include +#include +#include +#include +#include +#include #include -#include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -46,9 +42,7 @@ namespace autoware::multi_object_tracker using Label = autoware_perception_msgs::msg::ObjectClassification; PedestrianTracker::PedestrianTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("PedestrianTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -56,7 +50,7 @@ PedestrianTracker::PedestrianTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // OBJECT SHAPE MODEL bounding_box_ = { @@ -148,17 +142,16 @@ bool PedestrianTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject PedestrianTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, +types::DynamicObject PedestrianTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) const { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; return updating_object; } -bool PedestrianTracker::measureWithPose( - const autoware_perception_msgs::msg::DetectedObject & object) +bool PedestrianTracker::measureWithPose(const types::DynamicObject & object) { // update motion model bool is_updated = false; @@ -178,8 +171,7 @@ bool PedestrianTracker::measureWithPose( return is_updated; } -bool PedestrianTracker::measureWithShape( - const autoware_perception_msgs::msg::DetectedObject & object) +bool PedestrianTracker::measureWithShape(const types::DynamicObject & object) { constexpr double gain = 0.1; constexpr double gain_inv = 1.0 - gain; @@ -235,7 +227,7 @@ bool PedestrianTracker::measureWithShape( } bool PedestrianTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -259,8 +251,7 @@ bool PedestrianTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -269,9 +260,9 @@ bool PedestrianTracker::measure( } bool PedestrianTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp index 24e2b0c9f5acf..31ad1bf94cadd 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/tracker_base.cpp @@ -16,8 +16,6 @@ #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" - #include #include #include @@ -81,9 +79,8 @@ void Tracker::initializeExistenceProbabilities( } bool Tracker::updateWithMeasurement( - const autoware_perception_msgs::msg::DetectedObject & object, - const rclcpp::Time & measurement_time, const geometry_msgs::msg::Transform & self_transform, - const uint & channel_index) + const types::DynamicObject & object, const rclcpp::Time & measurement_time, + const geometry_msgs::msg::Transform & self_transform) { // Update existence probability { @@ -97,6 +94,7 @@ bool Tracker::updateWithMeasurement( constexpr float probability_false_detection = 0.2; // update measured channel probability without decay + const uint channel_index = object.channel_index; existence_probabilities_[channel_index] = updateProbability( existence_probabilities_[channel_index], probability_true_detection, probability_false_detection); @@ -202,7 +200,7 @@ void Tracker::updateClassification( geometry_msgs::msg::PoseWithCovariance Tracker::getPoseWithCovariance( const rclcpp::Time & time) const { - autoware_perception_msgs::msg::TrackedObject object; + types::DynamicObject object; getTrackedObject(time, object); return object.kinematics.pose_with_covariance; } diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp index 2805e43b88323..933eba820b1d1 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/unknown_tracker.cpp @@ -15,18 +15,15 @@ #include "autoware/multi_object_tracker/tracker/model/unknown_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" - #include #include +#include +#include +#include +#include +#include #include -#include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -34,15 +31,12 @@ #else #include #endif -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" namespace autoware::multi_object_tracker { UnknownTracker::UnknownTracker( - const rclcpp::Time & time, const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const rclcpp::Time & time, const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), logger_(rclcpp::get_logger("UnknownTracker")), z_(object.kinematics.pose_with_covariance.pose.position.z) @@ -50,7 +44,7 @@ UnknownTracker::UnknownTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // initialize params // measurement noise covariance @@ -142,11 +136,10 @@ bool UnknownTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/) +types::DynamicObject UnknownTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & /*self_transform*/) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // UNCERTAINTY MODEL if (!object.kinematics.has_position_covariance) { @@ -169,7 +162,7 @@ autoware_perception_msgs::msg::DetectedObject UnknownTracker::getUpdatingObject( return updating_object; } -bool UnknownTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool UnknownTracker::measureWithPose(const types::DynamicObject & object) { // update motion model bool is_updated = false; @@ -190,7 +183,7 @@ bool UnknownTracker::measureWithPose(const autoware_perception_msgs::msg::Detect } bool UnknownTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -207,17 +200,16 @@ bool UnknownTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); return true; } bool UnknownTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp index 2d1f48a3ad5ee..f497df1f52c6c 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/model/vehicle_tracker.cpp @@ -19,19 +19,17 @@ #include "autoware/multi_object_tracker/tracker/model/vehicle_tracker.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include #include +#include +#include +#include +#include +#include #include -#include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -47,9 +45,7 @@ using Label = autoware_perception_msgs::msg::ObjectClassification; VehicleTracker::VehicleTracker( const object_model::ObjectModel & object_model, const rclcpp::Time & time, - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & /*self_transform*/, const size_t channel_size, - const uint & channel_index) + const types::DynamicObject & object, const size_t channel_size) : Tracker(time, object.classification, channel_size), object_model_(object_model), logger_(rclcpp::get_logger("VehicleTracker")), @@ -59,7 +55,7 @@ VehicleTracker::VehicleTracker( object_ = object; // initialize existence probability - initializeExistenceProbabilities(channel_index, object.existence_probability); + initializeExistenceProbabilities(object.channel_index, object.existence_probability); // velocity deviation threshold // if the predicted velocity is close to the observed velocity, @@ -71,8 +67,8 @@ VehicleTracker::VehicleTracker( bounding_box_ = { object.shape.dimensions.x, object.shape.dimensions.y, object.shape.dimensions.z}; } else { - autoware_perception_msgs::msg::DetectedObject bbox_object; - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + types::DynamicObject bbox_object; + if (!shapes::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, "VehicleTracker::VehicleTracker: Failed to convert convex hull to bounding " @@ -167,17 +163,16 @@ bool VehicleTracker::predict(const rclcpp::Time & time) return motion_model_.predictState(time); } -autoware_perception_msgs::msg::DetectedObject VehicleTracker::getUpdatingObject( - const autoware_perception_msgs::msg::DetectedObject & object, - const geometry_msgs::msg::Transform & self_transform) +types::DynamicObject VehicleTracker::getUpdatingObject( + const types::DynamicObject & object, const geometry_msgs::msg::Transform & self_transform) { - autoware_perception_msgs::msg::DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // OBJECT SHAPE MODEL // convert to bounding box if input is convex shape - autoware_perception_msgs::msg::DetectedObject bbox_object = object; + types::DynamicObject bbox_object = object; if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { - if (!utils::convertConvexHullToBoundingBox(object, bbox_object)) { + if (!shapes::convertConvexHullToBoundingBox(object, bbox_object)) { RCLCPP_WARN( logger_, "VehicleTracker::getUpdatingObject: Failed to convert convex hull to bounding box."); @@ -191,16 +186,16 @@ autoware_perception_msgs::msg::DetectedObject VehicleTracker::getUpdatingObject( const double tracked_yaw = motion_model_.getStateElement(IDX::YAW); // get offset measurement - const int nearest_corner_index = utils::getNearestCornerOrSurface( + const int nearest_corner_index = shapes::getNearestCornerOrSurface( tracked_x, tracked_y, tracked_yaw, bounding_box_.width, bounding_box_.length, self_transform); - utils::calcAnchorPointOffset( + shapes::calcAnchorPointOffset( bounding_box_.width, bounding_box_.length, nearest_corner_index, bbox_object, tracked_yaw, updating_object, tracking_offset_); return updating_object; } -bool VehicleTracker::measureWithPose(const autoware_perception_msgs::msg::DetectedObject & object) +bool VehicleTracker::measureWithPose(const types::DynamicObject & object) { // current (predicted) state const double tracked_vel = motion_model_.getStateElement(IDX::VEL); @@ -242,7 +237,7 @@ bool VehicleTracker::measureWithPose(const autoware_perception_msgs::msg::Detect return is_updated; } -bool VehicleTracker::measureWithShape(const autoware_perception_msgs::msg::DetectedObject & object) +bool VehicleTracker::measureWithShape(const types::DynamicObject & object) { if (object.shape.type != autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // do not update shape if the input is not a bounding box @@ -295,7 +290,7 @@ bool VehicleTracker::measureWithShape(const autoware_perception_msgs::msg::Detec } bool VehicleTracker::measure( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, + const types::DynamicObject & object, const rclcpp::Time & time, const geometry_msgs::msg::Transform & self_transform) { // keep the latest input object @@ -320,8 +315,7 @@ bool VehicleTracker::measure( } // update object - const autoware_perception_msgs::msg::DetectedObject updating_object = - getUpdatingObject(object, self_transform); + const types::DynamicObject updating_object = getUpdatingObject(object, self_transform); measureWithPose(updating_object); measureWithShape(updating_object); @@ -329,9 +323,9 @@ bool VehicleTracker::measure( } bool VehicleTracker::getTrackedObject( - const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObject & object) const + const rclcpp::Time & time, types::DynamicObject & object) const { - object = autoware::object_recognition_utils::toTrackedObject(object_); + object = object_; object.object_id = getUUID(); object.classification = getClassification(); diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp index 89258835f920b..b9518afeaf95a 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/bicycle_motion_model.cpp @@ -20,13 +20,14 @@ #include "autoware/multi_object_tracker/tracker/motion_model/bicycle_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include + +#include #include @@ -315,7 +316,7 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c double w = vel * sin_slip / lr_; const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW)); const double w_dtdt = w * dt * dt; - const double vv_dtdt__lr = vel * vel * dt * dt / lr_; + const double vv_dtdt_lr = vel * vel * dt * dt / lr_; // Predict state vector X t+1 Eigen::MatrixXd X_next_t(DIM, 1); // predicted state @@ -332,11 +333,11 @@ bool BicycleMotionModel::predictStateStep(const double dt, KalmanFilter & ekf) c A(IDX::X, IDX::YAW) = -vel * sin_yaw * dt - 0.5 * vel * cos_yaw * w_dtdt; A(IDX::X, IDX::VEL) = cos_yaw * dt - sin_yaw * w_dtdt; A(IDX::X, IDX::SLIP) = - -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt__lr; + -vel * sin_yaw * dt - 0.5 * (cos_slip * sin_yaw + sin_slip * cos_yaw) * vv_dtdt_lr; A(IDX::Y, IDX::YAW) = vel * cos_yaw * dt - 0.5 * vel * sin_yaw * w_dtdt; A(IDX::Y, IDX::VEL) = sin_yaw * dt + cos_yaw * w_dtdt; A(IDX::Y, IDX::SLIP) = - vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt__lr; + vel * cos_yaw * dt + 0.5 * (cos_slip * cos_yaw - sin_slip * sin_yaw) * vv_dtdt_lr; A(IDX::YAW, IDX::VEL) = 1.0 / lr_ * sin_slip * dt; A(IDX::YAW, IDX::SLIP) = vel / lr_ * cos_slip * dt; diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp index 700800e94ecd5..2a2d7ea80bae1 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/ctrv_motion_model.cpp @@ -20,13 +20,14 @@ #include "autoware/multi_object_tracker/tracker/motion_model/ctrv_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include + +#include namespace autoware::multi_object_tracker { diff --git a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp index e139419197d79..fd3b03da398e1 100644 --- a/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp +++ b/perception/autoware_multi_object_tracker/lib/tracker/motion_model/cv_motion_model.cpp @@ -20,13 +20,12 @@ #include "autoware/multi_object_tracker/tracker/motion_model/cv_motion_model.hpp" #include "autoware/multi_object_tracker/tracker/motion_model/motion_model_base.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" -#include "autoware/universe_utils/math/normalization.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/msg_covariance.hpp" #include #include +#include +#include +#include #include diff --git a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp index ddfdc7ef7cb10..bdd3a78a198a8 100644 --- a/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp +++ b/perception/autoware_multi_object_tracker/lib/uncertainty/uncertainty_processor.cpp @@ -20,6 +20,8 @@ #include #include +#include + #include namespace autoware::multi_object_tracker @@ -55,10 +57,10 @@ object_model::StateCovariance covarianceFromObjectClass(const ObjectClassificati return obj_class_model.measurement_covariance; } -DetectedObject modelUncertaintyByClass( - const DetectedObject & object, const ObjectClassification & object_class) +types::DynamicObject modelUncertaintyByClass( + const types::DynamicObject & object, const ObjectClassification & object_class) { - DetectedObject updating_object = object; + types::DynamicObject updating_object = object; // measurement noise covariance const object_model::StateCovariance measurement_covariance = @@ -87,8 +89,7 @@ DetectedObject modelUncertaintyByClass( pose_cov[XYZRPY_COV_IDX::YAW_Y] = 0.0; // yaw - y pose_cov[XYZRPY_COV_IDX::YAW_YAW] = measurement_covariance.yaw; // yaw - yaw const bool is_yaw_available = - object.kinematics.orientation_availability != - autoware_perception_msgs::msg::DetectedObjectKinematics::UNAVAILABLE; + object.kinematics.orientation_availability != types::OrientationAvailability::UNAVAILABLE; if (!is_yaw_available) { pose_cov[XYZRPY_COV_IDX::YAW_YAW] *= 1e3; // yaw is not available, multiply large value } @@ -103,10 +104,11 @@ DetectedObject modelUncertaintyByClass( return updating_object; } -DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) +types::DynamicObjectList modelUncertainty(const types::DynamicObjectList & detected_objects) { - DetectedObjects updating_objects; + types::DynamicObjectList updating_objects; updating_objects.header = detected_objects.header; + updating_objects.channel_index = detected_objects.channel_index; for (const auto & object : detected_objects.objects) { if (object.kinematics.has_position_covariance) { updating_objects.objects.push_back(object); @@ -119,7 +121,7 @@ DetectedObjects modelUncertainty(const DetectedObjects & detected_objects) return updating_objects; } -void normalizeUncertainty(DetectedObjects & detected_objects) +void normalizeUncertainty(types::DynamicObjectList & detected_objects) { constexpr double min_cov_dist = 1e-4; constexpr double min_cov_rad = 1e-6; @@ -140,7 +142,7 @@ void normalizeUncertainty(DetectedObjects & detected_objects) } } -void addOdometryUncertainty(const Odometry & odometry, DetectedObjects & detected_objects) +void addOdometryUncertainty(const Odometry & odometry, types::DynamicObjectList & detected_objects) { const auto & odom_pose = odometry.pose.pose; const auto & odom_pose_cov = odometry.pose.covariance; diff --git a/perception/autoware_multi_object_tracker/package.xml b/perception/autoware_multi_object_tracker/package.xml index 912165d7b6ab6..08f9ac89e1d91 100644 --- a/perception/autoware_multi_object_tracker/package.xml +++ b/perception/autoware_multi_object_tracker/package.xml @@ -2,7 +2,7 @@ autoware_multi_object_tracker - 0.40.0 + 0.41.0 The ROS 2 autoware_multi_object_tracker package Yukihiro Saito Yoshi Ri diff --git a/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json b/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json index a40eb12d99b38..d3bca273da803 100644 --- a/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json +++ b/perception/autoware_multi_object_tracker/schema/multi_object_tracker_node.schema.json @@ -56,6 +56,76 @@ "description": "If True, tracker use timers to schedule publishers and use prediction step to extrapolate object state at desired timestamp.", "default": false }, + "consider_odometry_uncertainty": { + "type": "boolean", + "description": "If True, consider odometry uncertainty in tracking.", + "default": false + }, + "tracker_lifetime": { + "type": "number", + "description": "Lifetime of the tracker in seconds.", + "default": 1.0 + }, + "min_known_object_removal_iou": { + "type": "number", + "description": "Minimum IOU between associated objects with known label to remove younger tracker", + "default": 0.1 + }, + "min_unknown_object_removal_iou": { + "type": "number", + "description": "Minimum IOU between associated objects with unknown label to remove unknown tracker", + "default": 0.001 + }, + "distance_threshold": { + "type": "number", + "description": "Distance threshold in meters.", + "default": 5.0 + }, + "confident_count_threshold": { + "type": "object", + "properties": { + "UNKNOWN": { + "type": "number", + "description": "Number of measurements to consider tracker as confident for unknown object classes.", + "default": 3 + }, + "CAR": { + "type": "number", + "description": "Number of measurements to consider tracker as confident for car object classes.", + "default": 3 + }, + "TRUCK": { + "type": "number", + "description": "Number of measurements to consider tracker as confident for truck object classes.", + "default": 3 + }, + "BUS": { + "type": "number", + "description": "Number of measurements to consider tracker as confident for bus object classes.", + "default": 3 + }, + "TRAILER": { + "type": "number", + "description": "Number of measurements to consider tracker as confident for trailer object classes.", + "default": 3 + }, + "MOTORBIKE": { + "type": "number", + "description": "Number of measurements to consider tracker as confident for motorbike object classes.", + "default": 3 + }, + "BICYCLE": { + "type": "number", + "description": "Number of measurements to consider tracker as confident for bicycle object classes.", + "default": 3 + }, + "PEDESTRIAN": { + "type": "number", + "description": "Number of measurements to consider tracker as confident for pedestrian object classes.", + "default": 3 + } + } + }, "publish_processing_time": { "type": "boolean", "description": "Enable to publish debug message of process time information.", @@ -93,6 +163,12 @@ "publish_rate", "world_frame_id", "enable_delay_compensation", + "consider_odometry_uncertainty", + "tracker_lifetime", + "min_known_object_removal_iou", + "min_unknown_object_removal_iou", + "distance_threshold", + "confident_count_threshold", "publish_processing_time", "publish_tentative_objects", "publish_debug_markers", diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp index bc27525d85f55..6a1703e029920 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.cpp @@ -80,8 +80,7 @@ void TrackerObjectDebugger::reset() void TrackerObjectDebugger::collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & /*reverse_assignment*/) { @@ -94,13 +93,13 @@ void TrackerObjectDebugger::collect( ++tracker_itr, ++tracker_idx) { ObjectData object_data; object_data.time = message_time; - object_data.channel_id = channel_index; - autoware_perception_msgs::msg::TrackedObject tracked_object; + types::DynamicObject tracked_object; (*(tracker_itr))->getTrackedObject(message_time, tracked_object); object_data.uuid = uuidToBoostUuid(tracked_object.object_id); object_data.uuid_int = uuidToInt(object_data.uuid); object_data.uuid_str = uuidToString(tracked_object.object_id); + object_data.channel_id = tracked_object.channel_index; // tracker bool is_associated = false; diff --git a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp index e564afc9fd9d0..d507a0350bbb8 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debug_object.hpp @@ -88,8 +88,7 @@ class TrackerObjectDebugger } void collect( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp index 9d830bb9e5b81..3761fc1e6f43a 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.cpp @@ -127,7 +127,7 @@ void TrackerDebugger::startMeasurementTime( stamp_process_start_ = now; if (debug_settings_.publish_processing_time) { double input_latency_ms = (now - last_input_stamp_).seconds() * 1e3; - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/input_latency_ms", input_latency_ms); } // initialize debug time stamps @@ -169,15 +169,15 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim double measurement_to_object_ms = (object_time - last_input_stamp_).seconds() * 1e3; // starting from the measurement time - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms_); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", processing_time_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_latency_ms", processing_latency_ms); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/meas_to_tracked_object_ms", measurement_to_object_ms); } stamp_publish_output_ = now; @@ -185,15 +185,13 @@ void TrackerDebugger::endPublishTime(const rclcpp::Time & now, const rclcpp::Tim void TrackerDebugger::collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment) { if (!debug_settings_.publish_debug_markers) return; object_debugger_.collect( - message_time, list_tracker, channel_index, detected_objects, direct_assignment, - reverse_assignment); + message_time, list_tracker, detected_objects, direct_assignment, reverse_assignment); } // ObjectDebugger diff --git a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp index 2c05da0c999e2..3df901a63505c 100644 --- a/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp +++ b/perception/autoware_multi_object_tracker/src/debugger/debugger.hpp @@ -101,8 +101,7 @@ class TrackerDebugger } void collectObjectInfo( const rclcpp::Time & message_time, const std::list> & list_tracker, - const uint & channel_index, - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const std::unordered_map & direct_assignment, const std::unordered_map & reverse_assignment); void publishObjectsMarkers(); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp index a8e27ed4f0146..b90c570e7f4a6 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.cpp @@ -11,14 +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. -// -// + #define EIGEN_MPL2_ONLY #include "multi_object_tracker_node.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" #include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" -#include "autoware/multi_object_tracker/utils/utils.hpp" #include #include @@ -38,43 +37,13 @@ #include #include -namespace -{ -// Function to get the transform between two frames -boost::optional getTransformAnonymous( - const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, - const std::string & target_frame_id, const rclcpp::Time & time) -{ - try { - // Check if the frames are ready - std::string errstr; // This argument prevents error msg from being displayed in the terminal. - if (!tf_buffer.canTransform( - target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { - return boost::none; - } - - // Lookup the transform - geometry_msgs::msg::TransformStamped self_transform_stamped; - self_transform_stamped = tf_buffer.lookupTransform( - /*target*/ target_frame_id, /*src*/ source_frame_id, time, - rclcpp::Duration::from_seconds(0.5)); - return self_transform_stamped.transform; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("multi_object_tracker"), ex.what()); - return boost::none; - } -} - -} // namespace - namespace autoware::multi_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; +using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type; MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) : rclcpp::Node("multi_object_tracker", node_options), - tf_buffer_(this->get_clock()), - tf_listener_(tf_buffer_), last_published_time_(this->now()), last_updated_time_(this->now()) { @@ -88,14 +57,18 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) double publish_rate = declare_parameter("publish_rate"); // [hz] world_frame_id_ = declare_parameter("world_frame_id"); bool enable_delay_compensation{declare_parameter("enable_delay_compensation")}; - enable_odometry_uncertainty_ = declare_parameter("consider_odometry_uncertainty"); + bool enable_odometry_uncertainty = declare_parameter("consider_odometry_uncertainty"); declare_parameter("selected_input_channels", std::vector()); std::vector selected_input_channels = get_parameter("selected_input_channels").as_string_array(); // ROS interface - Publisher - tracked_objects_pub_ = create_publisher("output", rclcpp::QoS{1}); + tracked_objects_pub_ = + create_publisher("output", rclcpp::QoS{1}); + + // Odometry manager + odometry_ = std::make_shared(*this, world_frame_id_, enable_odometry_uncertainty); // ROS interface - Input channels // Get input channels @@ -143,16 +116,11 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) } // Initialize input manager - input_manager_ = std::make_unique(*this); + input_manager_ = std::make_unique(*this, odometry_); input_manager_->init(input_channels_); // Initialize input manager, set subscriptions input_manager_->setTriggerFunction( std::bind(&MultiObjectTracker::onTrigger, this)); // Set trigger function - // Create tf timer - auto cti = std::make_shared( - this->get_node_base_interface(), this->get_node_timers_interface()); - tf_buffer_.setCreateTimerInterface(cti); - // Create ROS time based timer. // If the delay compensation is enabled, the timer is used to publish the output at the correct // time. @@ -166,23 +134,45 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options) // Initialize processor { - std::map tracker_map; - tracker_map.insert( + TrackerProcessorConfig config; + config.tracker_map.insert( std::make_pair(Label::CAR, this->declare_parameter("car_tracker"))); - tracker_map.insert( + config.tracker_map.insert( std::make_pair(Label::TRUCK, this->declare_parameter("truck_tracker"))); - tracker_map.insert( + config.tracker_map.insert( std::make_pair(Label::BUS, this->declare_parameter("bus_tracker"))); - tracker_map.insert( + config.tracker_map.insert( std::make_pair(Label::TRAILER, this->declare_parameter("trailer_tracker"))); - tracker_map.insert(std::make_pair( + config.tracker_map.insert(std::make_pair( Label::PEDESTRIAN, this->declare_parameter("pedestrian_tracker"))); - tracker_map.insert( + config.tracker_map.insert( std::make_pair(Label::BICYCLE, this->declare_parameter("bicycle_tracker"))); - tracker_map.insert(std::make_pair( + config.tracker_map.insert(std::make_pair( Label::MOTORCYCLE, this->declare_parameter("motorcycle_tracker"))); + config.channel_size = input_channel_size_; + + // Declare parameters + config.tracker_lifetime = declare_parameter("tracker_lifetime"); + config.min_known_object_removal_iou = declare_parameter("min_known_object_removal_iou"); + config.min_unknown_object_removal_iou = + declare_parameter("min_unknown_object_removal_iou"); + config.distance_threshold = declare_parameter("distance_threshold"); + + // Map from class name to label + std::map class_name_to_label = { + {"UNKNOWN", Label::UNKNOWN}, {"CAR", Label::CAR}, + {"TRUCK", Label::TRUCK}, {"BUS", Label::BUS}, + {"TRAILER", Label::TRAILER}, {"MOTORBIKE", Label::MOTORCYCLE}, + {"BICYCLE", Label::BICYCLE}, {"PEDESTRIAN", Label::PEDESTRIAN}}; + + // Declare parameters and initialize confident_count_threshold_map + for (const auto & [class_name, class_label] : class_name_to_label) { + int64_t value = declare_parameter("confident_count_threshold." + class_name); + config.confident_count_threshold[class_label] = static_cast(value); + } - processor_ = std::make_unique(tracker_map, input_channel_size_); + // Initialize processor with parameters + processor_ = std::make_unique(config); } // Data association initialization @@ -217,18 +207,18 @@ void MultiObjectTracker::onTrigger() // process start last_updated_time_ = current_time; - const rclcpp::Time latest_time(objects_list.back().second.header.stamp); + const rclcpp::Time latest_time(objects_list.back().header.stamp); debugger_->startMeasurementTime(this->now(), latest_time); - // run process for each DetectedObjects + // run process for each DynamicObject for (const auto & objects_data : objects_list) { - runProcess(objects_data.second, objects_data.first); + runProcess(objects_data); } // process end debugger_->endMeasurementTime(this->now()); // Publish without delay compensation if (!publish_timer_) { - const auto latest_object_time = rclcpp::Time(objects_list.back().second.header.stamp); + const auto latest_object_time = rclcpp::Time(objects_list.back().header.stamp); checkAndPublish(latest_object_time); } } @@ -256,63 +246,18 @@ void MultiObjectTracker::onTimer() if (should_publish) checkAndPublish(current_time); } -void MultiObjectTracker::runProcess( - const DetectedObjects & input_objects, const uint & channel_index) +void MultiObjectTracker::runProcess(const types::DynamicObjectList & detected_objects) { // Get the time of the measurement const rclcpp::Time measurement_time = - rclcpp::Time(input_objects.header.stamp, this->now().get_clock_type()); + rclcpp::Time(detected_objects.header.stamp, this->now().get_clock_type()); - // Get the transform of the self frame - const auto self_transform = - getTransformAnonymous(tf_buffer_, "base_link", world_frame_id_, measurement_time); + // Get the self transform + const auto self_transform = odometry_->getTransform(measurement_time); if (!self_transform) { return; } - // Transform the objects to the world frame - DetectedObjects transformed_objects; - if (!autoware::object_recognition_utils::transformObjects( - input_objects, world_frame_id_, tf_buffer_, transformed_objects)) { - return; - } - - // the object uncertainty - if (enable_odometry_uncertainty_) { - // Create a modeled odometry message - nav_msgs::msg::Odometry odometry; - odometry.header.stamp = measurement_time + rclcpp::Duration::from_seconds(0.001); - - // set odometry pose from self_transform - auto & odom_pose = odometry.pose.pose; - odom_pose.position.x = self_transform->translation.x; - odom_pose.position.y = self_transform->translation.y; - odom_pose.position.z = self_transform->translation.z; - odom_pose.orientation = self_transform->rotation; - - // set odometry twist - auto & odom_twist = odometry.twist.twist; - odom_twist.linear.x = 10.0; // m/s - odom_twist.linear.y = 0.1; // m/s - odom_twist.angular.z = 0.1; // rad/s - - // model the uncertainty - auto & odom_pose_cov = odometry.pose.covariance; - odom_pose_cov[0] = 0.1; // x-x - odom_pose_cov[7] = 0.1; // y-y - odom_pose_cov[35] = 0.0001; // yaw-yaw - - auto & odom_twist_cov = odometry.twist.covariance; - odom_twist_cov[0] = 2.0; // x-x [m^2/s^2] - odom_twist_cov[7] = 0.2; // y-y [m^2/s^2] - odom_twist_cov[35] = 0.001; // yaw-yaw [rad^2/s^2] - - // Add the odometry uncertainty to the object uncertainty - uncertainty::addOdometryUncertainty(odometry, transformed_objects); - } - // Normalize the object uncertainty - uncertainty::normalizeUncertainty(transformed_objects); - /* prediction */ processor_->predict(measurement_time); @@ -320,7 +265,6 @@ void MultiObjectTracker::runProcess( std::unordered_map direct_assignment, reverse_assignment; { const auto & list_tracker = processor_->getListTracker(); - const auto & detected_objects = transformed_objects; // global nearest neighbor Eigen::MatrixXd score_matrix = association_->calcScoreMatrix( detected_objects, list_tracker); // row : tracker, col : measurement @@ -328,19 +272,19 @@ void MultiObjectTracker::runProcess( // Collect debug information - tracker list, existence probabilities, association results debugger_->collectObjectInfo( - measurement_time, processor_->getListTracker(), channel_index, transformed_objects, - direct_assignment, reverse_assignment); + measurement_time, processor_->getListTracker(), detected_objects, direct_assignment, + reverse_assignment); } /* tracker update */ - processor_->update(transformed_objects, *self_transform, direct_assignment, channel_index); + processor_->update(detected_objects, *self_transform, direct_assignment); /* tracker pruning */ processor_->prune(measurement_time); /* spawn new tracker */ - if (input_manager_->isChannelSpawnEnabled(channel_index)) { - processor_->spawn(transformed_objects, *self_transform, reverse_assignment, channel_index); + if (input_manager_->isChannelSpawnEnabled(detected_objects.channel_index)) { + processor_->spawn(detected_objects, reverse_assignment); } } @@ -365,7 +309,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const return; } // Create output msg - TrackedObjects output_msg, tentative_objects_msg; + autoware_perception_msgs::msg::TrackedObjects output_msg; output_msg.header.frame_id = world_frame_id_; processor_->getTrackedObjects(time, output_msg); @@ -377,7 +321,7 @@ void MultiObjectTracker::publish(const rclcpp::Time & time) const debugger_->endPublishTime(this->now(), time); if (debugger_->shouldPublishTentativeObjects()) { - TrackedObjects tentative_output_msg; + autoware_perception_msgs::msg::TrackedObjects tentative_output_msg; tentative_output_msg.header.frame_id = world_frame_id_; processor_->getTentativeObjects(time, tentative_output_msg); debugger_->publishTentativeObjects(tentative_output_msg); diff --git a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp index 79a8c1b98dcca..6a904dc7e2ef9 100644 --- a/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp +++ b/perception/autoware_multi_object_tracker/src/multi_object_tracker_node.hpp @@ -20,6 +20,8 @@ #define MULTI_OBJECT_TRACKER_NODE_HPP_ #include "autoware/multi_object_tracker/association/association.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" +#include "autoware/multi_object_tracker/odometry.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include "debugger/debugger.hpp" #include "processor/input_manager.hpp" @@ -55,10 +57,6 @@ namespace autoware::multi_object_tracker { -using DetectedObject = autoware_perception_msgs::msg::DetectedObject; -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; -using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; - class MultiObjectTracker : public rclcpp::Node { public: @@ -66,10 +64,9 @@ class MultiObjectTracker : public rclcpp::Node private: // ROS interface - rclcpp::Publisher::SharedPtr tracked_objects_pub_; - rclcpp::Subscription::SharedPtr detected_object_sub_; - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; + rclcpp::Publisher::SharedPtr tracked_objects_pub_; + rclcpp::Subscription::SharedPtr + detected_object_sub_; // debugger std::unique_ptr debugger_; @@ -87,10 +84,10 @@ class MultiObjectTracker : public rclcpp::Node std::string world_frame_id_; // tracking frame std::unique_ptr association_; std::unique_ptr processor_; - bool enable_odometry_uncertainty_; // input manager std::unique_ptr input_manager_; + std::shared_ptr odometry_; std::vector input_channels_{}; size_t input_channel_size_{}; @@ -100,7 +97,7 @@ class MultiObjectTracker : public rclcpp::Node void onTrigger(); // publish processes - void runProcess(const DetectedObjects & input_objects, const uint & channel_index); + void runProcess(const types::DynamicObjectList & detected_objects); void checkAndPublish(const rclcpp::Time & time); void publish(const rclcpp::Time & time) const; inline bool shouldTrackerPublish(const std::shared_ptr tracker) const; diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp index fa333ea06a4b9..1e0a340b19d55 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.cpp @@ -14,9 +14,11 @@ #include "input_manager.hpp" -#include +#include "autoware/multi_object_tracker/object_model/types.hpp" +#include "autoware/multi_object_tracker/uncertainty/uncertainty_processor.hpp" #include +#include #include #include @@ -25,7 +27,8 @@ namespace autoware::multi_object_tracker /////////////////////////// /////// InputStream /////// /////////////////////////// -InputStream::InputStream(rclcpp::Node & node, uint & index) : node_(node), index_(index) +InputStream::InputStream(rclcpp::Node & node, uint & index, std::shared_ptr odometry) +: node_(node), index_(index), odometry_(odometry) { } @@ -53,13 +56,29 @@ void InputStream::init(const InputChannel & input_channel) void InputStream::onMessage( const autoware_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg) { - const DetectedObjects & objects = *msg; + const autoware_perception_msgs::msg::DetectedObjects & objects = *msg; + + types::DynamicObjectList dynamic_objects = types::toDynamicObjectList(objects, index_); // Model the object uncertainty only if it is not available - DetectedObjects objects_with_uncertainty = uncertainty::modelUncertainty(objects); + types::DynamicObjectList objects_with_uncertainty = + uncertainty::modelUncertainty(dynamic_objects); + + // Transform the objects to the world frame + auto transformed_objects = odometry_->transformObjects(objects_with_uncertainty); + if (!transformed_objects) { + RCLCPP_WARN( + node_.get_logger(), "InputManager::onMessage %s: Failed to transform objects.", + long_name_.c_str()); + return; + } + dynamic_objects = transformed_objects.value(); + + // Normalize the object uncertainty + uncertainty::normalizeUncertainty(dynamic_objects); // Move the objects_with_uncertainty to the objects queue - objects_que_.push_back(std::move(objects_with_uncertainty)); + objects_que_.push_back(std::move(dynamic_objects)); while (objects_que_.size() > que_size_) { objects_que_.pop_front(); } @@ -167,8 +186,7 @@ void InputStream::getObjectsOlderThan( // Add the object if the object is older than the specified latest time if (object_time <= object_latest_time) { - std::pair objects_pair(index_, objects); - objects_list.push_back(objects_pair); + objects_list.push_back(objects); } } @@ -186,7 +204,8 @@ void InputStream::getObjectsOlderThan( //////////////////////////// /////// InputManager /////// //////////////////////////// -InputManager::InputManager(rclcpp::Node & node) : node_(node) +InputManager::InputManager(rclcpp::Node & node, std::shared_ptr odometry) +: node_(node), odometry_(odometry) { latest_exported_object_time_ = node_.now() - rclcpp::Duration::from_seconds(3.0); } @@ -205,7 +224,7 @@ void InputManager::init(const std::vector & input_channels) bool is_any_spawn_enabled = false; for (size_t i = 0; i < input_size_; i++) { uint index(i); - InputStream input_stream(node_, index); + InputStream input_stream(node_, index, odometry_); input_stream.init(input_channels[i]); input_stream.setTriggerFunction( std::bind(&InputManager::onTrigger, this, std::placeholders::_1)); @@ -216,10 +235,11 @@ void InputManager::init(const std::vector & input_channels) RCLCPP_INFO( node_.get_logger(), "InputManager::init Initializing %s input stream from %s", input_channels[i].long_name.c_str(), input_channels[i].input_topic.c_str()); - std::function func = - std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); - sub_objects_array_.at(i) = node_.create_subscription( - input_channels[i].input_topic, rclcpp::QoS{1}, func); + std::function + func = std::bind(&InputStream::onMessage, input_streams_.at(i), std::placeholders::_1); + sub_objects_array_.at(i) = + node_.create_subscription( + input_channels[i].input_topic, rclcpp::QoS{1}, func); } // Check if any spawn enabled input streams @@ -263,11 +283,11 @@ void InputManager::getObjectTimeInterval( // The default object_earliest_time is to have a 1-second time interval const rclcpp::Time object_earliest_time_default = object_latest_time - rclcpp::Duration::from_seconds(1.0); - if (latest_exported_object_time_ < object_earliest_time_default) { - // if the latest exported object time is too old, set to the default - object_earliest_time = object_earliest_time_default; - } else if (latest_exported_object_time_ > object_latest_time) { - // if the latest exported object time is newer than the object_latest_time, set to the default + if ( + latest_exported_object_time_ < object_earliest_time_default || + latest_exported_object_time_ > object_latest_time) { + // if the latest exported object time is too old or newer than the object_latest_time, + // set to the default object_earliest_time = object_earliest_time_default; } else { // The object_earliest_time is the latest exported object time @@ -339,15 +359,14 @@ bool InputManager::getObjects(const rclcpp::Time & now, ObjectsList & objects_li // Sort objects by timestamp std::sort( objects_list.begin(), objects_list.end(), - [](const std::pair & a, const std::pair & b) { - return (rclcpp::Time(a.second.header.stamp) - rclcpp::Time(b.second.header.stamp)).seconds() < - 0; + [](const types::DynamicObjectList & a, const types::DynamicObjectList & b) { + return (rclcpp::Time(a.header.stamp) - rclcpp::Time(b.header.stamp)).seconds() < 0; }); // Update the latest exported object time bool is_any_object = !objects_list.empty(); if (is_any_object) { - latest_exported_object_time_ = rclcpp::Time(objects_list.back().second.header.stamp); + latest_exported_object_time_ = rclcpp::Time(objects_list.back().header.stamp); } else { // check time jump back if (now < latest_exported_object_time_) { diff --git a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp index 01b3148251743..da715c39bb710 100644 --- a/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/input_manager.hpp @@ -15,9 +15,11 @@ #ifndef PROCESSOR__INPUT_MANAGER_HPP_ #define PROCESSOR__INPUT_MANAGER_HPP_ +#include "autoware/multi_object_tracker/object_model/types.hpp" +#include "autoware/multi_object_tracker/odometry.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_perception_msgs/msg/detected_objects.hpp" +#include #include #include @@ -28,8 +30,7 @@ namespace autoware::multi_object_tracker { -using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; -using ObjectsList = std::vector>; +using ObjectsList = std::vector; struct InputChannel { @@ -42,7 +43,7 @@ struct InputChannel class InputStream { public: - explicit InputStream(rclcpp::Node & node, uint & index); + explicit InputStream(rclcpp::Node & node, uint & index, std::shared_ptr odometry); void init(const InputChannel & input_channel); @@ -75,6 +76,7 @@ class InputStream private: rclcpp::Node & node_; uint index_; + std::shared_ptr odometry_; std::string input_topic_; std::string long_name_; @@ -82,11 +84,10 @@ class InputStream bool is_spawn_enabled_{}; size_t que_size_{30}; - std::deque objects_que_; + std::deque objects_que_; std::function func_trigger_; - // bool is_time_initialized_{false}; int initial_count_{0}; double latency_mean_{}; double latency_var_{}; @@ -100,7 +101,7 @@ class InputStream class InputManager { public: - explicit InputManager(rclcpp::Node & node); + InputManager(rclcpp::Node & node, std::shared_ptr odometry); void init(const std::vector & input_channels); void setTriggerFunction(std::function func_trigger) { func_trigger_ = func_trigger; } @@ -115,7 +116,10 @@ class InputManager private: rclcpp::Node & node_; - std::vector::SharedPtr> sub_objects_array_{}; + std::shared_ptr odometry_; + + std::vector::SharedPtr> + sub_objects_array_{}; bool is_initialized_{false}; rclcpp::Time latest_exported_object_time_; diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.cpp b/perception/autoware_multi_object_tracker/src/processor/processor.cpp index ebc11ea63e199..02ad0767dc815 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.cpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.cpp @@ -15,10 +15,13 @@ #include "processor.hpp" #include "autoware/multi_object_tracker/object_model/object_model.hpp" +#include "autoware/multi_object_tracker/object_model/shapes.hpp" +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/tracker.hpp" -#include "autoware/object_recognition_utils/object_recognition_utils.hpp" -#include "autoware_perception_msgs/msg/tracked_objects.hpp" +#include + +#include #include #include @@ -30,21 +33,10 @@ namespace autoware::multi_object_tracker { using Label = autoware_perception_msgs::msg::ObjectClassification; +using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type; -TrackerProcessor::TrackerProcessor( - const std::map & tracker_map, const size_t & channel_size) -: tracker_map_(tracker_map), channel_size_(channel_size) +TrackerProcessor::TrackerProcessor(const TrackerProcessorConfig & config) : config_(config) { - // Set tracker lifetime parameters - max_elapsed_time_ = 1.0; // [s] - - // Set tracker overlap remover parameters - min_iou_ = 0.1; // [ratio] - min_iou_for_unknown_object_ = 0.001; // [ratio] - distance_threshold_ = 5.0; // [m] - - // Set tracker confidence threshold - confident_count_threshold_ = 3; // [count] } void TrackerProcessor::predict(const rclcpp::Time & time) @@ -55,9 +47,9 @@ void TrackerProcessor::predict(const rclcpp::Time & time) } void TrackerProcessor::update( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment, const uint & channel_index) + const std::unordered_map & direct_assignment) { int tracker_idx = 0; const auto & time = detected_objects.header.stamp; @@ -66,8 +58,7 @@ void TrackerProcessor::update( if (direct_assignment.find(tracker_idx) != direct_assignment.end()) { // found const auto & associated_object = detected_objects.objects.at(direct_assignment.find(tracker_idx)->second); - (*(tracker_itr)) - ->updateWithMeasurement(associated_object, time, self_transform, channel_index); + (*(tracker_itr))->updateWithMeasurement(associated_object, time, self_transform); } else { // not found (*(tracker_itr))->updateWithoutMeasurement(time); } @@ -75,9 +66,8 @@ void TrackerProcessor::update( } void TrackerProcessor::spawn( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, - const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment, const uint & channel_index) + const types::DynamicObjectList & detected_objects, + const std::unordered_map & reverse_assignment) { const auto & time = detected_objects.header.stamp; for (size_t i = 0; i < detected_objects.objects.size(); ++i) { @@ -85,44 +75,36 @@ void TrackerProcessor::spawn( continue; } const auto & new_object = detected_objects.objects.at(i); - std::shared_ptr tracker = - createNewTracker(new_object, time, self_transform, channel_index); + std::shared_ptr tracker = createNewTracker(new_object, time); if (tracker) list_tracker_.push_back(tracker); } } std::shared_ptr TrackerProcessor::createNewTracker( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const + const types::DynamicObject & object, const rclcpp::Time & time) const { - const std::uint8_t label = + const LabelType label = autoware::object_recognition_utils::getHighestProbLabel(object.classification); - if (tracker_map_.count(label) != 0) { - const auto tracker = tracker_map_.at(label); + if (config_.tracker_map.count(label) != 0) { + const auto tracker = config_.tracker_map.at(label); if (tracker == "bicycle_tracker") - return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + return std::make_shared(time, object, config_.channel_size); if (tracker == "big_vehicle_tracker") return std::make_shared( - object_model::big_vehicle, time, object, self_transform, channel_size_, channel_index); + object_model::big_vehicle, time, object, config_.channel_size); if (tracker == "multi_vehicle_tracker") - return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + return std::make_shared(time, object, config_.channel_size); if (tracker == "normal_vehicle_tracker") return std::make_shared( - object_model::normal_vehicle, time, object, self_transform, channel_size_, channel_index); + object_model::normal_vehicle, time, object, config_.channel_size); if (tracker == "pass_through_tracker") - return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + return std::make_shared(time, object, config_.channel_size); if (tracker == "pedestrian_and_bicycle_tracker") - return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + return std::make_shared(time, object, config_.channel_size); if (tracker == "pedestrian_tracker") - return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + return std::make_shared(time, object, config_.channel_size); } - return std::make_shared( - time, object, self_transform, channel_size_, channel_index); + return std::make_shared(time, object, config_.channel_size); } void TrackerProcessor::prune(const rclcpp::Time & time) @@ -137,7 +119,7 @@ void TrackerProcessor::removeOldTracker(const rclcpp::Time & time) { // Check elapsed time from last update for (auto itr = list_tracker_.begin(); itr != list_tracker_.end(); ++itr) { - const bool is_old = max_elapsed_time_ < (*itr)->getElapsedTimeFromLastUpdate(time); + const bool is_old = config_.tracker_lifetime < (*itr)->getElapsedTimeFromLastUpdate(time); // If the tracker is old, delete it if (is_old) { auto erase_itr = itr; @@ -152,12 +134,12 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) { // Iterate through the list of trackers for (auto itr1 = list_tracker_.begin(); itr1 != list_tracker_.end(); ++itr1) { - autoware_perception_msgs::msg::TrackedObject object1; + types::DynamicObject object1; if (!(*itr1)->getTrackedObject(time, object1)) continue; // Compare the current tracker with the remaining trackers for (auto itr2 = std::next(itr1); itr2 != list_tracker_.end(); ++itr2) { - autoware_perception_msgs::msg::TrackedObject object2; + types::DynamicObject object2; if (!(*itr2)->getTrackedObject(time, object2)) continue; // Calculate the distance between the two objects @@ -168,14 +150,13 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) object2.kinematics.pose_with_covariance.pose.position.y); // If the distance is too large, skip - if (distance > distance_threshold_) { + if (distance > config_.distance_threshold) { continue; } // Check the Intersection over Union (IoU) between the two objects - const double min_union_iou_area = 1e-2; - const auto iou = - autoware::object_recognition_utils::get2dIoU(object1, object2, min_union_iou_area); + constexpr double min_union_iou_area = 1e-2; + const auto iou = shapes::get2dIoU(object1, object2, min_union_iou_area); const auto & label1 = (*itr1)->getHighestProbLabel(); const auto & label2 = (*itr2)->getHighestProbLabel(); bool should_delete_tracker1 = false; @@ -184,7 +165,7 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) // If both trackers are UNKNOWN, delete the younger tracker // If one side of the tracker is UNKNOWN, delete UNKNOWN objects if (label1 == Label::UNKNOWN || label2 == Label::UNKNOWN) { - if (iou > min_iou_for_unknown_object_) { + if (iou > config_.min_unknown_object_removal_iou) { if (label1 == Label::UNKNOWN && label2 == Label::UNKNOWN) { if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { should_delete_tracker1 = true; @@ -198,7 +179,7 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time) } } } else { // If neither object is UNKNOWN, delete the younger tracker - if (iou > min_iou_) { + if (iou > config_.min_known_object_removal_iou) { if ((*itr1)->getTotalMeasurementCount() < (*itr2)->getTotalMeasurementCount()) { should_delete_tracker1 = true; } else { @@ -226,20 +207,21 @@ bool TrackerProcessor::isConfidentTracker(const std::shared_ptr & track // Confidence is determined by counting the number of measurements. // If the number of measurements is equal to or greater than the threshold, the tracker is // considered confident. - return tracker->getTotalMeasurementCount() >= confident_count_threshold_; + auto label = tracker->getHighestProbLabel(); + return tracker->getTotalMeasurementCount() >= config_.confident_count_threshold.at(label); } void TrackerProcessor::getTrackedObjects( const rclcpp::Time & time, autoware_perception_msgs::msg::TrackedObjects & tracked_objects) const { tracked_objects.header.stamp = time; + types::DynamicObject tracked_object; for (const auto & tracker : list_tracker_) { // Skip if the tracker is not confident if (!isConfidentTracker(tracker)) continue; // Get the tracked object, extrapolated to the given time - autoware_perception_msgs::msg::TrackedObject tracked_object; if (tracker->getTrackedObject(time, tracked_object)) { - tracked_objects.objects.push_back(tracked_object); + tracked_objects.objects.push_back(toTrackedObjectMsg(tracked_object)); } } } @@ -249,11 +231,11 @@ void TrackerProcessor::getTentativeObjects( autoware_perception_msgs::msg::TrackedObjects & tentative_objects) const { tentative_objects.header.stamp = time; + types::DynamicObject tracked_object; for (const auto & tracker : list_tracker_) { if (!isConfidentTracker(tracker)) { - autoware_perception_msgs::msg::TrackedObject tracked_object; if (tracker->getTrackedObject(time, tracked_object)) { - tentative_objects.objects.push_back(tracked_object); + tentative_objects.objects.push_back(toTrackedObjectMsg(tracked_object)); } } } diff --git a/perception/autoware_multi_object_tracker/src/processor/processor.hpp b/perception/autoware_multi_object_tracker/src/processor/processor.hpp index 851a0f6a26717..ad296b1c07d8d 100644 --- a/perception/autoware_multi_object_tracker/src/processor/processor.hpp +++ b/perception/autoware_multi_object_tracker/src/processor/processor.hpp @@ -15,6 +15,7 @@ #ifndef PROCESSOR__PROCESSOR_HPP_ #define PROCESSOR__PROCESSOR_HPP_ +#include "autoware/multi_object_tracker/object_model/types.hpp" #include "autoware/multi_object_tracker/tracker/model/tracker_base.hpp" #include @@ -31,23 +32,34 @@ namespace autoware::multi_object_tracker { +using LabelType = autoware_perception_msgs::msg::ObjectClassification::_label_type; + +struct TrackerProcessorConfig +{ + std::map tracker_map; + size_t channel_size; + float tracker_lifetime; // [s] + float min_known_object_removal_iou; // ratio [0, 1] + float min_unknown_object_removal_iou; // ratio [0, 1] + double distance_threshold; // [m] + std::map confident_count_threshold; // [count] +}; + class TrackerProcessor { public: - explicit TrackerProcessor( - const std::map & tracker_map, const size_t & channel_size); + explicit TrackerProcessor(const TrackerProcessorConfig & config); const std::list> & getListTracker() const { return list_tracker_; } // tracker processes void predict(const rclcpp::Time & time); void update( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, + const types::DynamicObjectList & detected_objects, const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & direct_assignment, const uint & channel_index); + const std::unordered_map & direct_assignment); void spawn( - const autoware_perception_msgs::msg::DetectedObjects & detected_objects, - const geometry_msgs::msg::Transform & self_transform, - const std::unordered_map & reverse_assignment, const uint & channel_index); + const types::DynamicObjectList & detected_objects, + const std::unordered_map & reverse_assignment); void prune(const rclcpp::Time & time); // output @@ -62,22 +74,12 @@ class TrackerProcessor void getExistenceProbabilities(std::vector> & existence_vectors) const; private: - std::map tracker_map_; + TrackerProcessorConfig config_; std::list> list_tracker_; - const size_t channel_size_; - - // parameters - float max_elapsed_time_; // [s] - float min_iou_; // [ratio] - float min_iou_for_unknown_object_; // [ratio] - double distance_threshold_; // [m] - int confident_count_threshold_; // [count] - void removeOldTracker(const rclcpp::Time & time); void removeOverlappedTracker(const rclcpp::Time & time); std::shared_ptr createNewTracker( - const autoware_perception_msgs::msg::DetectedObject & object, const rclcpp::Time & time, - const geometry_msgs::msg::Transform & self_transform, const uint & channel_index) const; + const types::DynamicObject & object, const rclcpp::Time & time) const; }; } // namespace autoware::multi_object_tracker diff --git a/perception/autoware_object_merger/CHANGELOG.rst b/perception/autoware_object_merger/CHANGELOG.rst index 004d56c406ce9..864cc6779fdf5 100644 --- a/perception/autoware_object_merger/CHANGELOG.rst +++ b/perception/autoware_object_merger/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package autoware_object_merger ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_object_merger): tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9893 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_object_merger + Co-authored-by: Taekjin LEE +* fix(perception): fix perception docs (`#9766 `_) + * fix: fix perception docs + * fix: fix missing parameter in schema + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * style(pre-commit): autofix + * chore: seperate paramters for different nodes + --------- + Co-authored-by: Taekjin LEE + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat(autoware_object_merger, autoware_tracking_object_merger): enable anonymized node names to be configurable (`#9733 `_) + feat: enable anonymized node names to be configurable +* Contributors: Fumiya Watanabe, Taekjin LEE, Vishal Chauhan, Yi-Hsiang Fang (Vivid) + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_object_merger/README.md b/perception/autoware_object_merger/README.md index c65353efa3a91..a78cd70052707 100644 --- a/perception/autoware_object_merger/README.md +++ b/perception/autoware_object_merger/README.md @@ -25,8 +25,16 @@ The successive shortest path algorithm is used to solve the data association pro ## Parameters +- object association merger + {{ json_to_markdown("perception/autoware_object_merger/schema/object_association_merger.schema.json") }} + +- data association matrix + {{ json_to_markdown("perception/autoware_object_merger/schema/data_association_matrix.schema.json") }} + +- overlapped judge + {{ json_to_markdown("perception/autoware_object_merger/schema/overlapped_judge.schema.json") }} ## Tips diff --git a/perception/autoware_object_merger/launch/object_association_merger.launch.xml b/perception/autoware_object_merger/launch/object_association_merger.launch.xml index f3c0e8bd5a829..4747b2af284fc 100644 --- a/perception/autoware_object_merger/launch/object_association_merger.launch.xml +++ b/perception/autoware_object_merger/launch/object_association_merger.launch.xml @@ -1,5 +1,6 @@ + @@ -8,7 +9,7 @@ - + diff --git a/perception/autoware_object_merger/package.xml b/perception/autoware_object_merger/package.xml index 481bb0babf20d..be9dec7a83c80 100644 --- a/perception/autoware_object_merger/package.xml +++ b/perception/autoware_object_merger/package.xml @@ -2,7 +2,7 @@ autoware_object_merger - 0.40.0 + 0.41.0 The autoware_object_merger package Yukihiro Saito Yoshi Ri diff --git a/perception/autoware_object_merger/schema/data_association_matrix.schema.json b/perception/autoware_object_merger/schema/data_association_matrix.schema.json index 68dc977224ba5..52f6aa3d8c37c 100644 --- a/perception/autoware_object_merger/schema/data_association_matrix.schema.json +++ b/perception/autoware_object_merger/schema/data_association_matrix.schema.json @@ -2,8 +2,8 @@ "$schema": "http://json-schema.org/draft-07/schema#", "title": "Data Association Matrix Parameters", "type": "object", - "properties": { - "ros__parameters": { + "definitions": { + "data_association_matrix": { "type": "object", "properties": { "can_assign_matrix": { @@ -11,31 +11,72 @@ "items": { "type": "number" }, - "description": "Assignment table for data association" + "description": "Assignment table for data association.", + "default": [ + 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 1, 1, 1, 1, + 0, 0, 0, 0, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, 0, 0, 0, 1, 1, 1, 0, 0, + 0, 0, 0, 1, 1, 1 + ] }, "max_dist_matrix": { "type": "array", "items": { "type": "number" }, - "description": "Maximum distance table for data association" + "description": "Maximum distance table for data association.", + "default": [ + 4.0, 4.0, 5.0, 5.0, 5.0, 2.0, 2.0, 2.0, 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 5.0, 5.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 3.0, 2.0, 1.0, 1.0, + 1.0, 1.0, 3.0, 3.0, 3.0, 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0 + ] }, "max_rad_matrix": { "type": "array", "items": { - "type": "number" + "type": "number", + "minimum": 0.0 }, - "description": "Maximum angle table for data association. If value is greater than pi, it will be ignored." + "description": "Maximum angle table for data association. If value is greater than pi, it will be ignored.", + "default": [ + 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 1.047, 1.047, 1.047, 1.047, 3.15, + 3.15, 3.15, 3.15, 1.047, 1.047, 1.047, 1.047, 3.15, 3.15, 3.15, 3.15, 1.047, 1.047, + 1.047, 1.047, 3.15, 3.15, 3.15, 3.15, 1.047, 1.047, 1.047, 1.047, 3.15, 3.15, 3.15, + 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, + 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15, 3.15 + ] }, "min_iou_matrix": { "type": "array", "items": { - "type": "number" + "type": "number", + "minimum": 0.0 }, - "description": "Minimum IoU threshold matrix for data association. If value is negative, it will be ignored." + "description": "Minimum IoU threshold matrix for data association. If value is negative, it will be ignored.", + "default": [ + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.3, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, 0.1, + 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, 0.2, + 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, + 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 + ] + } + }, + "required": ["can_assign_matrix", "max_dist_matrix", "max_rad_matrix", "min_iou_matrix"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/data_association_matrix" } }, - "required": ["can_assign_matrix", "max_dist_matrix", "max_rad_matrix", "min_iou_matrix"] + "required": ["ros__parameters"], + "additionalProperties": false } - } + }, + "required": ["/**"], + "additionalProperties": false } diff --git a/perception/autoware_object_merger/schema/object_association_merger.schema.json b/perception/autoware_object_merger/schema/object_association_merger.schema.json index 11090fab9c7b3..c31cb8866b8cc 100644 --- a/perception/autoware_object_merger/schema/object_association_merger.schema.json +++ b/perception/autoware_object_merger/schema/object_association_merger.schema.json @@ -45,7 +45,8 @@ "remove_overlapped_unknown_objects", "base_link_frame_id", "priority_mode" - ] + ], + "additionalProperties": false } }, "properties": { @@ -56,8 +57,10 @@ "$ref": "#/definitions/object_association_merger" } }, - "required": ["ros__parameters"] + "required": ["ros__parameters"], + "additionalProperties": false } }, - "required": ["/**"] + "required": ["/**"], + "additionalProperties": false } diff --git a/perception/autoware_object_merger/schema/overlapped_judge.schema.json b/perception/autoware_object_merger/schema/overlapped_judge.schema.json index b65464c6201d1..b8ed9f313eb3a 100644 --- a/perception/autoware_object_merger/schema/overlapped_judge.schema.json +++ b/perception/autoware_object_merger/schema/overlapped_judge.schema.json @@ -2,8 +2,8 @@ "$schema": "http://json-schema.org/draft-07/schema#", "title": "Overlapped Judge Parameters", "type": "object", - "properties": { - "ros__parameters": { + "definitions": { + "overlapped_judge": { "type": "object", "properties": { "distance_threshold_list": { @@ -11,17 +11,36 @@ "items": { "type": "number" }, - "description": "Distance threshold for each class used in judging overlap." + "description": "Distance threshold for each class used in judging overlap.", + "default": [9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0, 9.0] }, "generalized_iou_threshold": { "type": "array", "items": { - "type": "number" + "type": "number", + "minimum": -1.0, + "maximum": 1.0 }, - "description": "Generalized IoU threshold for each class." + "description": "Generalized IoU threshold for each class.", + "default": [-0.1, -0.1, -0.1, -0.6, -0.6, -0.1, -0.1, -0.1] + } + }, + "required": ["distance_threshold_list", "generalized_iou_threshold"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/overlapped_judge" } }, - "required": ["distance_threshold_list", "generalized_iou_threshold"] + "required": ["ros__parameters"], + "additionalProperties": false } - } + }, + "required": ["/**"], + "additionalProperties": false } diff --git a/perception/autoware_object_merger/src/object_association_merger_node.cpp b/perception/autoware_object_merger/src/object_association_merger_node.cpp index 0e2b88f4aa566..40bdd0a7d6938 100644 --- a/perception/autoware_object_merger/src/object_association_merger_node.cpp +++ b/perception/autoware_object_merger/src/object_association_merger_node.cpp @@ -235,9 +235,9 @@ void ObjectAssociationMergerNode::objectsCallback( merged_object_pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(merged_object_pub_, output_msg.header.stamp); // publish processing time - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace autoware::object_merger diff --git a/perception/autoware_object_range_splitter/CHANGELOG.rst b/perception/autoware_object_range_splitter/CHANGELOG.rst index 2ad998b66391f..b7969830ce765 100644 --- a/perception/autoware_object_range_splitter/CHANGELOG.rst +++ b/perception/autoware_object_range_splitter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_object_range_splitter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/perception/autoware_object_range_splitter/package.xml b/perception/autoware_object_range_splitter/package.xml index bfc8470834443..8c1d17faf629a 100644 --- a/perception/autoware_object_range_splitter/package.xml +++ b/perception/autoware_object_range_splitter/package.xml @@ -2,7 +2,7 @@ autoware_object_range_splitter - 0.40.0 + 0.41.0 The autoware_object_range_splitter package Yukihiro Saito Yoshi Ri diff --git a/perception/autoware_object_velocity_splitter/CHANGELOG.rst b/perception/autoware_object_velocity_splitter/CHANGELOG.rst index ec69fb1754d3b..2c7b06c4e5bb1 100644 --- a/perception/autoware_object_velocity_splitter/CHANGELOG.rst +++ b/perception/autoware_object_velocity_splitter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_object_velocity_splitter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/perception/autoware_object_velocity_splitter/package.xml b/perception/autoware_object_velocity_splitter/package.xml index 681b29b0c49e8..45a3c635150f4 100644 --- a/perception/autoware_object_velocity_splitter/package.xml +++ b/perception/autoware_object_velocity_splitter/package.xml @@ -2,7 +2,7 @@ autoware_object_velocity_splitter - 0.40.0 + 0.41.0 autoware_object_velocity_splitter Sathshi Tanaka Shunsuke Miura diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/CHANGELOG.rst b/perception/autoware_occupancy_grid_map_outlier_filter/CHANGELOG.rst index cf7dc5752c62d..db5cf2bb9a118 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/CHANGELOG.rst +++ b/perception/autoware_occupancy_grid_map_outlier_filter/CHANGELOG.rst @@ -2,6 +2,30 @@ Changelog for package autoware_occupancy_grid_map_outlier_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_occupancy_grid_map_outlier_filter): tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9894 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_occupancy_grid_map_outlier_filter + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* fix(perception): fix perception docs (`#9766 `_) + * fix: fix perception docs + * fix: fix missing parameter in schema + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * style(pre-commit): autofix + * chore: seperate paramters for different nodes + --------- + Co-authored-by: Taekjin LEE + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Vishal Chauhan, Yi-Hsiang Fang (Vivid) + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/README.md b/perception/autoware_occupancy_grid_map_outlier_filter/README.md index 7de2cc1dce92c..b920aa6908946 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/README.md +++ b/perception/autoware_occupancy_grid_map_outlier_filter/README.md @@ -40,7 +40,7 @@ The following video is a sample. Yellow points are high occupancy probability, g ## Parameters -{{ json_to_markdown("perception/occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json") }} +{{ json_to_markdown("perception/autoware_occupancy_grid_map_outlier_filter/schema/occupancy_grid_map_outlier_filter.schema.json") }} ## Assumptions / Known limits diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/package.xml b/perception/autoware_occupancy_grid_map_outlier_filter/package.xml index fba074fdc669f..113213b65e6d0 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/package.xml +++ b/perception/autoware_occupancy_grid_map_outlier_filter/package.xml @@ -2,7 +2,7 @@ autoware_occupancy_grid_map_outlier_filter - 0.40.0 + 0.41.0 The ROS 2 occupancy_grid_map_outlier_filter package amc-nu Yukihiro Saito diff --git a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp index eb52b903b90ca..094825056e62c 100644 --- a/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp +++ b/perception/autoware_occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_node.cpp @@ -420,9 +420,9 @@ void OccupancyGridMapOutlierFilterComponent::onOccupancyGridMapAndPointCloud2( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/CHANGELOG.rst b/perception/autoware_probabilistic_occupancy_grid_map/CHANGELOG.rst index 75d579b612168..b161d11d6524b 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/CHANGELOG.rst +++ b/perception/autoware_probabilistic_occupancy_grid_map/CHANGELOG.rst @@ -2,6 +2,57 @@ Changelog for package autoware_probabilistic_occupancy_grid_map ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_probabilistic_occupancy_grid_map): cuda accelerated implementation (`#9542 `_) + * feat: implemented a cuda accelerated ogm + * chore: fixed cspells + * chore: unused header and variable names + * chore: cspell fixes + * chore: cppcheck fix attempt + * fix: attempting to fix ci/cd regarding the cuda library + * chore: fixed the order of the cuda check in the cmakelist + * fix: removed cuda as a required dep for cpu only builds + * fix: missing cuda linking (?) + * feat: fixed single mode, added streams, and added the restrict keyword + * chore: replaced a potential indetermination using an epsilon + * Update perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater_kernel.cu + Co-authored-by: Yoshi Ri + * Update perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater_kernel.cu + Co-authored-by: Yoshi Ri + * style(pre-commit): autofix + * chore: added bound checkings in the update origin kernel + * chore: disabled tests since universe does not support cuda in ci/cd + * chore: added me as a maintainer + * fix: missedn the end in the cmake + * chore: moved the boudnary checks to only the cuda version since the cpu version uses the upstream logic + --------- + Co-authored-by: Yoshi Ri + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat(autoware_probabilistic_occupancy_grid_map): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_probabilistic_occupancy_grid_map (`#9895 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_probabilistic_occupancy_grid_map + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* fix(perception): fix perception docs (`#9766 `_) + * fix: fix perception docs + * fix: fix missing parameter in schema + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * Update perception/autoware_object_merger/schema/data_association_matrix.schema.json + Co-authored-by: Taekjin LEE + * style(pre-commit): autofix + * chore: seperate paramters for different nodes + --------- + Co-authored-by: Taekjin LEE + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* fix(autoware_probabilistic_occupancy_grid_map): fix bugprone-branch-clone (`#9652 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, Kenzo Lobos Tsunekawa, Vishal Chauhan, Yi-Hsiang Fang (Vivid), kobayu858 + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/perception/autoware_probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/autoware_probabilistic_occupancy_grid_map/CMakeLists.txt index 28342ecb5bcff..7dda1604f10f3 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/autoware_probabilistic_occupancy_grid_map/CMakeLists.txt @@ -4,27 +4,54 @@ project(autoware_probabilistic_occupancy_grid_map) find_package(autoware_cmake REQUIRED) autoware_package() +find_package(CUDA) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(PCL REQUIRED) +if(NOT ${CUDA_FOUND}) + message(WARNING "cuda was not found, so the autoware_probabilistic_occupancy_grid_map package will not be built.") + return() +endif() + include_directories( + include SYSTEM + ${CUDA_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} + ${grid_map_ros_INCLUDE_DIRS} +) + +# cSpell: ignore expt +list(APPEND CUDA_NVCC_FLAGS --expt-relaxed-constexpr -diag-suppress 20012 --compiler-options -fPIC) +set(CUDA_SEPARABLE_COMPILATION ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +cuda_add_library(${PROJECT_NAME}_cuda SHARED + lib/costmap_2d/occupancy_grid_map_fixed_kernel.cu + lib/costmap_2d/occupancy_grid_map_projective_kernel.cu + lib/updater/binary_bayes_filter_updater_kernel.cu + lib/updater/log_odds_bayes_filter_updater_kernel.cu + lib/utils/utils_kernel.cu +) + +target_link_libraries(${PROJECT_NAME}_cuda + ${CUDA_LIBRARIES} ) ament_auto_add_library(${PROJECT_NAME}_common SHARED + lib/costmap_2d/occupancy_grid_map_base.cpp lib/updater/binary_bayes_filter_updater.cpp lib/utils/utils.cpp ) target_link_libraries(${PROJECT_NAME}_common ${PCL_LIBRARIES} + ${PROJECT_NAME}_cuda ) # PointcloudBasedOccupancyGridMap ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp - lib/costmap_2d/occupancy_grid_map_base.cpp lib/costmap_2d/occupancy_grid_map_fixed.cpp lib/costmap_2d/occupancy_grid_map_projective.cpp ) @@ -32,6 +59,7 @@ ament_auto_add_library(pointcloud_based_occupancy_grid_map SHARED target_link_libraries(pointcloud_based_occupancy_grid_map ${PCL_LIBRARIES} ${PROJECT_NAME}_common + ${PROJECT_NAME}_cuda ) rclcpp_components_register_node(pointcloud_based_occupancy_grid_map @@ -73,6 +101,11 @@ rclcpp_components_register_node(synchronized_grid_map_fusion EXECUTABLE synchronized_grid_map_fusion_node ) +install( + TARGETS ${PROJECT_NAME}_cuda + DESTINATION lib +) + ament_auto_package( INSTALL_TO_SHARE launch @@ -80,27 +113,31 @@ ament_auto_package( ) # test -if(BUILD_TESTING) - # launch_testing - find_package(launch_testing_ament_cmake REQUIRED) - add_launch_test(test/test_pointcloud_based_method.py) - add_launch_test(test/test_synchronized_grid_map_fusion_node.py) - - # gtest - ament_add_gtest(test_utils - test/test_utils.cpp - ) - ament_add_gtest(costmap_unit_tests - test/cost_value_test.cpp - ) - ament_add_gtest(fusion_policy_unit_tests - test/fusion_policy_test.cpp - lib/fusion_policy/fusion_policy.cpp - ) - target_link_libraries(test_utils - ${PCL_LIBRARIES} - ${PROJECT_NAME}_common - ) - target_include_directories(costmap_unit_tests PRIVATE "include") - target_include_directories(fusion_policy_unit_tests PRIVATE "include") -endif() +# Temporary disabled, tracked by: +# https://github.com/autowarefoundation/autoware.universe/issues/7724 +#if(BUILD_TESTING) +# # launch_testing +# find_package(launch_testing_ament_cmake REQUIRED) +# add_launch_test(test/test_pointcloud_based_method.py) +# add_launch_test(test/test_synchronized_grid_map_fusion_node.py) +# +# # gtest +# ament_add_gtest(test_utils +# test/test_utils.cpp +# ) +# ament_add_gtest(costmap_unit_tests +# test/cost_value_test.cpp +# ) +# ament_add_gtest(fusion_policy_unit_tests +# test/fusion_policy_test.cpp +# lib/fusion_policy/fusion_policy.cpp +# ) +# target_link_libraries(test_utils +# ${CUDA_LIBRARIES} +# ${PCL_LIBRARIES} +# ${PROJECT_NAME}_common +# ${PROJECT_NAME}_cuda +# ) +# target_include_directories(costmap_unit_tests PRIVATE "include") +# target_include_directories(fusion_policy_unit_tests PRIVATE "include") +#endif() diff --git a/perception/autoware_probabilistic_occupancy_grid_map/README.md b/perception/autoware_probabilistic_occupancy_grid_map/README.md index 575411bcbd220..3637dcb10daeb 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/README.md +++ b/perception/autoware_probabilistic_occupancy_grid_map/README.md @@ -21,12 +21,29 @@ You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which mean ### Parameters -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/grid_map.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json") }} -{{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json") }} +- binary bayes filter updater + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/binary_bayes_filter_updater.schema.json") }} + +- grid map + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/grid_map.schema.json") }} + +- laserscan based occupancy grid map + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/laserscan_based_occupancy_grid_map.schema.json") }} + +- multi lidar pointcloud based occupancy grid map + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/multi_lidar_pointcloud_based_occupancy_grid_map.schema.json") }} + +- pointcloud based occupancy grid map + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/pointcloud_based_occupancy_grid_map.schema.json") }} + +- synchronized grid map fusion + + {{ json_to_markdown("perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json") }} ### Downsample input pointcloud(Optional) diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp index 93e768c0f6b4e..5fe3613cc7612 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map.hpp @@ -52,7 +52,8 @@ #ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_HPP_ -#include +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" + #include #include @@ -66,7 +67,7 @@ namespace costmap_2d using geometry_msgs::msg::Pose; using sensor_msgs::msg::PointCloud2; -class OccupancyGridMap : public nav2_costmap_2d::Costmap2D +class OccupancyGridMap : public OccupancyGridMapInterface { public: OccupancyGridMap( @@ -78,8 +79,6 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D void updateOccupiedCells(const PointCloud2 & pointcloud); - void updateOrigin(double new_origin_x, double new_origin_y) override; - private: void raytraceFreespace(const PointCloud2 & pointcloud, const Pose & robot_pose); @@ -87,6 +86,8 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; + void initRosParam([[maybe_unused]] rclcpp::Node & node) override {} + rclcpp::Logger logger_{rclcpp::get_logger("laserscan_based_occupancy_grid_map")}; rclcpp::Clock clock_{RCL_ROS_TIME}; }; diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp index 09ad0a4d554a1..c77345a973c50 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp @@ -52,6 +52,9 @@ #ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_BASE_HPP_ +#include "autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp" + +#include #include #include #include @@ -72,18 +75,17 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D { public: OccupancyGridMapInterface( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution); + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution); virtual void updateWithPointCloud( - const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, - const Pose & robot_pose, const Pose & scan_origin) = 0; + [[maybe_unused]] const CudaPointCloud2 & raw_pointcloud, + [[maybe_unused]] const CudaPointCloud2 & obstacle_pointcloud, + [[maybe_unused]] const Pose & robot_pose, [[maybe_unused]] const Pose & scan_origin) {}; void updateOrigin(double new_origin_x, double new_origin_y) override; - void raytrace( - const double source_x, const double source_y, const double target_x, const double target_y, - const unsigned char cost); - void setCellValue(const double wx, const double wy, const unsigned char cost); - using nav2_costmap_2d::Costmap2D::resetMaps; + + void resetMaps() override; virtual void initRosParam(rclcpp::Node & node) = 0; @@ -92,47 +94,34 @@ class OccupancyGridMapInterface : public nav2_costmap_2d::Costmap2D double min_height_; double max_height_; - void setFieldOffsets(const PointCloud2 & input_raw, const PointCloud2 & input_obstacle); - - int x_offset_raw_; - int y_offset_raw_; - int z_offset_raw_; - int x_offset_obstacle_; - int y_offset_obstacle_; - int z_offset_obstacle_; - bool offset_initialized_; - const double min_angle_ = autoware::universe_utils::deg2rad(-180.0); const double max_angle_ = autoware::universe_utils::deg2rad(180.0); const double angle_increment_inv_ = 1.0 / autoware::universe_utils::deg2rad(0.1); Eigen::Matrix4f mat_map_, mat_scan_; - bool isPointValid(const Eigen::Vector4f & pt) const - { - // Apply height filter and exclude invalid points - return min_height_ < pt[2] && pt[2] < max_height_ && std::isfinite(pt[0]) && - std::isfinite(pt[1]) && std::isfinite(pt[2]); - } - // Transform pt to (pt_map, pt_scan), then calculate angle_bin_index and range - void transformPointAndCalculate( - const Eigen::Vector4f & pt, Eigen::Vector4f & pt_map, int & angle_bin_index, - double & range) const - { - pt_map = mat_map_ * pt; - Eigen::Vector4f pt_scan(mat_scan_ * pt_map); - const double angle = atan2(pt_scan[1], pt_scan[0]); - angle_bin_index = (angle - min_angle_) * angle_increment_inv_; - range = std::sqrt(pt_scan[1] * pt_scan[1] + pt_scan[0] * pt_scan[0]); - } - -private: - bool worldToMap(double wx, double wy, unsigned int & mx, unsigned int & my) const; + bool isCudaEnabled() const; + + const autoware::cuda_utils::CudaUniquePtr & getDeviceCostmap() const; + void copyDeviceCostmapToHost() const; + +protected: rclcpp::Logger logger_{rclcpp::get_logger("pointcloud_based_occupancy_grid_map")}; rclcpp::Clock clock_{RCL_ROS_TIME}; double resolution_inv_; + + cudaStream_t stream_; + + bool use_cuda_; + autoware::cuda_utils::CudaUniquePtr device_costmap_; + autoware::cuda_utils::CudaUniquePtr device_costmap_aux_; + + autoware::cuda_utils::CudaUniquePtr device_rotation_map_; + autoware::cuda_utils::CudaUniquePtr device_translation_map_; + autoware::cuda_utils::CudaUniquePtr device_rotation_scan_; + autoware::cuda_utils::CudaUniquePtr device_translation_scan_; }; } // namespace costmap_2d diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp index 3301fd1987bd3..320d297e0a4d1 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_HPP_ #include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp" namespace autoware::occupancy_grid_map { @@ -28,21 +29,20 @@ class OccupancyGridMapFixedBlindSpot : public OccupancyGridMapInterface { public: OccupancyGridMapFixedBlindSpot( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution); + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution); void updateWithPointCloud( - const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, + const CudaPointCloud2 & raw_pointcloud, const CudaPointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) override; - using OccupancyGridMapInterface::raytrace; - using OccupancyGridMapInterface::setCellValue; - using OccupancyGridMapInterface::setFieldOffsets; - using OccupancyGridMapInterface::updateOrigin; - void initRosParam(rclcpp::Node & node) override; -private: +protected: double distance_margin_; + + autoware::cuda_utils::CudaUniquePtr raw_points_tensor_; + autoware::cuda_utils::CudaUniquePtr obstacle_points_tensor_; }; } // namespace costmap_2d diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed_kernel.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed_kernel.hpp new file mode 100644 index 0000000000000..d87c8eec718e1 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed_kernel.hpp @@ -0,0 +1,60 @@ +// 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__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_KERNEL_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_KERNEL_HPP_ + +#include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" + +#include + +#include + +namespace autoware::occupancy_grid_map +{ +namespace costmap_2d::map_fixed +{ + +void prepareTensorLaunch( + const float * input_pointcloud, const std::size_t num_points, const std::size_t points_step, + const std::size_t angle_bins, const std::size_t range_bins, const float min_height, + const float max_height, const float min_angle, const float angle_increment_inv, + const float range_resolution_inv, const Eigen::Matrix3f * rotation_map, + const Eigen::Vector3f * translation_map, const Eigen::Matrix3f * rotation_scan, + const Eigen::Vector3f * translation_scan, std::uint64_t * points_tensor, cudaStream_t stream); + +void fillEmptySpaceLaunch( + const std::uint64_t * points_tensor, const std::size_t angle_bins, const std::size_t range_bins, + const float map_resolution_inv, const float scan_origin_x, const float scan_origin_y, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t empty_value, std::uint8_t * costmap_tensor, cudaStream_t stream); + +void fillUnknownSpaceLaunch( + const std::uint64_t * raw_points_tensor, const std::uint64_t * obstacle_points_tensor, + const float distance_margin, const std::size_t angle_bins, const std::size_t range_bins, + const float map_resolution_inv, const float scan_origin_x, const float scan_origin_y, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t free_space_value, std::uint8_t no_information_value, std::uint8_t * costmap_tensor, + cudaStream_t stream); + +void fillObstaclesLaunch( + const std::uint64_t * points_tensor, const float distance_margin, const std::size_t angle_bins, + const std::size_t range_bins, const float map_resolution_inv, const float map_origin_x, + const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t obstacle_value, std::uint8_t * costmap_tensor, cudaStream_t stream); + +} // namespace costmap_2d::map_fixed +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_FIXED_KERNEL_HPP_ diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp index 7569a60b36466..9b23b9ecf8cb6 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp @@ -32,25 +32,22 @@ class OccupancyGridMapProjectiveBlindSpot : public OccupancyGridMapInterface { public: OccupancyGridMapProjectiveBlindSpot( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution); + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution); void updateWithPointCloud( - const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, + const CudaPointCloud2 & raw_pointcloud, const CudaPointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) override; - using OccupancyGridMapInterface::raytrace; - using OccupancyGridMapInterface::setCellValue; - using OccupancyGridMapInterface::setFieldOffsets; - using OccupancyGridMapInterface::updateOrigin; - void initRosParam(rclcpp::Node & node) override; private: - double projection_dz_threshold_; - double obstacle_separation_threshold_; - bool pub_debug_grid_; - grid_map::GridMap debug_grid_; - rclcpp::Publisher::SharedPtr debug_grid_map_publisher_ptr_; + float projection_dz_threshold_; + float obstacle_separation_threshold_; + + autoware::cuda_utils::CudaUniquePtr raw_points_tensor_; + autoware::cuda_utils::CudaUniquePtr obstacle_points_tensor_; + autoware::cuda_utils::CudaUniquePtr device_translation_scan_origin_; }; } // namespace costmap_2d diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective_kernel.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective_kernel.hpp new file mode 100644 index 0000000000000..916cf6ed9ec0a --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective_kernel.hpp @@ -0,0 +1,70 @@ +// 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__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_KERNEL_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_KERNEL_HPP_ + +#include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" + +#include + +#include + +namespace autoware::occupancy_grid_map +{ +namespace costmap_2d::map_projective +{ + +void prepareRawTensorLaunch( + const float * input_pointcloud, const std::size_t num_points, const std::size_t points_step, + const std::size_t angle_bins, const std::size_t range_bins, const float min_height, + const float max_height, const float min_angle, const float angle_increment_inv, + const float range_resolution_inv, const Eigen::Matrix3f * rotation_map, + const Eigen::Vector3f * translation_map, const Eigen::Matrix3f * rotation_scan, + const Eigen::Vector3f * translation_scan, std::uint64_t * points_tensor, cudaStream_t stream); + +void prepareObstacleTensorLaunch( + const float * input_pointcloud, const std::size_t num_points, const std::size_t points_step, + const std::size_t angle_bins, const std::size_t range_bins, const float min_height, + const float max_height, const float min_angle, const float angle_increment_inv, + const float range_resolution_inv, const float projection_dz_threshold, + const Eigen::Vector3f * translation_scan_origin, const Eigen::Matrix3f * rotation_map, + const Eigen::Vector3f * translation_map, const Eigen::Matrix3f * rotation_scan, + const Eigen::Vector3f * translation_scan, std::uint64_t * points_tensor, cudaStream_t stream); + +void fillEmptySpaceLaunch( + const std::uint64_t * points_tensor, const std::size_t angle_bins, const std::size_t range_bins, + const float map_resolution_inv, const float scan_origin_x, const float scan_origin_y, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t empty_value, std::uint8_t * costmap_tensor, cudaStream_t stream); + +void fillUnknownSpaceLaunch( + const std::uint64_t * raw_points_tensor, const std::uint64_t * obstacle_points_tensor, + const float obstacle_separation_threshold, const std::size_t angle_bins, + const std::size_t range_bins, const float map_resolution_inv, const float scan_origin_x, + const float scan_origin_y, const float scan_origin_z, const float map_origin_x, + const float map_origin_y, const float robot_pose_z, const int num_cells_x, const int num_cells_y, + std::uint8_t free_space_value, std::uint8_t no_information_value, std::uint8_t * costmap_tensor, + cudaStream_t stream); + +void fillObstaclesLaunch( + const std::uint64_t * points_tensor, const float distance_margin, const std::size_t angle_bins, + const std::size_t range_bins, const float map_resolution_inv, const float map_origin_x, + const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t obstacle_value, std::uint8_t * costmap_tensor, cudaStream_t stream); + +} // namespace costmap_2d::map_projective +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__COSTMAP_2D__OCCUPANCY_GRID_MAP_PROJECTIVE_KERNEL_HPP_ diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp index af28b7962b3bc..fe5739f2cadc7 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_HPP_ +#include "autoware/cuda_utils/cuda_unique_ptr.hpp" #include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" #include @@ -27,16 +28,19 @@ namespace costmap_2d class OccupancyGridMapBBFUpdater : public OccupancyGridMapUpdaterInterface { public: - enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; + enum Index : size_t { OCCUPIED = 0U, FREE = 1U, NUM_STATES = 2U }; OccupancyGridMapBBFUpdater( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution); - bool update(const Costmap2D & single_frame_occupancy_grid_map) override; + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution); + bool update(const OccupancyGridMapInterface & single_frame_occupancy_grid_map) override; void initRosParam(rclcpp::Node & node) override; private: inline unsigned char applyBBF(const unsigned char & z, const unsigned char & o); Eigen::Matrix2f probability_matrix_; double v_ratio_; + + autoware::cuda_utils::CudaUniquePtr device_probability_matrix_; }; } // namespace costmap_2d diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater_kernel.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater_kernel.hpp new file mode 100644 index 0000000000000..feef6f2815a0f --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater_kernel.hpp @@ -0,0 +1,36 @@ +// 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__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_KERNEL_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_KERNEL_HPP_ + +#include + +#include + +namespace autoware::occupancy_grid_map +{ +namespace costmap_2d +{ + +void applyBBFLaunch( + const std::uint8_t * z_costmap, const float * probability_matrix, const int num_states, + const int free_index, const int occupied_index, const std::uint8_t free_space_value, + const std::uint8_t lethal_obstacle_value, const std::uint8_t no_information_value, + const double v_ratio_, const int num_elements, std::uint8_t * o_costmap, cudaStream_t stream); + +} // namespace costmap_2d +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__BINARY_BAYES_FILTER_UPDATER_KERNEL_HPP_ diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp index d7bb1184c06d6..d24fc7c46331d 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp @@ -32,18 +32,18 @@ namespace costmap_2d class OccupancyGridMapLOBFUpdater : public OccupancyGridMapUpdaterInterface { public: - enum Index : size_t { OCCUPIED = 0U, FREE = 1U }; + enum Index : size_t { OCCUPIED = 0U, FREE = 1U, NUM_STATES = 2U }; OccupancyGridMapLOBFUpdater( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) - : OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution) + : OccupancyGridMapUpdaterInterface(use_cuda, cells_size_x, cells_size_y, resolution) { } - bool update(const Costmap2D & single_frame_occupancy_grid_map) override; + bool update(const OccupancyGridMapInterface & single_frame_occupancy_grid_map) override; void initRosParam(rclcpp::Node & node) override; private: inline unsigned char applyLOBF(const unsigned char & z, const unsigned char & o); - Eigen::Matrix2f probability_matrix_; }; } // namespace costmap_2d diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater_kernel.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater_kernel.hpp new file mode 100644 index 0000000000000..c8ff015ca27a2 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater_kernel.hpp @@ -0,0 +1,35 @@ +// 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__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_KERNEL_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_KERNEL_HPP_ + +#include + +#include + +namespace autoware::occupancy_grid_map +{ +namespace costmap_2d +{ + +// cspell: ignore LOBF +void applyLOBFLaunch( + const std::uint8_t * z_costmap, const std::uint8_t no_information_value, const int num_elements, + std::uint8_t * o_costmap, cudaStream_t stream); + +} // namespace costmap_2d +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__LOG_ODDS_BAYES_FILTER_UPDATER_KERNEL_HPP_ diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp index 75231089ac33c..ae9f824bb9beb 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp @@ -16,24 +16,26 @@ #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UPDATER__OGM_UPDATER_INTERFACE_HPP_ #include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" -#include #include namespace autoware::occupancy_grid_map { namespace costmap_2d { -class OccupancyGridMapUpdaterInterface : public nav2_costmap_2d::Costmap2D +class OccupancyGridMapUpdaterInterface : public OccupancyGridMapInterface { public: OccupancyGridMapUpdaterInterface( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) - : Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) + bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution) + : OccupancyGridMapInterface(use_cuda, cells_size_x, cells_size_y, resolution) { } virtual ~OccupancyGridMapUpdaterInterface() = default; - virtual bool update(const Costmap2D & single_frame_occupancy_grid_map) = 0; + virtual bool update(const OccupancyGridMapInterface & single_frame_occupancy_grid_map) = 0; virtual void initRosParam(rclcpp::Node & node) = 0; }; diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp new file mode 100644 index 0000000000000..8c78e2ad915b5 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp @@ -0,0 +1,55 @@ +// 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__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__CUDA_POINTCLOUD_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__CUDA_POINTCLOUD_HPP_ + +#include + +#include + +#include + +class CudaPointCloud2 : public sensor_msgs::msg::PointCloud2 +{ +public: + void fromROSMsgAsync(const sensor_msgs::msg::PointCloud2 & msg) + { + // cSpell: ignore knzo25 + // NOTE(knzo25): replace this with the cuda blackboard later + header = msg.header; + fields = msg.fields; + height = msg.height; + width = msg.width; + is_bigendian = msg.is_bigendian; + point_step = msg.point_step; + row_step = msg.row_step; + is_dense = msg.is_dense; + + if (!data || capacity_ < msg.data.size()) { + capacity_ = 1024 * (msg.data.size() / 1024 + 1); + data = autoware::cuda_utils::make_unique(capacity_); + } + + cudaMemcpyAsync(data.get(), msg.data.data(), msg.data.size(), cudaMemcpyHostToDevice, stream); + } + + autoware::cuda_utils::CudaUniquePtr data; + cudaStream_t stream; + +private: + std::size_t capacity_{0}; +}; + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__CUDA_POINTCLOUD_HPP_ diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp index 41c7294ded849..9a9a441af3d60 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils.hpp @@ -16,6 +16,8 @@ #define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_HPP_ #include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" #include #include @@ -50,52 +52,12 @@ namespace autoware::occupancy_grid_map namespace utils { -/** - * @brief 3D point struct for sorting and searching - * - */ -struct MyPoint3d -{ - float x; - float y; - float z; - - // constructor - MyPoint3d(float x, float y, float z) : x(x), y(y), z(z) {} - - // calc squared norm - float norm2() const { return powf(x, 2) + powf(y, 2) + powf(z, 2); } - - // calc arctan2 - float arctan2() const { return atan2f(y, x); } - - // overload operator< - bool operator<(const MyPoint3d & other) const - { - const auto a = norm2(); - const auto b = other.norm2(); - if (a == b) { // escape when norm2 is same - return arctan2() < other.arctan2(); - } else { - return a < b; - } - } - - // overload operator== - bool operator==(const MyPoint3d & other) const - { - return fabsf(x - other.x) < FLT_EPSILON && fabsf(y - other.y) < FLT_EPSILON && - fabsf(z - other.z) < FLT_EPSILON; - } -}; - bool transformPointcloud( const sensor_msgs::msg::PointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame, sensor_msgs::msg::PointCloud2 & output); -void transformPointcloud( - const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose, - sensor_msgs::msg::PointCloud2 & output); +bool transformPointcloudAsync( + CudaPointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame); Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose); @@ -116,10 +78,6 @@ geometry_msgs::msg::Pose getPose( // get inverted pose geometry_msgs::msg::Pose getInversePose(const geometry_msgs::msg::Pose & pose); -bool extractCommonPointCloud( - const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, - sensor_msgs::msg::PointCloud2 & output_obstacle_pc); - } // namespace utils } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp new file mode 100644 index 0000000000000..a335a4a6e5b93 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/include/autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp @@ -0,0 +1,52 @@ +// 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__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_KERNEL_HPP_ +#define AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_KERNEL_HPP_ + +#include + +#include + +#include + +namespace autoware::occupancy_grid_map +{ +namespace utils +{ + +void __device__ setCellValue( + float wx, float wy, float origin_x, float origin_y, float resolution_inv, int size_x, int size_y, + std::uint8_t value, std::uint8_t * costmap_tensor); + +void __device__ raytrace( + const float source_x, const float source_y, const float target_x, const float target_y, + const float origin_x, float origin_y, const float resolution_inv, const int size_x, + const int size_y, const std::uint8_t cost, std::uint8_t * costmap_tensor); + +void copyMapRegionLaunch( + const std::uint8_t * source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, + unsigned int sm_size_x, unsigned int sm_size_y, std::uint8_t * dest_map, + unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, + unsigned int dm_size_y, unsigned int region_size_x, unsigned int region_size_y, + cudaStream_t stream); + +void transformPointCloudLaunch( + std::uint8_t * points, std::size_t num_points, std::size_t points_step, + const Eigen::Matrix3f & rotation, const Eigen::Vector3f & translation, cudaStream_t stream); + +} // namespace utils +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__PROBABILISTIC_OCCUPANCY_GRID_MAP__UTILS__UTILS_KERNEL_HPP_ diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp index 25eafcd564e2d..082985ca824af 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map.cpp @@ -65,7 +65,7 @@ using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMap::OccupancyGridMap( const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) +: OccupancyGridMapInterface(false, cells_size_x, cells_size_y, resolution) { } @@ -85,57 +85,6 @@ bool OccupancyGridMap::worldToMap(double wx, double wy, unsigned int & mx, unsig return false; } -void OccupancyGridMap::updateOrigin(double new_origin_x, double new_origin_y) -{ - // project the new origin into the grid - int cell_ox{static_cast(std::floor((new_origin_x - origin_x_) / resolution_))}; - int cell_oy{static_cast(std::floor((new_origin_y - origin_y_) / resolution_))}; - - // compute the associated world coordinates for the origin cell - // because we want to keep things grid-aligned - double new_grid_ox{origin_x_ + cell_ox * resolution_}; - double new_grid_oy{origin_y_ + cell_oy * resolution_}; - - // To save casting from unsigned int to int a bunch of times - int size_x{static_cast(size_x_)}; - int size_y{static_cast(size_y_)}; - - // we need to compute the overlap of the new and existing windows - int lower_left_x{std::min(std::max(cell_ox, 0), size_x)}; - int lower_left_y{std::min(std::max(cell_oy, 0), size_y)}; - int upper_right_x{std::min(std::max(cell_ox + size_x, 0), size_x)}; - int upper_right_y{std::min(std::max(cell_oy + size_y, 0), size_y)}; - - unsigned int cell_size_x = upper_right_x - lower_left_x; - unsigned int cell_size_y = upper_right_y - lower_left_y; - - // we need a map to store the obstacles in the window temporarily - unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y]; - - // copy the local window in the costmap to the local map - copyMapRegion( - costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, - cell_size_y); - - // now we'll set the costmap to be completely unknown if we track unknown space - resetMaps(); - - // update the origin with the appropriate world coordinates - origin_x_ = new_grid_ox; - origin_y_ = new_grid_oy; - - // compute the starting cell location for copying data back in - int start_x{lower_left_x - cell_ox}; - int start_y{lower_left_y - cell_oy}; - - // now we want to copy the overlapping information back into the map, but in its new location - copyMapRegion( - local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); - - // make sure to clean up - delete[] local_map; -} - void OccupancyGridMap::raytrace2D(const PointCloud2 & pointcloud, const Pose & robot_pose) { // freespace diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp index 39855ec36260c..f4d290bd70d7c 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_base.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -77,34 +77,35 @@ namespace costmap_2d using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMapInterface::OccupancyGridMapInterface( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION) + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution) +: Costmap2D(cells_size_x, cells_size_y, resolution, 0.f, 0.f, cost_value::NO_INFORMATION), + use_cuda_(use_cuda) { - min_height_ = -std::numeric_limits::infinity(); - max_height_ = std::numeric_limits::infinity(); - resolution_inv_ = 1.0 / resolution_; - offset_initialized_ = false; -} - -inline bool OccupancyGridMapInterface::worldToMap( - double wx, double wy, unsigned int & mx, unsigned int & my) const -{ - if (wx < origin_x_ || wy < origin_y_) { - return false; + if (use_cuda_) { + min_height_ = -std::numeric_limits::infinity(); + max_height_ = std::numeric_limits::infinity(); + resolution_inv_ = 1.0 / resolution_; + + const auto num_cells_x = this->getSizeInCellsX(); + const auto num_cells_y = this->getSizeInCellsY(); + + cudaStreamCreate(&stream_); + device_costmap_ = autoware::cuda_utils::make_unique(num_cells_x * num_cells_y); + device_costmap_aux_ = + autoware::cuda_utils::make_unique(num_cells_x * num_cells_y); + + device_rotation_map_ = autoware::cuda_utils::make_unique(); + device_translation_map_ = autoware::cuda_utils::make_unique(); + device_rotation_scan_ = autoware::cuda_utils::make_unique(); + device_translation_scan_ = autoware::cuda_utils::make_unique(); } - - mx = static_cast(std::floor((wx - origin_x_) * resolution_inv_)); - my = static_cast(std::floor((wy - origin_y_) * resolution_inv_)); - - if (mx < size_x_ && my < size_y_) { - return true; - } - - return false; } void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_origin_y) { + using autoware::occupancy_grid_map::utils::copyMapRegionLaunch; + // project the new origin into the grid int cell_ox{static_cast(std::floor((new_origin_x - origin_x_) / resolution_))}; int cell_oy{static_cast(std::floor((new_origin_y - origin_y_) / resolution_))}; @@ -128,15 +129,26 @@ void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_ori unsigned int cell_size_y = upper_right_y - lower_left_y; // we need a map to store the obstacles in the window temporarily - unsigned char * local_map = new unsigned char[cell_size_x * cell_size_y]; + unsigned char * local_map{nullptr}; + + if (use_cuda_) { + copyMapRegionLaunch( + device_costmap_.get(), lower_left_x, lower_left_y, size_x_, size_y_, + device_costmap_aux_.get(), 0, 0, cell_size_x, cell_size_y, cell_size_x, cell_size_y, stream_); - // copy the local window in the costmap to the local map - copyMapRegion( - costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, - cell_size_y); + cudaMemset( + device_costmap_.get(), cost_value::NO_INFORMATION, size_x_ * size_y_ * sizeof(std::uint8_t)); + } else { + local_map = new unsigned char[cell_size_x * cell_size_y]; - // now we'll set the costmap to be completely unknown if we track unknown space - resetMaps(); + // copy the local window in the costmap to the local map + copyMapRegion( + costmap_, lower_left_x, lower_left_y, size_x_, local_map, 0, 0, cell_size_x, cell_size_x, + cell_size_y); + + // now we'll set the costmap to be completely unknown if we track unknown space + nav2_costmap_2d::Costmap2D::resetMaps(); + } // update the origin with the appropriate world coordinates origin_x_ = new_grid_ox; @@ -147,93 +159,41 @@ void OccupancyGridMapInterface::updateOrigin(double new_origin_x, double new_ori int start_y{lower_left_y - cell_oy}; // now we want to copy the overlapping information back into the map, but in its new location - copyMapRegion( - local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); - - // make sure to clean up - delete[] local_map; -} - -void OccupancyGridMapInterface::setCellValue( - const double wx, const double wy, const unsigned char cost) -{ - MarkCell marker(costmap_, cost); - unsigned int mx{}; - unsigned int my{}; - if (!worldToMap(wx, wy, mx, my)) { - RCLCPP_DEBUG(logger_, "Computing map coords failed"); - return; + if (use_cuda_) { + if ( + start_x < 0 || start_y < 0 || start_x + cell_size_x > size_x_ || + start_y + cell_size_y > size_y_) { + RCLCPP_ERROR( + rclcpp::get_logger("pointcloud_based_occupancy_grid_map"), + "update coordinates are negative or out of bounds: start.x=%d, start.y=%d, cell_size.x=%d, " + "cell_size.y=%d size_x:%d, size_y=%d", + start_x, start_y, cell_size_x, cell_size_y, size_x_, size_y_); + return; + } + + copyMapRegionLaunch( + device_costmap_aux_.get(), 0, 0, cell_size_x, cell_size_y, device_costmap_.get(), start_x, + start_y, size_x_, size_y_, cell_size_x, cell_size_y, stream_); + } else { + copyMapRegion( + local_map, 0, 0, cell_size_x, costmap_, start_x, start_y, size_x_, cell_size_x, cell_size_y); + + // make sure to clean up + if (local_map != nullptr) { + delete[] local_map; + } } - const unsigned int index = getIndex(mx, my); - marker(index); } -void OccupancyGridMapInterface::raytrace( - const double source_x, const double source_y, const double target_x, const double target_y, - const unsigned char cost) +void OccupancyGridMapInterface::resetMaps() { - unsigned int x0{}; - unsigned int y0{}; - const double ox{source_x}; - const double oy{source_y}; - if (!worldToMap(ox, oy, x0, y0)) { - RCLCPP_DEBUG( - logger_, - "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot " - "raytrace for it.", - ox, oy); - return; - } - - // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later - const double origin_x = origin_x_, origin_y = origin_y_; - const double map_end_x = origin_x + size_x_ * resolution_; - const double map_end_y = origin_y + size_y_ * resolution_; - - double wx = target_x; - double wy = target_y; - - // now we also need to make sure that the endpoint we're ray-tracing - // to isn't off the costmap and scale if necessary - const double a = wx - ox; - const double b = wy - oy; - - // the minimum value to raytrace from is the origin - if (wx < origin_x) { - const double t = (origin_x - ox) / a; - wx = origin_x; - wy = oy + b * t; + if (use_cuda_) { + cudaMemsetAsync( + device_costmap_.get(), cost_value::NO_INFORMATION, getSizeInCellsX() * getSizeInCellsY(), + stream_); + } else { + nav2_costmap_2d::Costmap2D::resetMaps(); } - if (wy < origin_y) { - const double t = (origin_y - oy) / b; - wx = ox + a * t; - wy = origin_y; - } - - // the maximum value to raytrace to is the end of the map - if (wx > map_end_x) { - const double t = (map_end_x - ox) / a; - wx = map_end_x - .001; - wy = oy + b * t; - } - if (wy > map_end_y) { - const double t = (map_end_y - oy) / b; - wx = ox + a * t; - wy = map_end_y - .001; - } - - // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint - unsigned int x1{}; - unsigned int y1{}; - - // check for legality just in case - if (!worldToMap(wx, wy, x1, y1)) { - return; - } - - constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold - MarkCell marker(costmap_, cost); - raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); } void OccupancyGridMapInterface::setHeightLimit(const double min_height, const double max_height) @@ -242,16 +202,22 @@ void OccupancyGridMapInterface::setHeightLimit(const double min_height, const do max_height_ = max_height; } -void OccupancyGridMapInterface::setFieldOffsets( - const PointCloud2 & input_raw, const PointCloud2 & input_obstacle) +bool OccupancyGridMapInterface::isCudaEnabled() const +{ + return use_cuda_; +} + +const autoware::cuda_utils::CudaUniquePtr & +OccupancyGridMapInterface::getDeviceCostmap() const +{ + return device_costmap_; +} + +void OccupancyGridMapInterface::copyDeviceCostmapToHost() const { - x_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "x")].offset; - y_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "y")].offset; - z_offset_raw_ = input_raw.fields[pcl::getFieldIndex(input_raw, "z")].offset; - x_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "x")].offset; - y_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "y")].offset; - z_offset_obstacle_ = input_obstacle.fields[pcl::getFieldIndex(input_obstacle, "z")].offset; - offset_initialized_ = true; + cudaMemcpy( + costmap_, device_costmap_.get(), getSizeInCellsX() * getSizeInCellsY() * sizeof(std::uint8_t), + cudaMemcpyDeviceToHost); } } // namespace costmap_2d diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp index 518052a1a4b8c..97f36f08c48bd 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -15,8 +15,11 @@ #include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed.hpp" #include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed_kernel.hpp" #include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" +#include #include #include #include @@ -41,9 +44,24 @@ namespace costmap_2d using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMapFixedBlindSpot::OccupancyGridMapFixedBlindSpot( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: OccupancyGridMapInterface(cells_size_x, cells_size_y, resolution) + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution) +: OccupancyGridMapInterface(use_cuda, cells_size_x, cells_size_y, resolution) { + if (use_cuda_) { + const size_t angle_bin_size = + ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/); + + const auto num_cells_x = this->getSizeInCellsX(); + const auto num_cells_y = this->getSizeInCellsY(); + const std::size_t range_bin_size = + static_cast(std::sqrt(2) * std::max(num_cells_x, num_cells_y) / 2.0) + 1; + + raw_points_tensor_ = + autoware::cuda_utils::make_unique(2 * angle_bin_size * range_bin_size); + obstacle_points_tensor_ = + autoware::cuda_utils::make_unique(2 * angle_bin_size * range_bin_size); + } } /** @@ -55,7 +73,7 @@ OccupancyGridMapFixedBlindSpot::OccupancyGridMapFixedBlindSpot( * @param scan_origin manually chosen grid map origin frame */ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( - const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, + const CudaPointCloud2 & raw_pointcloud, const CudaPointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { const size_t angle_bin_size = @@ -69,189 +87,81 @@ void OccupancyGridMapFixedBlindSpot::updateWithPointCloud( // Transform Matrix from map frame to scan frame mat_scan_ = utils::getTransformMatrix(scan2map_pose); - if (!offset_initialized_) { - setFieldOffsets(raw_pointcloud, obstacle_pointcloud); - } - - // Create angle bins and sort by distance - struct BinInfo - { - BinInfo() = default; - BinInfo(const double _range, const double _wx, const double _wy) - : range(_range), wx(_wx), wy(_wy) - { - } - double range{}; - double wx{}; - double wy{}; - }; - - std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); - std::vector> raw_pointcloud_angle_bins(angle_bin_size); - - const size_t raw_pointcloud_size = raw_pointcloud.width * raw_pointcloud.height; - const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height; - - // Reserve a certain amount of memory in advance for performance reasons - const size_t raw_reserve_size = raw_pointcloud_size / angle_bin_size; - const size_t obstacle_reserve_size = obstacle_pointcloud_size / angle_bin_size; - for (size_t i = 0; i < angle_bin_size; i++) { - raw_pointcloud_angle_bins[i].reserve(raw_reserve_size); - obstacle_pointcloud_angle_bins[i].reserve(obstacle_reserve_size); - } - - // Updated every loop inside transformPointAndCalculate() - Eigen::Vector4f pt_map; - int angle_bin_index; - double range; - - size_t global_offset = 0; - for (size_t i = 0; i < raw_pointcloud_size; ++i) { - Eigen::Vector4f pt( - *reinterpret_cast(&raw_pointcloud.data[global_offset + x_offset_raw_]), - *reinterpret_cast(&raw_pointcloud.data[global_offset + y_offset_raw_]), - *reinterpret_cast(&raw_pointcloud.data[global_offset + z_offset_raw_]), 1); - global_offset += raw_pointcloud.point_step; - if (!isPointValid(pt)) { - continue; - } - transformPointAndCalculate(pt, pt_map, angle_bin_index, range); - - raw_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]); - } - - for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { - std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) { - return a.range < b.range; - }); - } - - // Create obstacle angle bins and sort points by range - global_offset = 0; - for (size_t i = 0; i < obstacle_pointcloud_size; ++i) { - Eigen::Vector4f pt( - *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]), - *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]), - *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]), - 1); - global_offset += obstacle_pointcloud.point_step; - if (!isPointValid(pt)) { - continue; - } - transformPointAndCalculate(pt, pt_map, angle_bin_index, range); - - // Ignore obstacle points exceed the range of the raw points - if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) { - continue; // No raw point in this angle bin - } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { - continue; // Obstacle point exceeds the range of the raw points - } - obstacle_pointcloud_angle_bins.at(angle_bin_index).emplace_back(range, pt_map[0], pt_map[1]); - } - - for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { - std::sort( - obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(), - [](auto a, auto b) { return a.range < b.range; }); - } - - // First step: Initialize cells to the final point with freespace - for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - const auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); - - BinInfo end_distance; - if (raw_pointcloud_angle_bin.empty()) { - continue; - } else { - end_distance = raw_pointcloud_angle_bin.back(); - } - raytrace( - scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy, - cost_value::FREE_SPACE); - } - - // Second step: Add unknown cell - for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - const auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); - const auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); - auto raw_distance_iter = raw_pointcloud_angle_bin.begin(); - for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { - // Calculate next raw point from obstacle point - while (raw_distance_iter != raw_pointcloud_angle_bin.end()) { - if ( - raw_distance_iter->range < - obstacle_pointcloud_angle_bin.at(dist_index).range + distance_margin_) - raw_distance_iter++; - else - break; - } - - // There is no point far than the obstacle point. - const bool no_freespace_point = (raw_distance_iter == raw_pointcloud_angle_bin.end()); - - if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - if (!no_freespace_point) { - const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); - } - continue; - } - - auto next_obstacle_point_distance = std::abs( - obstacle_pointcloud_angle_bin.at(dist_index + 1).range - - obstacle_pointcloud_angle_bin.at(dist_index).range); - if (next_obstacle_point_distance <= distance_margin_) { - continue; - } else if (no_freespace_point) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); - continue; - } - - auto next_raw_distance = - std::abs(obstacle_pointcloud_angle_bin.at(dist_index).range - raw_distance_iter->range); - if (next_raw_distance < next_obstacle_point_distance) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); - continue; - } else { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); - continue; - } - } - } - - // Third step: Overwrite occupied cell - for (const auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { - for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { - const auto & obstacle_point = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(obstacle_point.wx, obstacle_point.wy, cost_value::LETHAL_OBSTACLE); - - if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { - continue; - } - - auto next_obstacle_point_distance = std::abs( - obstacle_pointcloud_angle_bin.at(dist_index + 1).range - - obstacle_pointcloud_angle_bin.at(dist_index).range); - if (next_obstacle_point_distance <= distance_margin_) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); - continue; - } - } - } + const auto map_res = this->getResolution(); + const auto num_cells_x = this->getSizeInCellsX(); + const auto num_cells_y = this->getSizeInCellsY(); + const std::size_t range_bin_size = + static_cast(std::sqrt(2) * std::max(num_cells_x, num_cells_y) / 2.0) + 1; + + cudaMemsetAsync( + raw_points_tensor_.get(), 0xFF, 2 * angle_bin_size * range_bin_size * sizeof(std::int64_t), + stream_); + cudaMemsetAsync( + obstacle_points_tensor_.get(), 0xFF, 2 * angle_bin_size * range_bin_size * sizeof(std::int64_t), + stream_); + cudaMemsetAsync( + device_costmap_.get(), cost_value::NO_INFORMATION, + num_cells_x * num_cells_y * sizeof(std::uint8_t), stream_); + + Eigen::Matrix3f rotation_map = mat_map_.block<3, 3>(0, 0); + Eigen::Vector3f translation_map = mat_map_.block<3, 1>(0, 3); + + Eigen::Matrix3f rotation_scan = mat_scan_.block<3, 3>(0, 0); + Eigen::Vector3f translation_scan = mat_scan_.block<3, 1>(0, 3); + + cudaMemcpyAsync( + device_rotation_map_.get(), &rotation_map, sizeof(Eigen::Matrix3f), cudaMemcpyHostToDevice, + stream_); + cudaMemcpyAsync( + device_translation_map_.get(), &translation_map, sizeof(Eigen::Vector3f), + cudaMemcpyHostToDevice, stream_); + cudaMemcpyAsync( + device_rotation_scan_.get(), &rotation_scan, sizeof(Eigen::Matrix3f), cudaMemcpyHostToDevice, + stream_); + cudaMemcpyAsync( + device_translation_scan_.get(), &translation_scan, sizeof(Eigen::Vector3f), + cudaMemcpyHostToDevice, stream_); + + const std::size_t num_raw_points = raw_pointcloud.width * raw_pointcloud.height; + float range_resolution_inv = 1.0 / map_res; + + cudaStreamSynchronize(stream_); + + map_fixed::prepareTensorLaunch( + reinterpret_cast(raw_pointcloud.data.get()), num_raw_points, + raw_pointcloud.point_step / sizeof(float), angle_bin_size, range_bin_size, min_height_, + max_height_, min_angle_, angle_increment_inv_, range_resolution_inv, device_rotation_map_.get(), + device_translation_map_.get(), device_rotation_scan_.get(), device_translation_scan_.get(), + raw_points_tensor_.get(), raw_pointcloud.stream); + + const std::size_t num_obstacle_points = obstacle_pointcloud.width * obstacle_pointcloud.height; + + map_fixed::prepareTensorLaunch( + reinterpret_cast(obstacle_pointcloud.data.get()), num_obstacle_points, + obstacle_pointcloud.point_step / sizeof(float), angle_bin_size, range_bin_size, min_height_, + max_height_, min_angle_, angle_increment_inv_, range_resolution_inv, device_rotation_map_.get(), + device_translation_map_.get(), device_rotation_scan_.get(), device_translation_scan_.get(), + obstacle_points_tensor_.get(), obstacle_pointcloud.stream); + + map_fixed::fillEmptySpaceLaunch( + raw_points_tensor_.get(), angle_bin_size, range_bin_size, range_resolution_inv, + scan_origin.position.x, scan_origin.position.y, origin_x_, origin_y_, num_cells_x, num_cells_y, + cost_value::FREE_SPACE, device_costmap_.get(), raw_pointcloud.stream); + + cudaStreamSynchronize(obstacle_pointcloud.stream); + + map_fixed::fillUnknownSpaceLaunch( + raw_points_tensor_.get(), obstacle_points_tensor_.get(), distance_margin_, angle_bin_size, + range_bin_size, range_resolution_inv, scan_origin.position.x, scan_origin.position.y, origin_x_, + origin_y_, num_cells_x, num_cells_y, cost_value::FREE_SPACE, cost_value::NO_INFORMATION, + device_costmap_.get(), raw_pointcloud.stream); + + map_fixed::fillObstaclesLaunch( + obstacle_points_tensor_.get(), distance_margin_, angle_bin_size, range_bin_size, + range_resolution_inv, origin_x_, origin_y_, num_cells_x, num_cells_y, + cost_value::LETHAL_OBSTACLE, device_costmap_.get(), raw_pointcloud.stream); + + cudaStreamSynchronize(raw_pointcloud.stream); } void OccupancyGridMapFixedBlindSpot::initRosParam(rclcpp::Node & node) diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed_kernel.cu b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed_kernel.cu new file mode 100644 index 0000000000000..3614e025875a3 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_fixed_kernel.cu @@ -0,0 +1,388 @@ +// 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/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_fixed_kernel.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" + +#include +#include + +#include +#include + +namespace autoware::occupancy_grid_map +{ +namespace costmap_2d::map_fixed +{ + +static constexpr float RANGE_DISCRETIZATION_RESOLUTION = 0.001f; + +__global__ void prepareTensorKernel( + const float * __restrict__ input_pointcloud, const std::size_t num_points, + const std::size_t points_step, const std::size_t angle_bins, const std::size_t range_bins, + const float min_height, const float max_height, const float min_angle, + const float angle_increment_inv, const float range_resolution_inv, + const Eigen::Matrix3f * __restrict__ rotation_map, + const Eigen::Vector3f * __restrict__ translation_map, + const Eigen::Matrix3f * __restrict__ rotation_scan, + const Eigen::Vector3f * __restrict__ translation_scan, std::uint64_t * __restrict__ points_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= num_points) { + return; + } + + Eigen::Map point(input_pointcloud + idx * points_step); + + if ( + point.z() > max_height || point.z() < min_height || !isfinite(point.x()) || + !isfinite(point.y()) || !isfinite(point.z())) { + return; + } + + Eigen::Vector3f map_point = (*rotation_map) * point + (*translation_map); + Eigen::Vector3f scan_point = (*rotation_scan) * map_point + (*translation_scan); + + float angle = atan2(scan_point.y(), scan_point.x()); + int angle_bin_index = static_cast((angle - min_angle) * angle_increment_inv); + float range = sqrt(scan_point.y() * scan_point.y() + scan_point.x() * scan_point.x()); + int range_bin_index = static_cast(range * range_resolution_inv); + + if ( + angle_bin_index < 0 || angle_bin_index >= angle_bins || range_bin_index < 0 || + range_bin_index >= range_bins) { + return; + } + + std::uint64_t range_int = static_cast(range / RANGE_DISCRETIZATION_RESOLUTION); + std::uint32_t world_x_int = __float_as_uint(map_point.x()); + std::uint32_t world_y_int = __float_as_uint(map_point.y()); + + // Can not use std::uint64_t for cuda reasons... + static_assert( + sizeof(std::uint64_t) == sizeof(unsigned long long int), + "unsigned long long int is not 8 bytes"); + unsigned long long int range_and_x_int = (range_int << 32) | world_x_int; + unsigned long long int range_and_y_int = (range_int << 32) | world_y_int; + + std::uint64_t * element_address = + points_tensor + 2 * (angle_bin_index * range_bins + range_bin_index); + + atomicMin(reinterpret_cast(element_address), range_and_x_int); + atomicMin(reinterpret_cast(element_address + 1), range_and_y_int); +} + +__global__ void fillEmptySpaceKernel( + const std::uint64_t * __restrict__ points_tensor, const std::size_t angle_bins, + const std::size_t range_bins, const float map_resolution_inv, const float scan_origin_x, + const float scan_origin_y, const float map_origin_x, const float map_origin_y, + const int num_cells_x, const int num_cells_y, std::uint8_t empty_value, + std::uint8_t * __restrict__ costmap_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= angle_bins * range_bins) return; + + int angle_bin_index = idx / range_bins; + int range_bin_index = idx % range_bins; + + std::uint64_t range_and_x = + points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 0]; + std::uint32_t range_int = range_and_x >> 32; + + if (range_int == 0xFFFFFFFF) { + return; + } + + std::uint64_t range_and_y = + points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 1]; + float world_x = __uint_as_float(range_and_x & 0xFFFFFFFF); + float world_y = __uint_as_float(range_and_y & 0xFFFFFFFF); + + if (world_x < map_origin_x || world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + scan_origin_x, scan_origin_y, world_x, world_y, map_origin_x, map_origin_y, map_resolution_inv, + num_cells_x, num_cells_y, empty_value, costmap_tensor); +} + +__global__ void fillUnknownSpaceKernel( + const std::uint64_t * __restrict__ raw_points_tensor, + const std::uint64_t * __restrict__ obstacle_points_tensor, const float distance_margin, + const std::size_t angle_bins, const std::size_t range_bins, const float map_resolution_inv, + const float scan_origin_x, const float scan_origin_y, const float map_origin_x, + const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t free_space_value, std::uint8_t no_information_value, + std::uint8_t * __restrict__ costmap_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= angle_bins * range_bins) return; + + int angle_bin_index = idx / range_bins; + int range_bin_index = idx % range_bins; + + std::uint64_t obs_range_and_x = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 0]; + std::uint64_t obs_range_and_y = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 1]; + std::uint32_t obs_range_int = obs_range_and_x >> 32; + float obs_range = obs_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float obs_world_x = __uint_as_float(obs_range_and_x & 0xFFFFFFFF); + float obs_world_y = __uint_as_float(obs_range_and_y & 0xFFFFFFFF); + + if (obs_range_int == 0xFFFFFFFF || obs_world_x < map_origin_x || obs_world_y < map_origin_y) { + return; + } + + int next_raw_range_bin_index = range_bin_index + 1; + int next_obs_range_bin_index = range_bin_index + 1; + + for (; next_raw_range_bin_index < range_bins; next_raw_range_bin_index++) { + std::uint64_t next_raw_range_and_x = + raw_points_tensor[2 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 0]; + std::uint32_t next_raw_range_int = next_raw_range_and_x >> 32; + float next_raw_range = next_raw_range_int * RANGE_DISCRETIZATION_RESOLUTION; + + if (next_raw_range_int != 0xFFFFFFFF && abs(next_raw_range - obs_range) > distance_margin) { + break; + } + } + + for (; next_obs_range_bin_index < range_bins; next_obs_range_bin_index++) { + std::uint64_t next_obs_range_and_x = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0]; + std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32; + + if (next_obs_range_int != 0xFFFFFFFF) { + break; + } + } + + const std::uint64_t next_obs_range_and_x = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0]; + const std::uint64_t next_obs_range_and_y = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 1]; + const std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32; + const float next_obs_world_x = __uint_as_float(next_obs_range_and_x & 0xFFFFFFFF); + const float next_obs_world_y = __uint_as_float(next_obs_range_and_y & 0xFFFFFFFF); + + const std::uint64_t next_raw_range_and_x = + raw_points_tensor[2 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 0]; + const std::uint64_t next_raw_range_and_y = + raw_points_tensor[2 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 1]; + const std::uint32_t next_raw_range_int = next_raw_range_and_x >> 32; + const float next_raw_world_x = __uint_as_float(next_raw_range_and_x & 0xFFFFFFFF); + const float next_raw_world_y = __uint_as_float(next_raw_range_and_y & 0xFFFFFFFF); + + if (next_obs_range_int == 0xFFFFFFFF) { + if ( + next_raw_range_int == 0xFFFFFFFF || next_raw_world_x < map_origin_x || + next_raw_world_y < map_origin_y) { + return; + } + + // if there is no more obstacles after the current one but there are more raw points + // the space between the current obstacle and the next raw point flagged as no_information_value + autoware::occupancy_grid_map::utils::raytrace( + obs_world_x, obs_world_y, next_raw_world_x, next_raw_world_y, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, no_information_value, costmap_tensor); + + autoware::occupancy_grid_map::utils::setCellValue( + next_raw_world_x, next_raw_world_y, map_origin_x, map_origin_y, map_resolution_inv, + num_cells_x, num_cells_y, free_space_value, costmap_tensor); + return; + } + + float next_obs_range = next_obs_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float obs_to_obs_distance = next_obs_range - obs_range; + + if (obs_to_obs_distance <= distance_margin) { + return; + } else if (next_raw_range_int == 0xFFFFFFFF) { + // fill with no information between obstacles + + if (next_obs_world_x < map_origin_x || next_obs_world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + obs_world_x, obs_world_y, next_obs_world_x, next_obs_world_y, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, no_information_value, costmap_tensor); + + return; + } + + float next_raw_range = next_raw_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float raw_to_obs_distance = abs(next_raw_range - obs_range); + + if (raw_to_obs_distance < obs_to_obs_distance) { + // fill with free space between raw and obstacle + + if (next_raw_world_x < map_origin_x || next_raw_world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + obs_world_x, obs_world_y, next_raw_world_x, next_raw_world_y, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, no_information_value, costmap_tensor); + + autoware::occupancy_grid_map::utils::setCellValue( + next_raw_world_x, next_raw_world_y, map_origin_x, map_origin_y, map_resolution_inv, + num_cells_x, num_cells_y, free_space_value, costmap_tensor); + return; + } else { + // fill with no information between obstacles + + if (next_obs_world_x < map_origin_x || next_obs_world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + obs_world_x, obs_world_y, next_obs_world_x, next_obs_world_y, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, no_information_value, costmap_tensor); + + return; + } +} + +__global__ void fillObstaclesKernel( + const std::uint64_t * __restrict__ obstacle_points_tensor, const float distance_margin, + const std::size_t angle_bins, const std::size_t range_bins, const float map_resolution_inv, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t obstacle_value, std::uint8_t * __restrict__ costmap_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= angle_bins * range_bins) return; + + int angle_bin_index = idx / range_bins; + int range_bin_index = idx % range_bins; + + std::uint64_t range_and_x = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 0]; + std::uint64_t range_and_y = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 1]; + + std::uint32_t range_int = range_and_x >> 32; + float range = range_int * RANGE_DISCRETIZATION_RESOLUTION; + float world_x = __uint_as_float(range_and_x & 0xFFFFFFFF); + float world_y = __uint_as_float(range_and_y & 0xFFFFFFFF); + + if (range < 0.0 || range_int == 0xFFFFFFFF) { + return; + } + + autoware::occupancy_grid_map::utils::setCellValue( + world_x, world_y, map_origin_x, map_origin_y, map_resolution_inv, num_cells_x, num_cells_y, + obstacle_value, costmap_tensor); + + // Look for the next obstacle point + int next_obs_range_bin_index = range_bin_index + 1; + for (; next_obs_range_bin_index < range_bins; next_obs_range_bin_index++) { + std::uint64_t next_obs_range_and_x = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0]; + std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32; + + if (next_obs_range_int != 0xFFFFFFFF) { + break; + } + } + + std::uint64_t next_obs_range_and_x = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 0]; + std::uint64_t next_obs_range_and_y = + obstacle_points_tensor[2 * (angle_bin_index * range_bins + range_bin_index) + 1]; + + std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32; + float next_obs_range = next_obs_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float next_obs_world_x = __uint_as_float(next_obs_range_and_x & 0xFFFFFFFF); + float next_obs_world_y = __uint_as_float(next_obs_range_and_y & 0xFFFFFFFF); + + if (next_obs_range_int == 0xFFFFFFFF || abs(next_obs_range - range) > distance_margin) { + return; + } + + if (next_obs_world_x < map_origin_x || next_obs_world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + world_x, world_y, next_obs_world_x, next_obs_world_y, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, obstacle_value, costmap_tensor); +} + +void prepareTensorLaunch( + const float * input_pointcloud, const std::size_t num_points, const std::size_t points_step, + const std::size_t angle_bins, const std::size_t range_bins, const float min_height, + const float max_height, const float min_angle, const float angle_increment_inv, + const float range_resolution_inv, const Eigen::Matrix3f * rotation_map, + const Eigen::Vector3f * translation_map, const Eigen::Matrix3f * rotation_scan, + const Eigen::Vector3f * translation_scan, std::uint64_t * points_tensor, cudaStream_t stream) +{ + const int threadsPerBlock = 256; + const int blocksPerGrid = (num_points + threadsPerBlock - 1) / threadsPerBlock; + + prepareTensorKernel<<>>( + input_pointcloud, num_points, points_step, angle_bins, range_bins, min_height, max_height, + min_angle, angle_increment_inv, range_resolution_inv, rotation_map, translation_map, + rotation_scan, translation_scan, points_tensor); +} + +void fillEmptySpaceLaunch( + const std::uint64_t * points_tensor, const std::size_t angle_bins, const std::size_t range_bins, + const float map_resolution_inv, const float scan_origin_x, const float scan_origin_y, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t empty_value, std::uint8_t * costmap_tensor, cudaStream_t stream) +{ + const int threadsPerBlock = 256; + const int blocksPerGrid = (angle_bins * range_bins + threadsPerBlock - 1) / threadsPerBlock; + + fillEmptySpaceKernel<<>>( + points_tensor, angle_bins, range_bins, map_resolution_inv, scan_origin_x, scan_origin_y, + map_origin_x, map_origin_y, num_cells_x, num_cells_y, empty_value, costmap_tensor); +} + +void fillUnknownSpaceLaunch( + const std::uint64_t * raw_points_tensor, const std::uint64_t * obstacle_points_tensor, + const float distance_margin, const std::size_t angle_bins, const std::size_t range_bins, + const float map_resolution_inv, const float scan_origin_x, const float scan_origin_y, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t free_space_value, std::uint8_t no_information_value, std::uint8_t * costmap_tensor, + cudaStream_t stream) +{ + const int threadsPerBlock = 256; + const int blocksPerGrid = (angle_bins * range_bins + threadsPerBlock - 1) / threadsPerBlock; + + fillUnknownSpaceKernel<<>>( + raw_points_tensor, obstacle_points_tensor, distance_margin, angle_bins, range_bins, + map_resolution_inv, scan_origin_x, scan_origin_y, map_origin_x, map_origin_y, num_cells_x, + num_cells_y, free_space_value, no_information_value, costmap_tensor); +} + +void fillObstaclesLaunch( + const std::uint64_t * obstacle_points_tensor, const float distance_margin, + const std::size_t angle_bins, const std::size_t range_bins, const float map_resolution_inv, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t obstacle_value, std::uint8_t * costmap_tensor, cudaStream_t stream) +{ + const int threadsPerBlock = 256; + const int blocksPerGrid = (angle_bins * range_bins + threadsPerBlock - 1) / threadsPerBlock; + + fillObstaclesKernel<<>>( + obstacle_points_tensor, distance_margin, angle_bins, range_bins, map_resolution_inv, + map_origin_x, map_origin_y, num_cells_x, num_cells_y, obstacle_value, costmap_tensor); +} + +} // namespace costmap_2d::map_fixed +} // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp index dd934dea26c55..5bc30872fcf70 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -15,23 +15,17 @@ #include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective.hpp" #include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective_kernel.hpp" #include "autoware/probabilistic_occupancy_grid_map/utils/utils.hpp" #include #include #include #include - -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else #include +#include #include -#endif #include @@ -42,9 +36,25 @@ namespace costmap_2d using sensor_msgs::PointCloud2ConstIterator; OccupancyGridMapProjectiveBlindSpot::OccupancyGridMapProjectiveBlindSpot( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: OccupancyGridMapInterface(cells_size_x, cells_size_y, resolution) + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution) +: OccupancyGridMapInterface(use_cuda, cells_size_x, cells_size_y, resolution) { + if (use_cuda) { + const size_t angle_bin_size = + ((max_angle_ - min_angle_) * angle_increment_inv_) + size_t(1 /*margin*/); + + const auto num_cells_x = this->getSizeInCellsX(); + const auto num_cells_y = this->getSizeInCellsY(); + const std::size_t range_bin_size = + static_cast(std::sqrt(2) * std::max(num_cells_x, num_cells_y) / 2.0) + 1; + + raw_points_tensor_ = + autoware::cuda_utils::make_unique(7 * angle_bin_size * range_bin_size); + obstacle_points_tensor_ = + autoware::cuda_utils::make_unique(7 * angle_bin_size * range_bin_size); + device_translation_scan_origin_ = autoware::cuda_utils::make_unique(); + } } /** @@ -56,7 +66,7 @@ OccupancyGridMapProjectiveBlindSpot::OccupancyGridMapProjectiveBlindSpot( * @param scan_origin manually chosen grid map origin frame */ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( - const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, + const CudaPointCloud2 & raw_pointcloud, const CudaPointCloud2 & obstacle_pointcloud, const Pose & robot_pose, const Pose & scan_origin) { const size_t angle_bin_size = @@ -67,261 +77,96 @@ void OccupancyGridMapProjectiveBlindSpot::updateWithPointCloud( const auto scan2map_pose = utils::getInversePose(scan_origin); // scan -> map transform pose - // Transform from map frame to scan frame + // Transform Matrix from map frame to scan frame mat_scan_ = utils::getTransformMatrix(scan2map_pose); - if (!offset_initialized_) { - setFieldOffsets(raw_pointcloud, obstacle_pointcloud); - } - - // Create angle bins and sort points by range - struct BinInfo3D - { - explicit BinInfo3D( - const double _range = 0.0, const double _wx = 0.0, const double _wy = 0.0, - const double _wz = 0.0, const double _projection_length = 0.0, - const double _projected_wx = 0.0, const double _projected_wy = 0.0) - : range(_range), - wx(_wx), - wy(_wy), - wz(_wz), - projection_length(_projection_length), - projected_wx(_projected_wx), - projected_wy(_projected_wy) - { - } - double range; - double wx; - double wy; - double wz; - double projection_length; - double projected_wx; - double projected_wy; - }; - - std::vector> obstacle_pointcloud_angle_bins(angle_bin_size); - std::vector> raw_pointcloud_angle_bins(angle_bin_size); - - const size_t raw_pointcloud_size = raw_pointcloud.width * raw_pointcloud.height; - const size_t obstacle_pointcloud_size = obstacle_pointcloud.width * obstacle_pointcloud.height; - - // Updated every loop inside transformPointAndCalculate() - Eigen::Vector4f pt_map; - int angle_bin_index; - double range; - - size_t global_offset = 0; - for (size_t i = 0; i < raw_pointcloud_size; i++) { - Eigen::Vector4f pt( - *reinterpret_cast(&raw_pointcloud.data[global_offset + x_offset_raw_]), - *reinterpret_cast(&raw_pointcloud.data[global_offset + y_offset_raw_]), - *reinterpret_cast(&raw_pointcloud.data[global_offset + z_offset_raw_]), 1); - global_offset += raw_pointcloud.point_step; - if (!isPointValid(pt)) { - continue; - } - transformPointAndCalculate(pt, pt_map, angle_bin_index, range); - - raw_pointcloud_angle_bins.at(angle_bin_index) - .emplace_back(range, pt_map[0], pt_map[1], pt_map[2]); - } - - for (auto & raw_pointcloud_angle_bin : raw_pointcloud_angle_bins) { - std::sort(raw_pointcloud_angle_bin.begin(), raw_pointcloud_angle_bin.end(), [](auto a, auto b) { - return a.range < b.range; - }); - } - - // Create obstacle angle bins and sort points by range - global_offset = 0; - for (size_t i = 0; i < obstacle_pointcloud_size; i++) { - Eigen::Vector4f pt( - *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + x_offset_obstacle_]), - *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + y_offset_obstacle_]), - *reinterpret_cast( - &obstacle_pointcloud.data[global_offset + z_offset_obstacle_]), - 1); - global_offset += obstacle_pointcloud.point_step; - if (!isPointValid(pt)) { - continue; - } - transformPointAndCalculate(pt, pt_map, angle_bin_index, range); - const double scan_z = scan_origin.position.z - robot_pose.position.z; - const double obstacle_z = (pt_map[2]) - robot_pose.position.z; - const double dz = scan_z - obstacle_z; - - // Ignore obstacle points exceed the range of the raw points - if (raw_pointcloud_angle_bins.at(angle_bin_index).empty()) { - continue; // No raw point in this angle bin - } else if (range > raw_pointcloud_angle_bins.at(angle_bin_index).back().range) { - continue; // Obstacle point exceeds the range of the raw points - } - - if (dz > projection_dz_threshold_) { - const double ratio = obstacle_z / dz; - const double projection_length = range * ratio; - const double projected_wx = (pt_map[0]) + ((pt_map[0]) - scan_origin.position.x) * ratio; - const double projected_wy = (pt_map[1]) + ((pt_map[1]) - scan_origin.position.y) * ratio; - obstacle_pointcloud_angle_bins.at(angle_bin_index) - .emplace_back( - range, pt_map[0], pt_map[1], pt_map[2], projection_length, projected_wx, projected_wy); - } else { - obstacle_pointcloud_angle_bins.at(angle_bin_index) - .emplace_back( - range, pt_map[0], pt_map[1], pt_map[2], std::numeric_limits::infinity(), - std::numeric_limits::infinity(), std::numeric_limits::infinity()); - } - } - - for (auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { - std::sort( - obstacle_pointcloud_angle_bin.begin(), obstacle_pointcloud_angle_bin.end(), - [](auto a, auto b) { return a.range < b.range; }); - } - - grid_map::Costmap2DConverter converter; - if (pub_debug_grid_) { - debug_grid_.clearAll(); - debug_grid_.setFrameId("map"); - debug_grid_.setGeometry( - grid_map::Length(size_x_ * resolution_, size_y_ * resolution_), resolution_, - grid_map::Position( - origin_x_ + size_x_ * resolution_ / 2.0, origin_y_ + size_y_ * resolution_ / 2.0)); - } - - auto is_visible_beyond_obstacle = [&](const BinInfo3D & obstacle, const BinInfo3D & raw) -> bool { - if (raw.range < obstacle.range) { - return false; - } - - if (std::isinf(obstacle.projection_length)) { - return false; - } - - // y = ax + b - const double a = -(scan_origin.position.z - robot_pose.position.z) / - (obstacle.range + obstacle.projection_length); - const double b = scan_origin.position.z; - return raw.wz > (a * raw.range + b); - }; - - // First step: Initialize cells to the final point with freespace - for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - const auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); - - BinInfo3D ray_end; - if (raw_pointcloud_angle_bin.empty()) { - continue; - } else { - ray_end = raw_pointcloud_angle_bin.back(); - } - raytrace( - scan_origin.position.x, scan_origin.position.y, ray_end.wx, ray_end.wy, - cost_value::FREE_SPACE); - } - - if (pub_debug_grid_) - converter.addLayerFromCostmap2D(*this, "filled_free_to_farthest", debug_grid_); - - // Second step: Add unknown cell - for (size_t bin_index = 0; bin_index < obstacle_pointcloud_angle_bins.size(); ++bin_index) { - const auto & obstacle_pointcloud_angle_bin = obstacle_pointcloud_angle_bins.at(bin_index); - const auto & raw_pointcloud_angle_bin = raw_pointcloud_angle_bins.at(bin_index); - auto raw_distance_iter = raw_pointcloud_angle_bin.begin(); - for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { - // Calculate next raw point from obstacle point - const auto & obstacle_bin = obstacle_pointcloud_angle_bin.at(dist_index); - while (raw_distance_iter != raw_pointcloud_angle_bin.end()) { - if (!is_visible_beyond_obstacle(obstacle_bin, *raw_distance_iter)) - raw_distance_iter++; - else - break; - } - - // There is no point farther than the obstacle point. - const bool no_visible_point_beyond = (raw_distance_iter == raw_pointcloud_angle_bin.end()); - if (no_visible_point_beyond) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - raytrace( - source.wx, source.wy, source.projected_wx, source.projected_wy, - cost_value::NO_INFORMATION); - break; - } - - if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - raytrace( - source.wx, source.wy, source.projected_wx, source.projected_wy, - cost_value::NO_INFORMATION); - continue; - } - - auto next_obstacle_point_distance = std::abs( - obstacle_pointcloud_angle_bin.at(dist_index + 1).range - - obstacle_pointcloud_angle_bin.at(dist_index).range); - if (next_obstacle_point_distance <= obstacle_separation_threshold_) { - continue; - } - - auto next_raw_distance = - std::abs(obstacle_pointcloud_angle_bin.at(dist_index).range - raw_distance_iter->range); - if (next_raw_distance < next_obstacle_point_distance) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - const auto & target = *raw_distance_iter; - raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); - setCellValue(target.wx, target.wy, cost_value::FREE_SPACE); - continue; - } else { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::NO_INFORMATION); - continue; - } - } - } - - if (pub_debug_grid_) converter.addLayerFromCostmap2D(*this, "added_unknown", debug_grid_); - - // Third step: Overwrite occupied cell - for (const auto & obstacle_pointcloud_angle_bin : obstacle_pointcloud_angle_bins) { - for (size_t dist_index = 0; dist_index < obstacle_pointcloud_angle_bin.size(); ++dist_index) { - const auto & obstacle_point = obstacle_pointcloud_angle_bin.at(dist_index); - setCellValue(obstacle_point.wx, obstacle_point.wy, cost_value::LETHAL_OBSTACLE); - - if (dist_index + 1 == obstacle_pointcloud_angle_bin.size()) { - continue; - } - - auto next_obstacle_point_distance = std::abs( - obstacle_pointcloud_angle_bin.at(dist_index + 1).range - - obstacle_pointcloud_angle_bin.at(dist_index).range); - if (next_obstacle_point_distance <= obstacle_separation_threshold_) { - const auto & source = obstacle_pointcloud_angle_bin.at(dist_index); - const auto & target = obstacle_pointcloud_angle_bin.at(dist_index + 1); - raytrace(source.wx, source.wy, target.wx, target.wy, cost_value::LETHAL_OBSTACLE); - continue; - } - } - } - - if (pub_debug_grid_) converter.addLayerFromCostmap2D(*this, "added_obstacle", debug_grid_); - if (pub_debug_grid_) { - debug_grid_map_publisher_ptr_->publish(grid_map::GridMapRosConverter::toMessage(debug_grid_)); - } + const auto map_res = this->getResolution(); + const auto num_cells_x = this->getSizeInCellsX(); + const auto num_cells_y = this->getSizeInCellsY(); + const std::size_t range_bin_size = + static_cast(std::sqrt(2) * std::max(num_cells_x, num_cells_y) / 2.0) + 1; + + cudaMemsetAsync( + raw_points_tensor_.get(), 0xFF, 6 * angle_bin_size * range_bin_size * sizeof(std::int64_t), + stream_); + cudaMemsetAsync( + obstacle_points_tensor_.get(), 0xFF, 6 * angle_bin_size * range_bin_size * sizeof(std::int64_t), + stream_); + cudaMemsetAsync( + device_costmap_.get(), cost_value::NO_INFORMATION, + num_cells_x * num_cells_y * sizeof(std::uint8_t), stream_); + + Eigen::Matrix3f rotation_map = mat_map_.block<3, 3>(0, 0); + Eigen::Vector3f translation_map = mat_map_.block<3, 1>(0, 3); + + Eigen::Matrix3f rotation_scan = mat_scan_.block<3, 3>(0, 0); + Eigen::Vector3f translation_scan = mat_scan_.block<3, 1>(0, 3); + + Eigen::Vector3f scan_origin_position( + scan_origin.position.x, scan_origin.position.y, scan_origin.position.z); + + cudaMemcpyAsync( + device_rotation_map_.get(), &rotation_map, sizeof(Eigen::Matrix3f), cudaMemcpyHostToDevice, + stream_); + cudaMemcpyAsync( + device_translation_map_.get(), &translation_map, sizeof(Eigen::Vector3f), + cudaMemcpyHostToDevice, stream_); + cudaMemcpyAsync( + device_rotation_scan_.get(), &rotation_scan, sizeof(Eigen::Matrix3f), cudaMemcpyHostToDevice, + stream_); + cudaMemcpyAsync( + device_translation_scan_.get(), &translation_scan, sizeof(Eigen::Vector3f), + cudaMemcpyHostToDevice, stream_); + cudaMemcpyAsync( + device_translation_scan_origin_.get(), &scan_origin_position, sizeof(Eigen::Vector3f), + cudaMemcpyHostToDevice, stream_); + + const std::size_t num_raw_points = raw_pointcloud.width * raw_pointcloud.height; + float range_resolution_inv = 1.0 / map_res; + + map_projective::prepareRawTensorLaunch( + reinterpret_cast(raw_pointcloud.data.get()), num_raw_points, + raw_pointcloud.point_step / sizeof(float), angle_bin_size, range_bin_size, min_height_, + max_height_, min_angle_, angle_increment_inv_, range_resolution_inv, device_rotation_map_.get(), + device_translation_map_.get(), device_rotation_scan_.get(), device_translation_scan_.get(), + raw_points_tensor_.get(), stream_); + + const std::size_t num_obstacle_points = obstacle_pointcloud.width * obstacle_pointcloud.height; + + map_projective::prepareObstacleTensorLaunch( + reinterpret_cast(obstacle_pointcloud.data.get()), num_obstacle_points, + obstacle_pointcloud.point_step / sizeof(float), angle_bin_size, range_bin_size, min_height_, + max_height_, min_angle_, angle_increment_inv_, range_resolution_inv, projection_dz_threshold_, + device_translation_scan_origin_.get(), device_rotation_map_.get(), + device_translation_map_.get(), device_rotation_scan_.get(), device_translation_scan_.get(), + obstacle_points_tensor_.get(), stream_); + + map_projective::fillEmptySpaceLaunch( + raw_points_tensor_.get(), angle_bin_size, range_bin_size, range_resolution_inv, + scan_origin.position.x, scan_origin.position.y, origin_x_, origin_y_, num_cells_x, num_cells_y, + cost_value::FREE_SPACE, device_costmap_.get(), stream_); + + map_projective::fillUnknownSpaceLaunch( + raw_points_tensor_.get(), obstacle_points_tensor_.get(), obstacle_separation_threshold_, + angle_bin_size, range_bin_size, range_resolution_inv, scan_origin.position.x, + scan_origin.position.y, scan_origin.position.z, origin_x_, origin_y_, robot_pose.position.z, + num_cells_x, num_cells_y, cost_value::FREE_SPACE, cost_value::NO_INFORMATION, + device_costmap_.get(), stream_); + + map_projective::fillObstaclesLaunch( + obstacle_points_tensor_.get(), obstacle_separation_threshold_, angle_bin_size, range_bin_size, + range_resolution_inv, origin_x_, origin_y_, num_cells_x, num_cells_y, + cost_value::LETHAL_OBSTACLE, device_costmap_.get(), stream_); + + cudaStreamSynchronize(stream_); } void OccupancyGridMapProjectiveBlindSpot::initRosParam(rclcpp::Node & node) { projection_dz_threshold_ = - node.declare_parameter("OccupancyGridMapProjectiveBlindSpot.projection_dz_threshold"); - obstacle_separation_threshold_ = node.declare_parameter( + node.declare_parameter("OccupancyGridMapProjectiveBlindSpot.projection_dz_threshold"); + obstacle_separation_threshold_ = node.declare_parameter( "OccupancyGridMapProjectiveBlindSpot.obstacle_separation_threshold"); - pub_debug_grid_ = - node.declare_parameter("OccupancyGridMapProjectiveBlindSpot.pub_debug_grid"); - debug_grid_map_publisher_ptr_ = node.create_publisher( - "~/debug/grid_map", rclcpp::QoS(1).durability_volatile()); } } // namespace costmap_2d diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective_kernel.cu b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective_kernel.cu new file mode 100644 index 0000000000000..975a08c9d41b8 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/costmap_2d/occupancy_grid_map_projective_kernel.cu @@ -0,0 +1,534 @@ +// 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/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_projective_kernel.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" + +#include +#include + +#include +#include +#include + +namespace autoware::occupancy_grid_map +{ +namespace costmap_2d::map_projective +{ + +static constexpr float RANGE_DISCRETIZATION_RESOLUTION = 0.001f; + +bool __device__ isVisibleBeyondObstacle( + const std::uint64_t * obstacle_element, const std::uint64_t * raw_element, + const float & scan_origin_z, const float & robot_pose_z) +{ + std::uint64_t raw_range_and_z = raw_element[2]; + std::uint32_t raw_range_int = raw_range_and_z >> 32; + float raw_range = raw_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float raw_world_z = __uint_as_float(raw_range_and_z & 0xFFFFFFFF); + + std::uint64_t obstacle_range_and_pl = raw_element[3]; + std::uint32_t obstacle_range_int = obstacle_range_and_pl >> 32; + float obstacle_range = obstacle_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float obstacle_pl = __uint_as_float(obstacle_range_and_pl & 0xFFFFFFFF); + + if (raw_range < obstacle_range) { + return false; + } + + if (std::isinf(obstacle_pl)) { + return false; + } + + // y = ax + b + const double a = -(scan_origin_z - robot_pose_z) / (obstacle_range + obstacle_pl); + const double b = scan_origin_z; + return raw_world_z > (a * raw_range + b); +}; + +__global__ void prepareRawTensorKernel( + const float * input_pointcloud, const std::size_t num_points, const std::size_t points_step, + const std::size_t angle_bins, const std::size_t range_bins, const float min_height, + const float max_height, const float min_angle, const float angle_increment_inv, + const float range_resolution_inv, const Eigen::Matrix3f * rotation_map, + const Eigen::Vector3f * translation_map, const Eigen::Matrix3f * rotation_scan, + const Eigen::Vector3f * translation_scan, std::uint64_t * points_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= num_points) { + return; + } + + Eigen::Map point(input_pointcloud + idx * points_step); + + if ( + point.z() > max_height || point.z() < min_height || !isfinite(point.x()) || + !isfinite(point.y()) || !isfinite(point.z())) { + return; + } + + Eigen::Vector3f map_point = (*rotation_map) * point + (*translation_map); + Eigen::Vector3f scan_point = (*rotation_scan) * map_point + (*translation_scan); + + float angle = atan2(scan_point.y(), scan_point.x()); + int angle_bin_index = static_cast((angle - min_angle) * angle_increment_inv); + float range = sqrt(scan_point.y() * scan_point.y() + scan_point.x() * scan_point.x()); + int range_bin_index = static_cast(range * range_resolution_inv); + + if ( + angle_bin_index < 0 || angle_bin_index >= angle_bins || range_bin_index < 0 || + range_bin_index >= range_bins) { + return; + } + + std::uint64_t range_int = static_cast(range / RANGE_DISCRETIZATION_RESOLUTION); + std::uint32_t world_x_int = __float_as_uint(map_point.x()); + std::uint32_t world_y_int = __float_as_uint(map_point.y()); + std::uint32_t world_z_int = __float_as_uint(map_point.z()); + std::uint32_t projected_length_int = __float_as_uint(0.f); + std::uint32_t projected_world_x_int = __float_as_uint(0.f); + std::uint32_t projected_world_y_int = __float_as_uint(0.f); + + // Can not use std::uint64_t for cuda reasons... + static_assert( + sizeof(std::uint64_t) == sizeof(unsigned long long int), + "unsigned long long int is not 8 bytes"); + unsigned long long int range_and_x_int = (range_int << 32) | world_x_int; + unsigned long long int range_and_y_int = (range_int << 32) | world_y_int; + unsigned long long int range_and_z_int = (range_int << 32) | world_z_int; + unsigned long long int range_and_pl_int = (range_int << 32) | projected_length_int; + unsigned long long int range_and_px_int = (range_int << 32) | projected_world_x_int; + unsigned long long int range_and_py_int = (range_int << 32) | projected_world_y_int; + + std::uint64_t * element_address = + points_tensor + 6 * (angle_bin_index * range_bins + range_bin_index); + + atomicMin(reinterpret_cast(element_address + 0), range_and_x_int); + atomicMin(reinterpret_cast(element_address + 1), range_and_y_int); + atomicMin(reinterpret_cast(element_address + 2), range_and_z_int); + atomicMin(reinterpret_cast(element_address + 3), range_and_pl_int); + atomicMin(reinterpret_cast(element_address + 4), range_and_px_int); + atomicMin(reinterpret_cast(element_address + 5), range_and_py_int); +} + +__global__ void prepareObstacleTensorKernel( + const float * input_pointcloud, const std::size_t num_points, const std::size_t points_step, + const std::size_t angle_bins, const std::size_t range_bins, const float min_height, + const float max_height, const float min_angle, const float angle_increment_inv, + const float range_resolution_inv, const float projection_dz_threshold, + const Eigen::Vector3f * translation_scan_origin, const Eigen::Matrix3f * rotation_map, + const Eigen::Vector3f * translation_map, const Eigen::Matrix3f * rotation_scan, + const Eigen::Vector3f * translation_scan, std::uint64_t * points_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= num_points) { + return; + } + + Eigen::Map point(input_pointcloud + idx * points_step); + + if ( + point.z() > max_height || point.z() < min_height || !isfinite(point.x()) || + !isfinite(point.y()) || !isfinite(point.z())) { + return; + } + + Eigen::Vector3f map_point = (*rotation_map) * point + (*translation_map); + Eigen::Vector3f scan_point = (*rotation_scan) * map_point + (*translation_scan); + + float angle = atan2(scan_point.y(), scan_point.x()); + int angle_bin_index = static_cast((angle - min_angle) * angle_increment_inv); + float range = sqrt(scan_point.y() * scan_point.y() + scan_point.x() * scan_point.x()); + int range_bin_index = static_cast(range * range_resolution_inv); + + if ( + angle_bin_index < 0 || angle_bin_index >= angle_bins || range_bin_index < 0 || + range_bin_index >= range_bins) { + return; + } + + std::uint64_t range_int = static_cast(range / RANGE_DISCRETIZATION_RESOLUTION); + std::uint32_t world_x_int = __float_as_uint(map_point.x()); + std::uint32_t world_y_int = __float_as_uint(map_point.y()); + std::uint32_t world_z_int = __float_as_uint(map_point.z()); + + const float scan_z = translation_scan_origin->z(); + -translation_map->z(); + const float obstacle_z = map_point.z() - translation_scan_origin->z(); + const float dz = scan_z - obstacle_z; + + float projected_length, projected_world_x, projected_world_y; + + if (dz > projection_dz_threshold) { + const float ratio = obstacle_z / dz; + projected_length = range * ratio; + projected_world_x = map_point.x() + (map_point.x() - translation_scan_origin->x()) * ratio; + projected_world_y = map_point.y() + (map_point.y() - translation_scan_origin->y()) * ratio; + } else { + projected_length = INFINITY; + projected_world_x = INFINITY; + projected_world_y = INFINITY; + } + + std::uint32_t projected_length_int = __float_as_uint(projected_length); + std::uint32_t projected_world_x_int = __float_as_uint(projected_world_x); + std::uint32_t projected_world_y_int = __float_as_uint(projected_world_y); + + // Can not use std::uint64_t for cuda reasons... + static_assert( + sizeof(std::uint64_t) == sizeof(unsigned long long int), + "unsigned long long int is not 8 bytes"); + unsigned long long int range_and_x_int = (range_int << 32) | world_x_int; + unsigned long long int range_and_y_int = (range_int << 32) | world_y_int; + unsigned long long int range_and_z_int = (range_int << 32) | world_z_int; + unsigned long long int range_and_pl_int = (range_int << 32) | projected_length_int; + unsigned long long int range_and_px_int = (range_int << 32) | projected_world_x_int; + unsigned long long int range_and_py_int = (range_int << 32) | projected_world_y_int; + + std::uint64_t * element_address = + points_tensor + 6 * (angle_bin_index * range_bins + range_bin_index); + + atomicMin(reinterpret_cast(element_address + 0), range_and_x_int); + atomicMin(reinterpret_cast(element_address + 1), range_and_y_int); + atomicMin(reinterpret_cast(element_address + 2), range_and_z_int); + atomicMin(reinterpret_cast(element_address + 3), range_and_pl_int); + atomicMin(reinterpret_cast(element_address + 4), range_and_px_int); + atomicMin(reinterpret_cast(element_address + 5), range_and_py_int); +} + +__global__ void fillEmptySpaceKernel( + const std::uint64_t * points_tensor, const std::size_t angle_bins, const std::size_t range_bins, + const float map_resolution_inv, const float scan_origin_x, const float scan_origin_y, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t empty_value, std::uint8_t * costmap_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= angle_bins * range_bins) return; + + int angle_bin_index = idx / range_bins; + int range_bin_index = idx % range_bins; + + std::uint64_t range_and_x = + points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 0]; + std::uint32_t range_int = range_and_x >> 32; + + if (range_int == 0xFFFFFFFF) { + return; + } + + std::uint64_t range_and_y = + points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 1]; + float world_x = __uint_as_float(range_and_x & 0xFFFFFFFF); + float world_y = __uint_as_float(range_and_y & 0xFFFFFFFF); + + if (world_x < map_origin_x || world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + scan_origin_x, scan_origin_y, world_x, world_y, map_origin_x, map_origin_y, map_resolution_inv, + num_cells_x, num_cells_y, empty_value, costmap_tensor); +} + +__global__ void fillUnknownSpaceKernel( + const std::uint64_t * raw_points_tensor, const std::uint64_t * obstacle_points_tensor, + const float obstacle_separation_threshold, const std::size_t angle_bins, + const std::size_t range_bins, const float map_resolution_inv, const float scan_origin_x, + const float scan_origin_y, const float scan_origin_z, const float map_origin_x, + const float map_origin_y, const float robot_pose_z, const int num_cells_x, const int num_cells_y, + std::uint8_t free_space_value, std::uint8_t no_information_value, std::uint8_t * costmap_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= angle_bins * range_bins) return; + + int angle_bin_index = idx / range_bins; + int range_bin_index = idx % range_bins; + + std::uint64_t obs_range_and_x = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 0]; + std::uint64_t obs_range_and_y = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 1]; + std::uint64_t obs_range_and_px = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 4]; + std::uint64_t obs_range_and_py = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 5]; + std::uint32_t obs_range_int = obs_range_and_x >> 32; + float obs_range = obs_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float obs_world_x = __uint_as_float(obs_range_and_x & 0xFFFFFFFF); + float obs_world_y = __uint_as_float(obs_range_and_y & 0xFFFFFFFF); + float obs_world_px = __uint_as_float(obs_range_and_px & 0xFFFFFFFF); + float obs_world_py = __uint_as_float(obs_range_and_py & 0xFFFFFFFF); + + if (obs_range_int == 0xFFFFFFFF || obs_world_x < map_origin_x || obs_world_y < map_origin_y) { + return; + } + + int next_raw_range_bin_index = range_bin_index + 1; + int next_obs_range_bin_index = range_bin_index + 1; + + for (; next_raw_range_bin_index < range_bins; next_raw_range_bin_index++) { + std::uint64_t next_raw_range_and_x = + raw_points_tensor[6 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 0]; + std::uint32_t next_raw_range_int = next_raw_range_and_x >> 32; + + if ( + next_raw_range_int != 0xFFFFFFFF && + isVisibleBeyondObstacle( + obstacle_points_tensor + 6 * (angle_bin_index * range_bins + range_bin_index), + raw_points_tensor + 6 * (angle_bin_index * range_bins + next_raw_range_bin_index), + scan_origin_z, robot_pose_z)) { + break; + } + } + + for (; next_obs_range_bin_index < range_bins; next_obs_range_bin_index++) { + std::uint64_t next_obs_range_and_x = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0]; + std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32; + + if (next_obs_range_int != 0xFFFFFFFF) { + break; + } + } + + const std::uint64_t next_obs_range_and_x = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0]; + const std::uint64_t next_obs_range_and_y = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 1]; + const std::uint64_t next_obs_range_and_px = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 4]; + const std::uint64_t next_obs_range_and_py = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 5]; + const std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32; + const float next_obs_world_x = __uint_as_float(next_obs_range_and_x & 0xFFFFFFFF); + const float next_obs_world_y = __uint_as_float(next_obs_range_and_y & 0xFFFFFFFF); + const float next_obs_world_px = __uint_as_float(next_obs_range_and_px & 0xFFFFFFFF); + const float next_obs_world_py = __uint_as_float(next_obs_range_and_py & 0xFFFFFFFF); + + float next_obs_range = next_obs_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float obs_to_obs_distance = next_obs_range - obs_range; + + const std::uint64_t next_raw_range_and_x = + raw_points_tensor[6 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 0]; + const std::uint64_t next_raw_range_and_y = + raw_points_tensor[6 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 1]; + const std::uint64_t next_raw_range_and_px = + raw_points_tensor[6 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 4]; + const std::uint64_t next_raw_range_and_py = + raw_points_tensor[6 * (angle_bin_index * range_bins + next_raw_range_bin_index) + 5]; + const std::uint32_t next_raw_range_int = next_raw_range_and_x >> 32; + const float next_raw_world_x = __uint_as_float(next_raw_range_and_x & 0xFFFFFFFF); + const float next_raw_world_y = __uint_as_float(next_raw_range_and_y & 0xFFFFFFFF); + const float next_raw_world_px = __uint_as_float(next_raw_range_and_px & 0xFFFFFFFF); + const float next_raw_world_py = __uint_as_float(next_raw_range_and_py & 0xFFFFFFFF); + + if (next_raw_range_int == 0xFFFFFFFF) { + autoware::occupancy_grid_map::utils::raytrace( + obs_world_x, obs_world_y, obs_world_px, obs_world_py, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, no_information_value, costmap_tensor); + return; + } + + if (next_obs_range_int == 0xFFFFFFFF) { + autoware::occupancy_grid_map::utils::raytrace( + obs_world_x, obs_world_y, obs_world_px, obs_world_py, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, no_information_value, costmap_tensor); + return; + } + + float next_raw_range = next_raw_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float raw_to_obs_distance = abs(next_raw_range - obs_range); + + if (obs_to_obs_distance <= obstacle_separation_threshold) { + return; + } + + if (raw_to_obs_distance < obs_to_obs_distance) { + if (next_raw_world_x < map_origin_x || next_raw_world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + obs_world_x, obs_world_y, next_raw_world_x, next_raw_world_y, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, no_information_value, costmap_tensor); + + autoware::occupancy_grid_map::utils::setCellValue( + next_raw_world_x, next_raw_world_y, map_origin_x, map_origin_y, map_resolution_inv, + num_cells_x, num_cells_y, free_space_value, costmap_tensor); + return; + } else { + // fill with no information between obstacles + + if (next_obs_world_x < map_origin_x || next_obs_world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + obs_world_x, obs_world_y, next_obs_world_x, next_obs_world_y, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, no_information_value, costmap_tensor); + + return; + } +} + +__global__ void fillObstaclesKernel( + const std::uint64_t * obstacle_points_tensor, const float distance_margin, + const std::size_t angle_bins, const std::size_t range_bins, const float map_resolution_inv, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t obstacle_value, std::uint8_t * costmap_tensor) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= angle_bins * range_bins) return; + + int angle_bin_index = idx / range_bins; + int range_bin_index = idx % range_bins; + + std::uint64_t range_and_x = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 0]; + std::uint64_t range_and_y = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 1]; + + std::uint32_t range_int = range_and_x >> 32; + float range = range_int * RANGE_DISCRETIZATION_RESOLUTION; + float world_x = __uint_as_float(range_and_x & 0xFFFFFFFF); + float world_y = __uint_as_float(range_and_y & 0xFFFFFFFF); + + if (range < 0.0 || range_int == 0xFFFFFFFF) { + return; + } + + autoware::occupancy_grid_map::utils::setCellValue( + world_x, world_y, map_origin_x, map_origin_y, map_resolution_inv, num_cells_x, num_cells_y, + obstacle_value, costmap_tensor); + + // Look for the next obstacle point + int next_obs_range_bin_index = range_bin_index + 1; + for (; next_obs_range_bin_index < range_bins; next_obs_range_bin_index++) { + std::uint64_t next_obs_range_and_x = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + next_obs_range_bin_index) + 0]; + std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32; + + if (next_obs_range_int != 0xFFFFFFFF) { + break; + } + } + + std::uint64_t next_obs_range_and_x = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 0]; + std::uint64_t next_obs_range_and_y = + obstacle_points_tensor[6 * (angle_bin_index * range_bins + range_bin_index) + 1]; + + std::uint32_t next_obs_range_int = next_obs_range_and_x >> 32; + float next_obs_range = next_obs_range_int * RANGE_DISCRETIZATION_RESOLUTION; + float next_obs_world_x = __uint_as_float(next_obs_range_and_x & 0xFFFFFFFF); + float next_obs_world_y = __uint_as_float(next_obs_range_and_y & 0xFFFFFFFF); + + if (next_obs_range_int == 0xFFFFFFFF || abs(next_obs_range - range) > distance_margin) { + return; + } + + if (next_obs_world_x < map_origin_x || next_obs_world_y < map_origin_y) { + return; + } + + autoware::occupancy_grid_map::utils::raytrace( + world_x, world_y, next_obs_world_x, next_obs_world_y, map_origin_x, map_origin_y, + map_resolution_inv, num_cells_x, num_cells_y, obstacle_value, costmap_tensor); +} + +void prepareRawTensorLaunch( + const float * input_pointcloud, const std::size_t num_points, const std::size_t points_step, + const std::size_t angle_bins, const std::size_t range_bins, const float min_height, + const float max_height, const float min_angle, const float angle_increment_inv, + const float range_resolution_inv, const Eigen::Matrix3f * rotation_map, + const Eigen::Vector3f * translation_map, const Eigen::Matrix3f * rotation_scan, + const Eigen::Vector3f * translation_scan, std::uint64_t * points_tensor, cudaStream_t stream) +{ + const int threads_per_block = 256; + const int num_blocks = (num_points + threads_per_block - 1) / threads_per_block; + + prepareRawTensorKernel<<>>( + input_pointcloud, num_points, points_step, angle_bins, range_bins, min_height, max_height, + min_angle, angle_increment_inv, range_resolution_inv, rotation_map, translation_map, + rotation_scan, translation_scan, points_tensor); +} + +void prepareObstacleTensorLaunch( + const float * input_pointcloud, const std::size_t num_points, const std::size_t points_step, + const std::size_t angle_bins, const std::size_t range_bins, const float min_height, + const float max_height, const float min_angle, const float angle_increment_inv, + const float range_resolution_inv, const float projection_dz_threshold, + const Eigen::Vector3f * translation_scan_origin, const Eigen::Matrix3f * rotation_map, + const Eigen::Vector3f * translation_map, const Eigen::Matrix3f * rotation_scan, + const Eigen::Vector3f * translation_scan, std::uint64_t * points_tensor, cudaStream_t stream) +{ + const int threads_per_block = 256; + const int num_blocks = (num_points + threads_per_block - 1) / threads_per_block; + + prepareObstacleTensorKernel<<>>( + input_pointcloud, num_points, points_step, angle_bins, range_bins, min_height, max_height, + min_angle, angle_increment_inv, range_resolution_inv, projection_dz_threshold, + translation_scan_origin, rotation_map, translation_map, rotation_scan, translation_scan, + points_tensor); +} + +void fillEmptySpaceLaunch( + const std::uint64_t * points_tensor, const std::size_t angle_bins, const std::size_t range_bins, + const float map_resolution_inv, const float scan_origin_x, const float scan_origin_y, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t empty_value, std::uint8_t * costmap_tensor, cudaStream_t stream) +{ + const int threads_per_block = 256; + const int num_blocks = (angle_bins * range_bins + threads_per_block - 1) / threads_per_block; + + fillEmptySpaceKernel<<>>( + points_tensor, angle_bins, range_bins, map_resolution_inv, scan_origin_x, scan_origin_y, + map_origin_x, map_origin_y, num_cells_x, num_cells_y, empty_value, costmap_tensor); +} + +void fillUnknownSpaceLaunch( + const std::uint64_t * raw_points_tensor, const std::uint64_t * obstacle_points_tensor, + const float obstacle_separation_threshold, const std::size_t angle_bins, + const std::size_t range_bins, const float map_resolution_inv, const float scan_origin_x, + const float scan_origin_y, const float scan_origin_z, const float map_origin_x, + const float map_origin_y, const float robot_pose_z, const int num_cells_x, const int num_cells_y, + std::uint8_t free_space_value, std::uint8_t no_information_value, std::uint8_t * costmap_tensor, + cudaStream_t stream) +{ + const int threads_per_block = 256; + const int num_blocks = (angle_bins * range_bins + threads_per_block - 1) / threads_per_block; + + fillUnknownSpaceKernel<<>>( + raw_points_tensor, obstacle_points_tensor, obstacle_separation_threshold, angle_bins, + range_bins, map_resolution_inv, scan_origin_x, scan_origin_y, scan_origin_z, map_origin_x, + map_origin_y, robot_pose_z, num_cells_x, num_cells_y, free_space_value, no_information_value, + costmap_tensor); +} + +void fillObstaclesLaunch( + const std::uint64_t * obstacle_points_tensor, const float distance_margin, + const std::size_t angle_bins, const std::size_t range_bins, const float map_resolution_inv, + const float map_origin_x, const float map_origin_y, const int num_cells_x, const int num_cells_y, + std::uint8_t obstacle_value, std::uint8_t * costmap_tensor, cudaStream_t stream) +{ + const int threads_per_block = 256; + const int num_blocks = (angle_bins * range_bins + threads_per_block - 1) / threads_per_block; + + fillObstaclesKernel<<>>( + obstacle_points_tensor, distance_margin, angle_bins, range_bins, map_resolution_inv, + map_origin_x, map_origin_y, num_cells_x, num_cells_y, obstacle_value, costmap_tensor); +} + +} // namespace costmap_2d::map_projective +} // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp index 771eaabf4f6f9..cdcb0677b7243 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -15,6 +15,7 @@ #include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" #include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater_kernel.hpp" #include #include @@ -25,8 +26,9 @@ namespace costmap_2d { OccupancyGridMapBBFUpdater::OccupancyGridMapBBFUpdater( - const unsigned int cells_size_x, const unsigned int cells_size_y, const float resolution) -: OccupancyGridMapUpdaterInterface(cells_size_x, cells_size_y, resolution) + const bool use_cuda, const unsigned int cells_size_x, const unsigned int cells_size_y, + const float resolution) +: OccupancyGridMapUpdaterInterface(use_cuda, cells_size_x, cells_size_y, resolution) { } @@ -41,6 +43,22 @@ void OccupancyGridMapBBFUpdater::initRosParam(rclcpp::Node & node) probability_matrix_(Index::OCCUPIED, Index::FREE) = node.declare_parameter("probability_matrix.free_to_occupied"); v_ratio_ = node.declare_parameter("v_ratio"); + + device_probability_matrix_ = + autoware::cuda_utils::make_unique(Index::NUM_STATES * Index::NUM_STATES); + + std::vector probability_matrix_vector; + probability_matrix_vector.resize(Index::NUM_STATES * Index::NUM_STATES); + + for (size_t j = 0; j < Index::NUM_STATES; j++) { + for (size_t i = 0; i < Index::NUM_STATES; i++) { + probability_matrix_vector[j * Index::NUM_STATES + i] = probability_matrix_(j, i); + } + } + + cudaMemcpy( + device_probability_matrix_.get(), probability_matrix_vector.data(), + sizeof(float) * Index::NUM_STATES * Index::NUM_STATES, cudaMemcpyHostToDevice); } inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( @@ -69,16 +87,37 @@ inline unsigned char OccupancyGridMapBBFUpdater::applyBBF( static_cast(254)); } -bool OccupancyGridMapBBFUpdater::update(const Costmap2D & single_frame_occupancy_grid_map) +bool OccupancyGridMapBBFUpdater::update( + const OccupancyGridMapInterface & single_frame_occupancy_grid_map) { updateOrigin( single_frame_occupancy_grid_map.getOriginX(), single_frame_occupancy_grid_map.getOriginY()); - for (unsigned int x = 0; x < getSizeInCellsX(); x++) { - for (unsigned int y = 0; y < getSizeInCellsY(); y++) { - unsigned int index = getIndex(x, y); - costmap_[index] = applyBBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + + if (use_cuda_ != single_frame_occupancy_grid_map.isCudaEnabled()) { + throw std::runtime_error("The CUDA setting of the updater and the map do not match."); + } + + if (use_cuda_) { + applyBBFLaunch( + single_frame_occupancy_grid_map.getDeviceCostmap().get(), device_probability_matrix_.get(), + Index::NUM_STATES, Index::FREE, Index::OCCUPIED, cost_value::FREE_SPACE, + cost_value::LETHAL_OBSTACLE, cost_value::NO_INFORMATION, v_ratio_, + getSizeInCellsX() * getSizeInCellsY(), device_costmap_.get(), stream_); + + cudaMemcpy( + costmap_, device_costmap_.get(), getSizeInCellsX() * getSizeInCellsY() * sizeof(std::uint8_t), + cudaMemcpyDeviceToHost); + + cudaStreamSynchronize(stream_); + } else { + for (unsigned int x = 0; x < getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < getSizeInCellsY(); y++) { + unsigned int index = getIndex(x, y); + costmap_[index] = applyBBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + } } } + return true; } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater_kernel.cu b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater_kernel.cu new file mode 100644 index 0000000000000..f4d4886eb0182 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/binary_bayes_filter_updater_kernel.cu @@ -0,0 +1,81 @@ +// 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__probabilistic_occupancy_grid_map__UPDATER__BINARY_BAYES_FILTER_UPDATER_KERNEL_HPP_ +#define AUTOWARE__probabilistic_occupancy_grid_map__UPDATER__BINARY_BAYES_FILTER_UPDATER_KERNEL_HPP_ + +#include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater_kernel.hpp" + +#include + +__global__ void applyBBFKernel( + const std::uint8_t * z_costmap, const float * probability_matrix, const int num_states, + const int free_index, const int occupied_index, const std::uint8_t free_space_value, + const std::uint8_t lethal_obstacle_value, const std::uint8_t no_information_value, + const double v_ratio_, const int num_elements, std::uint8_t * o_costmap) +{ + const int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= num_elements) { + return; + } + + const std::uint8_t z = z_costmap[idx]; + const std::uint8_t o = o_costmap[idx]; + + constexpr float cost2p = 1.f / 255.f; + const float po = o * cost2p; + float pz{}; + float not_pz{}; + float po_hat{}; + if (z == lethal_obstacle_value) { + pz = probability_matrix[occupied_index * num_states + occupied_index]; + not_pz = probability_matrix[free_index * num_states + occupied_index]; + po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); + } else if (z == free_space_value) { + pz = 1.f - probability_matrix[free_index * num_states + free_index]; + not_pz = 1.f - probability_matrix[occupied_index * num_states + free_index]; + po_hat = ((po * pz) / ((po * pz) + ((1.f - po) * not_pz))); + } else if (z == no_information_value) { + const float inv_v_ratio = 1.f / v_ratio_; + po_hat = ((po + (0.5f * inv_v_ratio)) / ((1.f * inv_v_ratio) + 1.f)); + } + + o_costmap[idx] = std::min( + std::max( + static_cast(std::lround(po_hat * 255.f)), static_cast(1)), + static_cast(254)); +} + +namespace autoware::occupancy_grid_map +{ +namespace costmap_2d +{ + +void applyBBFLaunch( + const std::uint8_t * z_costmap, const float * probability_matrix, const int num_states, + const int free_index, const int occupied_index, const std::uint8_t free_space_value, + const std::uint8_t lethal_obstacle_value, const std::uint8_t no_information_value, + const double v_ratio_, const int num_elements, std::uint8_t * o_costmap, cudaStream_t stream) +{ + const int threads_per_block = 256; + const int num_blocks = (num_elements + threads_per_block - 1) / threads_per_block; + applyBBFKernel<<>>( + z_costmap, probability_matrix, num_states, free_index, occupied_index, free_space_value, + lethal_obstacle_value, no_information_value, v_ratio_, num_elements, o_costmap); +} + +} // namespace costmap_2d +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__probabilistic_occupancy_grid_map__UPDATER__BINARY_BAYES_FILTER_UPDATER_KERNEL_HPP_ diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp index 9f3c3c7e40eaf..b1a8ff937c495 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// 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. @@ -15,6 +15,7 @@ #include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater.hpp" #include "autoware/probabilistic_occupancy_grid_map/cost_value/cost_value.hpp" +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater_kernel.hpp" #include @@ -39,7 +40,7 @@ inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( constexpr unsigned char unknown = cost_value::NO_INFORMATION; constexpr unsigned char unknown_margin = 1; - /* Tau and ST decides how fast the observation decay to the unknown status*/ + // Tau and ST decides how fast the observation decay to the unknown status constexpr double tau = 0.75; constexpr double sample_time = 0.1; @@ -58,16 +59,35 @@ inline unsigned char OccupancyGridMapLOBFUpdater::applyLOBF( } } -bool OccupancyGridMapLOBFUpdater::update(const Costmap2D & single_frame_occupancy_grid_map) +bool OccupancyGridMapLOBFUpdater::update( + const OccupancyGridMapInterface & single_frame_occupancy_grid_map) { updateOrigin( single_frame_occupancy_grid_map.getOriginX(), single_frame_occupancy_grid_map.getOriginY()); - for (unsigned int x = 0; x < getSizeInCellsX(); x++) { - for (unsigned int y = 0; y < getSizeInCellsY(); y++) { - unsigned int index = getIndex(x, y); - costmap_[index] = applyLOBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + + if (use_cuda_ != single_frame_occupancy_grid_map.isCudaEnabled()) { + throw std::runtime_error("The CUDA setting of the updater and the map do not match."); + } + + if (use_cuda_) { + applyLOBFLaunch( + single_frame_occupancy_grid_map.getDeviceCostmap().get(), cost_value::NO_INFORMATION, + getSizeInCellsX() * getSizeInCellsY(), device_costmap_.get(), stream_); + + cudaMemcpy( + costmap_, device_costmap_.get(), getSizeInCellsX() * getSizeInCellsY() * sizeof(std::uint8_t), + cudaMemcpyDeviceToHost); + + cudaStreamSynchronize(stream_); + } else { + for (unsigned int x = 0; x < getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < getSizeInCellsY(); y++) { + unsigned int index = getIndex(x, y); + costmap_[index] = applyLOBF(single_frame_occupancy_grid_map.getCost(x, y), costmap_[index]); + } } } + return true; } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater_kernel.cu b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater_kernel.cu new file mode 100644 index 0000000000000..16705adac31b9 --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/updater/log_odds_bayes_filter_updater_kernel.cu @@ -0,0 +1,95 @@ +// 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__probabilistic_occupancy_grid_map__UPDATER__log_odds_bayes_filter_updater_kernel_HPP_ +#define AUTOWARE__probabilistic_occupancy_grid_map__UPDATER__log_odds_bayes_filter_updater_kernel_HPP_ + +#include "autoware/probabilistic_occupancy_grid_map/updater/log_odds_bayes_filter_updater_kernel.hpp" + +#include + +#define EPSILON_PROB 0.03 + +namespace autoware::occupancy_grid_map +{ +namespace costmap_2d +{ + +__host__ __device__ __forceinline__ double convertCharToProbability(const std::uint8_t value) +{ + return static_cast(value) / 255.0; +} + +__host__ __device__ __forceinline__ std::uint8_t convertProbabilityToChar(const double value) +{ + return static_cast(std::max(0.0, std::min(1.0, value)) * 255.0); + ; +} + +__host__ __device__ __forceinline__ double logOddsFusion(const double p1, const double p2) +{ + double log_odds = 0.0; + + const double p1_norm = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, p1)); + log_odds += std::log(p1_norm / (1.0 - p1_norm)); + + const double p2_norm = std::max(EPSILON_PROB, std::min(1.0 - EPSILON_PROB, p2)); + log_odds += std::log(p2_norm / (1.0 - p2_norm)); + + return 1.0 / (1.0 + std::exp(-log_odds)); +} + +// cspell: ignore LOBF +__global__ void applyLOBFKernel( + const std::uint8_t * z_costmap, const std::uint8_t unknown_value, const int num_elements, + std::uint8_t * o_costmap) +{ + const int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= num_elements) { + return; + } + + const std::uint8_t z = z_costmap[idx]; + const std::uint8_t o = o_costmap[idx]; + + const std::uint8_t unknown_margin = 1; + const double tau = 0.75; + const double sample_time = 0.1; + + if (z >= unknown_value - unknown_margin && z <= unknown_value + unknown_margin) { + const int diff = static_cast(o) - static_cast(unknown_value); + const double decay = std::exp(-sample_time / tau); + const double fused = static_cast(unknown_value) + static_cast(diff) * decay; + o_costmap[idx] = static_cast(fused); + } else { + const unsigned char fused = convertProbabilityToChar( + logOddsFusion(convertCharToProbability(z), convertCharToProbability(o))); + o_costmap[idx] = static_cast(fused); + } +} + +void applyLOBFLaunch( + const std::uint8_t * z_costmap, const std::uint8_t no_information_value, const int num_elements, + std::uint8_t * o_costmap, cudaStream_t stream) +{ + const int threads_per_block = 256; + const int num_blocks = (num_elements + threads_per_block - 1) / threads_per_block; + applyLOBFKernel<<>>( + z_costmap, no_information_value, num_elements, o_costmap); +} + +} // namespace costmap_2d +} // namespace autoware::occupancy_grid_map + +#endif // AUTOWARE__probabilistic_occupancy_grid_map__UPDATER__log_odds_bayes_filter_updater_kernel_HPP_ diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp index 9d190664b8858..6ed51f9fccebc 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -47,17 +47,29 @@ bool transformPointcloud( return true; } -// used in pointcloud based occupancy grid map -void transformPointcloud( - const sensor_msgs::msg::PointCloud2 & input, const geometry_msgs::msg::Pose & pose, - sensor_msgs::msg::PointCloud2 & output) +bool transformPointcloudAsync( + CudaPointCloud2 & input, const tf2_ros::Buffer & tf2, const std::string & target_frame) { - const auto transform = autoware::universe_utils::pose2transform(pose); - Eigen::Matrix4f tf_matrix = tf2::transformToEigen(transform).matrix().cast(); - - pcl_ros::transformPointCloud(tf_matrix, input, output); - output.header.stamp = input.header.stamp; - output.header.frame_id = ""; + geometry_msgs::msg::TransformStamped tf_stamped; + // lookup transform + try { + tf_stamped = tf2.lookupTransform( + target_frame, input.header.frame_id, input.header.stamp, rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN( + rclcpp::get_logger("probabilistic_occupancy_grid_map"), "Failed to lookup transform: %s", + ex.what()); + return false; + } + // transform pointcloud + Eigen::Matrix4f tf_matrix = tf2::transformToEigen(tf_stamped.transform).matrix().cast(); + Eigen::Matrix3f rotation = tf_matrix.block<3, 3>(0, 0); + Eigen::Vector3f translation = tf_matrix.block<3, 1>(0, 3); + transformPointCloudLaunch( + input.data.get(), input.width * input.height, input.point_step, rotation, translation, + input.stream); + input.header.frame_id = target_frame; + return true; } Eigen::Matrix4f getTransformMatrix(const geometry_msgs::msg::Pose & pose) @@ -145,51 +157,5 @@ geometry_msgs::msg::Pose getInversePose(const geometry_msgs::msg::Pose & pose) return inv_pose; } -/** - * @brief extract Common Pointcloud between obstacle pc and raw pc - * @param obstacle_pc - * @param raw_pc - * @param output_obstacle_pc - */ -bool extractCommonPointCloud( - const sensor_msgs::msg::PointCloud2 & obstacle_pc, const sensor_msgs::msg::PointCloud2 & raw_pc, - sensor_msgs::msg::PointCloud2 & output_obstacle_pc) -{ - // Convert to vector of 3d points - std::vector v_obstacle_pc, v_raw_pc, v_output_obstacle_pc; - for (sensor_msgs::PointCloud2ConstIterator iter_x(obstacle_pc, "x"), - iter_y(obstacle_pc, "y"), iter_z(obstacle_pc, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - v_obstacle_pc.push_back(MyPoint3d(*iter_x, *iter_y, *iter_z)); - } - for (sensor_msgs::PointCloud2ConstIterator iter_x(raw_pc, "x"), iter_y(raw_pc, "y"), - iter_z(raw_pc, "z"); - iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - v_raw_pc.push_back(MyPoint3d(*iter_x, *iter_y, *iter_z)); - } - - // sort pointclouds for searching cross points: O(nlogn) - std::sort(v_obstacle_pc.begin(), v_obstacle_pc.end(), [](auto a, auto b) { return a < b; }); - std::sort(v_raw_pc.begin(), v_raw_pc.end(), [](auto a, auto b) { return a < b; }); - - // calc intersection points of two pointclouds: O(n) - std::set_intersection( - v_obstacle_pc.begin(), v_obstacle_pc.end(), v_raw_pc.begin(), v_raw_pc.end(), - std::back_inserter(v_output_obstacle_pc)); - if (v_output_obstacle_pc.size() == 0) { - return false; - } - - // Convert to ros msg - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - for (auto p : v_output_obstacle_pc) { - pcl_output->push_back(pcl::PointXYZ(p.x, p.y, p.z)); - } - pcl::toROSMsg(*pcl_output, output_obstacle_pc); - output_obstacle_pc.header = obstacle_pc.header; - - return true; -} - } // namespace utils } // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils_kernel.cu b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils_kernel.cu new file mode 100644 index 0000000000000..696466fa89fbe --- /dev/null +++ b/perception/autoware_probabilistic_occupancy_grid_map/lib/utils/utils_kernel.cu @@ -0,0 +1,300 @@ +// 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. +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + *********************************************************************/ + +#include "autoware/probabilistic_occupancy_grid_map/utils/utils_kernel.hpp" + +namespace autoware::occupancy_grid_map +{ +namespace utils +{ + +inline __device__ bool worldToMap( + float wx, float wy, unsigned int & mx, unsigned int & my, float origin_x, float origin_y, + float resolution_inv, int size_x, int size_y) +{ + if (wx < origin_x || wy < origin_y) { + return false; + } + + mx = static_cast(std::floor((wx - origin_x) * resolution_inv)); + my = static_cast(std::floor((wy - origin_y) * resolution_inv)); + + if (mx < size_x && my < size_y) { + return true; + } + + return false; +} + +__device__ void setCellValue( + float wx, float wy, float origin_x, float origin_y, float resolution_inv, int size_x, int size_y, + std::uint8_t value, std::uint8_t * costmap_tensor) +{ + unsigned int mx, my; + if (!worldToMap(wx, wy, mx, my, origin_x, origin_y, resolution_inv, size_x, size_y)) { + return; + } + + costmap_tensor[my * size_x + mx] = value; +} + +inline __device__ void bresenham2D( + unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, + unsigned int offset, unsigned int max_length, std::uint8_t cost, std::uint8_t * costmap_tensor) +{ + unsigned int end = min(max_length, abs_da); + for (unsigned int i = 0; i < end; ++i) { + costmap_tensor[offset] = cost; + offset += offset_a; + error_b += abs_db; + if ((unsigned int)error_b >= abs_da) { + offset += offset_b; + error_b -= abs_da; + } + } + costmap_tensor[offset] = cost; +} + +/** + * @brief Raytrace a line and apply some action at each step + * @param at The action to take... a functor + * @param x0 The starting x coordinate + * @param y0 The starting y coordinate + * @param x1 The ending x coordinate + * @param y1 The ending y coordinate + * @param max_length The maximum desired length of the segment... + * allows you to not go all the way to the endpoint + * @param min_length The minimum desired length of the segment + */ +inline __device__ void raytraceLine( + unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length, + unsigned int min_length, unsigned int size_x, std::uint8_t cost, std::uint8_t * costmap_tensor) +{ + int dx_full = x1 - x0; + int dy_full = y1 - y0; + + // we need to chose how much to scale our dominant dimension, + // based on the maximum length of the line + float dist = sqrt((float)(dx_full * dx_full + dy_full * dy_full)); // hypot(dx_full, dy_full); + if (dist < min_length) { + return; + } + + unsigned int min_x0, min_y0; + if (dist > 0.0) { + // Adjust starting point and offset to start from min_length distance + min_x0 = (unsigned int)(x0 + dx_full / dist * min_length); + min_y0 = (unsigned int)(y0 + dy_full / dist * min_length); + } else { + // dist can be 0 if [x0, y0]==[x1, y1]. + // In this case only this cell should be processed. + min_x0 = x0; + min_y0 = y0; + } + unsigned int offset = min_y0 * size_x + min_x0; + + int dx = x1 - min_x0; + int dy = y1 - min_y0; + + unsigned int abs_dx = abs(dx); + unsigned int abs_dy = abs(dy); + + int offset_dx = dx > 0 ? 1 : -1; // sign(dx); + int offset_dy = dy > 0 ? size_x : -size_x; // sign(dy) * size_x; + + constexpr float epsilon = 1e-6; + float scale = (dist < epsilon) ? 1.0 : min(1.f, max_length / dist); + // if x is dominant + if (abs_dx >= abs_dy) { + int error_y = abs_dx / 2; + + bresenham2D( + abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx), cost, + costmap_tensor); + return; + } + + // otherwise y is dominant + int error_x = abs_dy / 2; + + bresenham2D( + abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy), cost, + costmap_tensor); +} + +void __device__ raytrace( + const float source_x, const float source_y, const float target_x, const float target_y, + const float origin_x, float origin_y, const float resolution_inv, const int size_x, + const int size_y, const std::uint8_t cost, std::uint8_t * costmap_tensor) +{ + unsigned int x0{}; + unsigned int y0{}; + const float ox{source_x}; + const float oy{source_y}; + + if (!worldToMap(ox, oy, x0, y0, origin_x, origin_y, resolution_inv, size_x, size_y)) { + return; + } + + // we can pre-compute the endpoints of the map outside of the inner loop... we'll need these later + const float resolution = 1.0 / resolution_inv; + const float map_end_x = origin_x + size_x * resolution; + const float map_end_y = origin_y + size_y * resolution; + + float wx = target_x; + float wy = target_y; + + // now we also need to make sure that the endpoint we're ray-tracing + // to isn't off the costmap and scale if necessary + const float a = wx - ox; + const float b = wy - oy; + + // the minimum value to raytrace from is the origin + if (wx < origin_x) { + const float t = (origin_x - ox) / a; + wx = origin_x; + wy = oy + b * t; + } + if (wy < origin_y) { + const float t = (origin_y - oy) / b; + wx = ox + a * t; + wy = origin_y; + } + + // the maximum value to raytrace to is the end of the map + if (wx > map_end_x) { + const float t = (map_end_x - ox) / a; + wx = map_end_x - .001; + wy = oy + b * t; + } + if (wy > map_end_y) { + const float t = (map_end_y - oy) / b; + wx = ox + a * t; + wy = map_end_y - .001; + } + + // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint + unsigned int x1{}; + unsigned int y1{}; + + // check for legality just in case + if (!worldToMap(wx, wy, x1, y1, origin_x, origin_y, resolution_inv, size_x, size_y)) { + return; + } + + constexpr unsigned int cell_raytrace_range = 10000; // large number to ignore range threshold + raytraceLine(x0, y0, x1, y1, cell_raytrace_range, 0, size_x, cost, costmap_tensor); +} + +__global__ void transformPointCloudKernel( + std::uint8_t * points, std::size_t num_points, std::size_t points_step, + const Eigen::Matrix3f & rotation, const Eigen::Vector3f & translation) +{ + int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= num_points) { + return; + } + + Eigen::Map point_map(reinterpret_cast(points + idx * points_step)); + point_map = rotation * point_map + translation; +} + +__global__ void copyMapRegionKernel( + const std::uint8_t * source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, + unsigned int sm_size_x, unsigned int sm_size_y, std::uint8_t * dest_map, + unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, + unsigned int dm_size_y, unsigned int region_size_x, unsigned int region_size_y) +{ + const int idx = blockIdx.x * blockDim.x + threadIdx.x; + if (idx >= region_size_x * region_size_y) return; + + const int region_y = idx / region_size_x; + const int region_x = idx % region_size_x; + + const int sm_index = + sm_lower_left_y * sm_size_x + sm_lower_left_x + region_y * sm_size_x + region_x; + const int dm_index = + dm_lower_left_y * dm_size_x + dm_lower_left_x + region_y * dm_size_x + region_x; + + if (sm_index < sm_size_x * sm_size_y && dm_index < dm_size_x * dm_size_y) { + dest_map[dm_index] = source_map[sm_index]; + } +} + +void transformPointCloudLaunch( + std::uint8_t * points, std::size_t num_points, std::size_t points_step, + const Eigen::Matrix3f & rotation, const Eigen::Vector3f & translation, cudaStream_t stream) +{ + // Launch kernel + int threads_per_block = 256; + int num_blocks = (num_points + threads_per_block - 1) / threads_per_block; + transformPointCloudKernel<<>>( + points, num_points, points_step, rotation, translation); +} + +void copyMapRegionLaunch( + const std::uint8_t * source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, + unsigned int sm_size_x, unsigned int sm_size_y, std::uint8_t * dest_map, + unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x, + unsigned int dm_size_y, unsigned int region_size_x, unsigned int region_size_y, + cudaStream_t stream) +{ + const int threads_per_block = 256; + const int num_blocks = + (region_size_x * region_size_y + threads_per_block - 1) / threads_per_block; + + copyMapRegionKernel<<>>( + source_map, sm_lower_left_x, sm_lower_left_y, sm_size_x, sm_size_y, dest_map, dm_lower_left_x, + dm_lower_left_y, dm_size_x, dm_size_y, region_size_x, region_size_y); +} + +} // namespace utils +} // namespace autoware::occupancy_grid_map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/package.xml b/perception/autoware_probabilistic_occupancy_grid_map/package.xml index ece7ef0dec663..b0e6463519fd9 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/package.xml +++ b/perception/autoware_probabilistic_occupancy_grid_map/package.xml @@ -2,11 +2,12 @@ autoware_probabilistic_occupancy_grid_map - 0.40.0 + 0.41.0 generate probabilistic occupancy grid map Yukihiro Saito Yoshi Ri Mamoru Sobue + Kenzo Lobos-Tsunekawa Apache License 2.0 Yukihiro Saito @@ -15,6 +16,7 @@ autoware_cmake eigen3_cmake_module + autoware_cuda_utils autoware_universe_utils grid_map_costmap_2d grid_map_msgs diff --git a/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json b/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json index 14f4305f55de8..de7c1e194ed42 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json +++ b/perception/autoware_probabilistic_occupancy_grid_map/schema/synchronized_grid_map_fusion_node.schema.json @@ -2,104 +2,118 @@ "$schema": "http://json-schema.org/draft-07/schema#", "title": "Parameters for Synchronized Grid Map Fusion Node", "type": "object", + "definitions": { + "synchronized_grid_map_fusion": { + "type": "object", + "properties": { + "fusion_input_ogm_topics": { + "type": "array", + "description": "List of fusion input occupancy grid map topics.", + "items": { + "type": "string" + }, + "default": ["topic1", "topic2"] + }, + "input_ogm_reliabilities": { + "type": "array", + "description": "Reliability of each sensor for fusion.", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + }, + "default": [0.8, 0.2] + }, + "fusion_method": { + "type": "string", + "description": "Method for occupancy grid map fusion.", + "enum": ["overwrite", "log-odds", "dempster-shafer"], + "default": "overwrite" + }, + "match_threshold_sec": { + "type": "number", + "description": "Time threshold for matching in seconds.", + "default": 0.01 + }, + "timeout_sec": { + "type": "number", + "description": "Timeout for synchronization in seconds.", + "default": 0.1 + }, + "input_offset_sec": { + "type": "array", + "description": "Offset for each input in seconds.", + "items": { + "type": "number" + }, + "default": [0.0, 0.0] + }, + "map_frame_": { + "type": "string", + "description": "The frame ID of the map.", + "default": "map" + }, + "base_link_frame_": { + "type": "string", + "description": "The frame ID of the base link.", + "default": "base_link" + }, + "grid_map_origin_frame_": { + "type": "string", + "description": "The frame ID of the grid map origin.", + "default": "base_link" + }, + "fusion_map_length_x": { + "type": "number", + "description": "The length of the fusion map in the x direction.", + "default": 100.0 + }, + "fusion_map_length_y": { + "type": "number", + "description": "The length of the fusion map in the y direction.", + "default": 100.0 + }, + "fusion_map_resolution": { + "type": "number", + "description": "The resolution of the fusion map.", + "default": 0.5 + }, + "publish_processing_time_detail": { + "type": "boolean", + "description": "True for showing detail of publish processing time.", + "default": false + } + }, + "required": [ + "fusion_input_ogm_topics", + "input_ogm_reliabilities", + "fusion_method", + "match_threshold_sec", + "timeout_sec", + "input_offset_sec", + "map_frame_", + "base_link_frame_", + "grid_map_origin_frame_", + "fusion_map_length_x", + "fusion_map_length_y", + "fusion_map_resolution", + "publish_processing_time_detail" + ], + "additionalProperties": false + } + }, "properties": { "/**": { "type": "object", "properties": { "ros__parameters": { - "type": "object", - "properties": { - "fusion_input_ogm_topics": { - "type": "array", - "description": "List of fusion input occupancy grid map topics.", - "items": { - "type": "string" - }, - "default": ["topic1", "topic2"] - }, - "input_ogm_reliabilities": { - "type": "array", - "description": "Reliability of each sensor for fusion.", - "items": { - "type": "number", - "minimum": 0.0, - "maximum": 1.0 - }, - "default": [0.8, 0.2] - }, - "fusion_method": { - "type": "string", - "description": "Method for occupancy grid map fusion.", - "enum": ["overwrite", "log-odds", "dempster-shafer"], - "default": "overwrite" - }, - "match_threshold_sec": { - "type": "number", - "description": "Time threshold for matching in seconds.", - "default": 0.01 - }, - "timeout_sec": { - "type": "number", - "description": "Timeout for synchronization in seconds.", - "default": 0.1 - }, - "input_offset_sec": { - "type": "array", - "description": "Offset for each input in seconds.", - "items": { - "type": "number" - }, - "default": [0.0, 0.0] - }, - "map_frame_": { - "type": "string", - "description": "The frame ID of the map.", - "default": "map" - }, - "base_link_frame_": { - "type": "string", - "description": "The frame ID of the base link.", - "default": "base_link" - }, - "grid_map_origin_frame_": { - "type": "string", - "description": "The frame ID of the grid map origin.", - "default": "base_link" - }, - "fusion_map_length_x": { - "type": "number", - "description": "The length of the fusion map in the x direction.", - "default": 100.0 - }, - "fusion_map_length_y": { - "type": "number", - "description": "The length of the fusion map in the y direction.", - "default": 100.0 - }, - "fusion_map_resolution": { - "type": "number", - "description": "The resolution of the fusion map.", - "default": 0.5 - } - }, - "required": [ - "fusion_input_ogm_topics", - "input_ogm_reliabilities", - "fusion_method", - "match_threshold_sec", - "timeout_sec", - "input_offset_sec", - "map_frame_", - "base_link_frame_", - "grid_map_origin_frame_", - "fusion_map_length_x", - "fusion_map_length_y", - "fusion_map_resolution" - ] + "$ref": "#/definitions/synchronized_grid_map_fusion" } }, - "required": ["ros__parameters"] + "required": ["ros__parameters"], + "additionalProperties": false } }, - "required": ["/**"] + "required": ["/**"], + "additionalProperties": false } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp index 880dfda03c495..cf1d828493035 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 TIER IV, Inc. +// 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. @@ -115,8 +115,8 @@ GridMapFusionNode::GridMapFusionNode(const rclcpp::NodeOptions & node_options) // updater occupancy_grid_map_updater_ptr_ = std::make_shared( - fusion_map_length_x_ / fusion_map_resolution_, fusion_map_length_y_ / fusion_map_resolution_, - fusion_map_resolution_); + false, fusion_map_length_x_ / fusion_map_resolution_, + fusion_map_length_y_ / fusion_map_resolution_, fusion_map_resolution_); // Set timer const auto period_ns = std::chrono::duration_cast( @@ -309,7 +309,7 @@ void GridMapFusionNode::publish() // fusion map // single frame gridmap fusion - auto fused_map = SingleFrameOccupancyFusion(subscribed_maps, latest_stamp, weights); + auto & fused_map = SingleFrameOccupancyFusion(subscribed_maps, latest_stamp, weights); // multi frame gridmap fusion occupancy_grid_map_updater_ptr_->update(fused_map); @@ -332,16 +332,16 @@ void GridMapFusionNode::publish() std::chrono::duration( std::chrono::nanoseconds((this->get_clock()->now() - latest_stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } -OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( +const OccupancyGridMapFixedBlindSpot & GridMapFusionNode::SingleFrameOccupancyFusion( std::vector & occupancy_grid_maps, const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights) { @@ -367,22 +367,30 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( if (time_keeper_) inner_st_ptr = std::make_unique("create_fused_map", *time_keeper_); + if ( + !fused_map_ || fused_map_->getSizeInCellsX() != occupancy_grid_maps[0].getSizeInCellsX() || + fused_map_->getSizeInCellsY() != occupancy_grid_maps[0].getSizeInCellsY() || + fused_map_->getResolution() != occupancy_grid_maps[0].getResolution()) { + fused_map_ = std::make_unique( + false, occupancy_grid_maps[0].getSizeInCellsX(), occupancy_grid_maps[0].getSizeInCellsY(), + occupancy_grid_maps[0].getResolution()); + } + // init fused map with calculated origin - OccupancyGridMapFixedBlindSpot fused_map( - occupancy_grid_maps[0].getSizeInCellsX(), occupancy_grid_maps[0].getSizeInCellsY(), - occupancy_grid_maps[0].getResolution()); - fused_map.updateOrigin( - gridmap_origin.position.x - fused_map.getSizeInMetersX() / 2, - gridmap_origin.position.y - fused_map.getSizeInMetersY() / 2); + fused_map_->resetMaps(); + + fused_map_->updateOrigin( + gridmap_origin.position.x - fused_map_->getSizeInMetersX() / 2, + gridmap_origin.position.y - fused_map_->getSizeInMetersY() / 2); // fix origin of each map for (auto & map : occupancy_grid_maps) { - map.updateOrigin(fused_map.getOriginX(), fused_map.getOriginY()); + map.updateOrigin(fused_map_->getOriginX(), fused_map_->getOriginY()); } // assume map is same size and resolutions - for (unsigned int x = 0; x < fused_map.getSizeInCellsX(); x++) { - for (unsigned int y = 0; y < fused_map.getSizeInCellsY(); y++) { + for (unsigned int x = 0; x < fused_map_->getSizeInCellsX(); x++) { + for (unsigned int y = 0; y < fused_map_->getSizeInCellsY(); y++) { // get cost of each map std::vector costs; for (auto & map : occupancy_grid_maps) { @@ -393,11 +401,11 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::SingleFrameOccupancyFusion( auto fused_cost = fusion_policy::singleFrameOccupancyFusion(costs, fusion_method_, weights); // set max cost to fused map - fused_map.setCost(x, y, fused_cost); + fused_map_->setCost(x, y, fused_cost); } } - return fused_map; + return *fused_map_; } // scope for time keeper ends } @@ -408,7 +416,7 @@ OccupancyGridMapFixedBlindSpot GridMapFusionNode::OccupancyGridMsgToGridMap( if (time_keeper_) st_ptr = std::make_unique(__func__, *time_keeper_); OccupancyGridMapFixedBlindSpot gridmap( - occupancy_grid_map.info.width, occupancy_grid_map.info.height, + false, occupancy_grid_map.info.width, occupancy_grid_map.info.height, occupancy_grid_map.info.resolution); gridmap.updateOrigin( occupancy_grid_map.info.origin.position.x, occupancy_grid_map.info.origin.position.y); diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp index 0f9475e476921..0d894d99a68a1 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/fusion/synchronized_grid_map_fusion_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -69,7 +69,7 @@ class GridMapFusionNode : public rclcpp::Node OccupancyGridMapFixedBlindSpot OccupancyGridMsgToGridMap( const nav_msgs::msg::OccupancyGrid & occupancy_grid_map); - OccupancyGridMapFixedBlindSpot SingleFrameOccupancyFusion( + const OccupancyGridMapFixedBlindSpot & SingleFrameOccupancyFusion( std::vector & occupancy_grid_maps, const builtin_interfaces::msg::Time latest_stamp, const std::vector & weights); @@ -103,6 +103,7 @@ class GridMapFusionNode : public rclcpp::Node // cache for fusion std::mutex mutex_; + std::unique_ptr fused_map_; std::shared_ptr occupancy_grid_map_updater_ptr_; // contains fused grid map std::map diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index cb1b22a519e2d..4a8c9626de013 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -95,14 +95,14 @@ LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode( const std::string updater_type = this->declare_parameter("updater_type"); if (updater_type == "binary_bayes_filter") { occupancy_grid_map_updater_ptr_ = std::make_shared( - map_length / map_resolution, map_width / map_resolution, map_resolution); + false, map_length / map_resolution, map_width / map_resolution, map_resolution); } else { RCLCPP_WARN( get_logger(), "specified occupancy grid map updater type [%s] is not found, use binary_bayes_filter", updater_type.c_str()); occupancy_grid_map_updater_ptr_ = std::make_shared( - map_length / map_resolution, map_width / map_resolution, map_resolution); + false, map_length / map_resolution, map_width / map_resolution, map_resolution); } occupancy_grid_map_updater_ptr_->initRosParam(*this); diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index 4d442ad2c33db..ba24e29284825 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -85,26 +85,27 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( const std::string updater_type = this->declare_parameter("updater_type"); if (updater_type == "binary_bayes_filter") { occupancy_grid_map_updater_ptr_ = std::make_unique( - map_length / map_resolution, map_length / map_resolution, map_resolution); + true, map_length / map_resolution, map_length / map_resolution, map_resolution); } else { RCLCPP_WARN( get_logger(), "specified occupancy grid map updater type [%s] is not found, use binary_bayes_filter", updater_type.c_str()); occupancy_grid_map_updater_ptr_ = std::make_unique( - map_length / map_resolution, map_length / map_resolution, map_resolution); + true, map_length / map_resolution, map_length / map_resolution, map_resolution); } occupancy_grid_map_updater_ptr_->initRosParam(*this); const std::string grid_map_type = this->declare_parameter("grid_map_type"); + if (grid_map_type == "OccupancyGridMapProjectiveBlindSpot") { occupancy_grid_map_ptr_ = std::make_unique( - occupancy_grid_map_updater_ptr_->getSizeInCellsX(), + true, occupancy_grid_map_updater_ptr_->getSizeInCellsX(), occupancy_grid_map_updater_ptr_->getSizeInCellsY(), occupancy_grid_map_updater_ptr_->getResolution()); } else if (grid_map_type == "OccupancyGridMapFixedBlindSpot") { occupancy_grid_map_ptr_ = std::make_unique( - occupancy_grid_map_updater_ptr_->getSizeInCellsX(), + true, occupancy_grid_map_updater_ptr_->getSizeInCellsX(), occupancy_grid_map_updater_ptr_->getSizeInCellsY(), occupancy_grid_map_updater_ptr_->getResolution()); } else { @@ -113,7 +114,7 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( "specified occupancy grid map type [%s] is not found, use OccupancyGridMapFixedBlindSpot", grid_map_type.c_str()); occupancy_grid_map_ptr_ = std::make_unique( - occupancy_grid_map_updater_ptr_->getSizeInCellsX(), + true, occupancy_grid_map_updater_ptr_->getSizeInCellsX(), occupancy_grid_map_updater_ptr_->getSizeInCellsY(), occupancy_grid_map_updater_ptr_->getResolution()); } @@ -139,6 +140,9 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( time_keeper_ = std::make_shared(time_keeper); } } + + cudaStreamCreate(&raw_pointcloud_.stream); + cudaStreamCreate(&obstacle_pointcloud_.stream); } void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( @@ -151,30 +155,27 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( if (stop_watch_ptr_) { stop_watch_ptr_->toc("processing_time", true); } - // if scan_origin_frame_ is "", replace it with input_raw_msg->header.frame_id + + raw_pointcloud_.fromROSMsgAsync(*input_raw_msg); + obstacle_pointcloud_.fromROSMsgAsync(*input_obstacle_msg); + + // if scan_origin_frame_ is "", replace it with raw_pointcloud_.header.frame_id if (scan_origin_frame_.empty()) { - scan_origin_frame_ = input_raw_msg->header.frame_id; + scan_origin_frame_ = raw_pointcloud_.header.frame_id; } - PointCloud2 trans_input_raw{}, trans_input_obstacle{}; - bool is_raw_transformed = false; - bool is_obstacle_transformed = false; - // Prepare for applying height filter if (use_height_filter_) { // Make sure that the frame is base_link - if (input_raw_msg->header.frame_id != base_link_frame_) { - if (!utils::transformPointcloud(*input_raw_msg, *tf2_, base_link_frame_, trans_input_raw)) { + if (raw_pointcloud_.header.frame_id != base_link_frame_) { + if (!utils::transformPointcloudAsync(raw_pointcloud_, *tf2_, base_link_frame_)) { return; } - is_raw_transformed = true; } if (input_obstacle_msg->header.frame_id != base_link_frame_) { - if (!utils::transformPointcloud( - *input_obstacle_msg, *tf2_, base_link_frame_, trans_input_obstacle)) { + if (!utils::transformPointcloudAsync(obstacle_pointcloud_, *tf2_, base_link_frame_)) { return; } - is_obstacle_transformed = true; } occupancy_grid_map_ptr_->setHeightLimit(min_height_, max_height_); } else { @@ -182,32 +183,16 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( -std::numeric_limits::infinity(), std::numeric_limits::infinity()); } - const PointCloud2::ConstSharedPtr input_raw_use = - is_raw_transformed ? std::make_shared(trans_input_raw) : input_raw_msg; - const PointCloud2::ConstSharedPtr input_obstacle_use = - is_obstacle_transformed ? std::make_shared(trans_input_obstacle) - : input_obstacle_msg; - - // Filter obstacle pointcloud by raw pointcloud - PointCloud2 input_obstacle_pc_common{}; - bool use_input_obstacle_pc_common = false; - if (filter_obstacle_pointcloud_by_raw_pointcloud_) { - if (utils::extractCommonPointCloud( - *input_obstacle_use, *input_raw_use, input_obstacle_pc_common)) { - use_input_obstacle_pc_common = true; - } - } - // Get from map to sensor frame pose Pose robot_pose{}; Pose gridmap_origin{}; Pose scan_origin{}; try { - robot_pose = utils::getPose(input_raw_msg->header.stamp, *tf2_, base_link_frame_, map_frame_); + robot_pose = utils::getPose(raw_pointcloud_.header.stamp, *tf2_, base_link_frame_, map_frame_); gridmap_origin = - utils::getPose(input_raw_msg->header.stamp, *tf2_, gridmap_origin_frame_, map_frame_); + utils::getPose(raw_pointcloud_.header.stamp, *tf2_, gridmap_origin_frame_, map_frame_); scan_origin = - utils::getPose(input_raw_msg->header.stamp, *tf2_, scan_origin_frame_, map_frame_); + utils::getPose(raw_pointcloud_.header.stamp, *tf2_, scan_origin_frame_, map_frame_); } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM(get_logger(), ex.what()); return; @@ -224,9 +209,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( gridmap_origin.position.x - occupancy_grid_map_ptr_->getSizeInMetersX() / 2, gridmap_origin.position.y - occupancy_grid_map_ptr_->getSizeInMetersY() / 2); occupancy_grid_map_ptr_->updateWithPointCloud( - *input_raw_use, - (use_input_obstacle_pc_common ? input_obstacle_pc_common : *input_obstacle_use), robot_pose, - scan_origin); + raw_pointcloud_, obstacle_pointcloud_, robot_pose, scan_origin); } if (enable_single_frame_mode_) { @@ -234,6 +217,8 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( if (time_keeper_) inner_st_ptr = std::make_unique("publish_occupancy_grid_map", *time_keeper_); + occupancy_grid_map_ptr_->copyDeviceCostmapToHost(); + // publish occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( map_frame_, input_raw_msg->header.stamp, robot_pose.position.z, @@ -261,11 +246,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( std::chrono::nanoseconds( (this->get_clock()->now() - input_raw_msg->header.stamp).nanoseconds())) .count(); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 34afd5edc7c37..c985414ffdf6b 100644 --- a/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/autoware_probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// 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. @@ -18,6 +18,7 @@ #include "autoware/probabilistic_occupancy_grid_map/costmap_2d/occupancy_grid_map_base.hpp" #include "autoware/probabilistic_occupancy_grid_map/updater/binary_bayes_filter_updater.hpp" #include "autoware/probabilistic_occupancy_grid_map/updater/ogm_updater_interface.hpp" +#include "autoware/probabilistic_occupancy_grid_map/utils/cuda_pointcloud.hpp" #include #include @@ -82,6 +83,9 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node std::unique_ptr occupancy_grid_map_ptr_; std::unique_ptr occupancy_grid_map_updater_ptr_; + CudaPointCloud2 raw_pointcloud_; + CudaPointCloud2 obstacle_pointcloud_; + // ROS Parameters std::string map_frame_; std::string base_link_frame_; diff --git a/perception/autoware_radar_crossing_objects_noise_filter/CHANGELOG.rst b/perception/autoware_radar_crossing_objects_noise_filter/CHANGELOG.rst index ff859a4d44ce3..fded0cf83bf65 100644 --- a/perception/autoware_radar_crossing_objects_noise_filter/CHANGELOG.rst +++ b/perception/autoware_radar_crossing_objects_noise_filter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_radar_crossing_objects_noise_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_radar_crossing_objects_noise_filter/package.xml b/perception/autoware_radar_crossing_objects_noise_filter/package.xml index f5900066cb10b..04aa0db6938c3 100644 --- a/perception/autoware_radar_crossing_objects_noise_filter/package.xml +++ b/perception/autoware_radar_crossing_objects_noise_filter/package.xml @@ -2,7 +2,7 @@ autoware_radar_crossing_objects_noise_filter - 0.40.0 + 0.41.0 autoware_radar_crossing_objects_noise_filter Sathshi Tanaka Shunsuke Miura diff --git a/perception/autoware_radar_fusion_to_detected_object/CHANGELOG.rst b/perception/autoware_radar_fusion_to_detected_object/CHANGELOG.rst index f6f48dfe36231..1a78bb1f0f199 100644 --- a/perception/autoware_radar_fusion_to_detected_object/CHANGELOG.rst +++ b/perception/autoware_radar_fusion_to_detected_object/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_radar_fusion_to_detected_object ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_radar_fusion_to_detected_object): fix bugprone-errors (`#9654 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/perception/autoware_radar_fusion_to_detected_object/package.xml b/perception/autoware_radar_fusion_to_detected_object/package.xml index 20d6331dc8a37..e16b4a5cdc2a0 100644 --- a/perception/autoware_radar_fusion_to_detected_object/package.xml +++ b/perception/autoware_radar_fusion_to_detected_object/package.xml @@ -2,7 +2,7 @@ autoware_radar_fusion_to_detected_object - 0.40.0 + 0.41.0 autoware_radar_fusion_to_detected_object Satoshi Tanaka Shunsuke Miura diff --git a/perception/autoware_radar_fusion_to_detected_object/src/node.cpp b/perception/autoware_radar_fusion_to_detected_object/src/node.cpp index 58e893032340c..fcebbbc905425 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/node.cpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/node.cpp @@ -25,7 +25,6 @@ using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -using std::placeholders::_1; namespace { @@ -58,7 +57,7 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( { // Parameter Server set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&RadarObjectFusionToDetectedObjectNode::onSetParam, this, _1)); + std::bind(&RadarObjectFusionToDetectedObjectNode::onSetParam, this, std::placeholders::_1)); // Node Parameter node_param_.update_rate_hz = declare_parameter("node_params.update_rate_hz"); @@ -93,11 +92,10 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( sub_object_.subscribe(this, "~/input/objects", rclcpp::QoS{1}.get_rmw_qos_profile()); sub_radar_.subscribe(this, "~/input/radars", rclcpp::QoS{1}.get_rmw_qos_profile()); - using std::placeholders::_1; - using std::placeholders::_2; sync_ptr_ = std::make_shared(SyncPolicy(20), sub_object_, sub_radar_); - sync_ptr_->registerCallback( - std::bind(&RadarObjectFusionToDetectedObjectNode::onData, this, _1, _2)); + sync_ptr_->registerCallback(std::bind( + &RadarObjectFusionToDetectedObjectNode::onData, this, std::placeholders::_1, + std::placeholders::_2)); // Publisher pub_objects_ = create_publisher("~/output/objects", 1); diff --git a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 98f429292ec73..6d19f299973bd 100644 --- a/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/autoware_radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -158,9 +158,7 @@ bool RadarFusionToDetectedObject::isYawCorrect( const double object_yaw = autoware::universe_utils::normalizeRadian( tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation)); const double diff_yaw = autoware::universe_utils::normalizeRadian(twist_yaw - object_yaw); - if (std::abs(diff_yaw) < yaw_threshold) { - return true; - } else if (M_PI - yaw_threshold < std::abs(diff_yaw)) { + if (std::abs(diff_yaw) < yaw_threshold || M_PI - yaw_threshold < std::abs(diff_yaw)) { return true; } else { return false; diff --git a/perception/autoware_radar_object_clustering/CHANGELOG.rst b/perception/autoware_radar_object_clustering/CHANGELOG.rst index b1609dd939090..02718bfcf0968 100644 --- a/perception/autoware_radar_object_clustering/CHANGELOG.rst +++ b/perception/autoware_radar_object_clustering/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_radar_object_clustering ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_radar_object_clustering/package.xml b/perception/autoware_radar_object_clustering/package.xml index c8b5278c60292..5ec802cf4225c 100644 --- a/perception/autoware_radar_object_clustering/package.xml +++ b/perception/autoware_radar_object_clustering/package.xml @@ -2,7 +2,7 @@ autoware_radar_object_clustering - 0.40.0 + 0.41.0 autoware_radar_object_clustering Sathshi Tanaka Shunsuke Miura diff --git a/perception/autoware_radar_object_tracker/CHANGELOG.rst b/perception/autoware_radar_object_tracker/CHANGELOG.rst index 028737b0d4482..bce0d26c71f35 100644 --- a/perception/autoware_radar_object_tracker/CHANGELOG.rst +++ b/perception/autoware_radar_object_tracker/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_radar_object_tracker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_radar_object_tracker/package.xml b/perception/autoware_radar_object_tracker/package.xml index 4868e93ba9e72..6fee7b009cb91 100644 --- a/perception/autoware_radar_object_tracker/package.xml +++ b/perception/autoware_radar_object_tracker/package.xml @@ -2,7 +2,7 @@ autoware_radar_object_tracker - 0.40.0 + 0.41.0 Do tracking radar object Yoshi Ri Yukihiro Saito diff --git a/perception/autoware_radar_tracks_msgs_converter/CHANGELOG.rst b/perception/autoware_radar_tracks_msgs_converter/CHANGELOG.rst index 9b60a1b24ec7a..d12916eb23e43 100644 --- a/perception/autoware_radar_tracks_msgs_converter/CHANGELOG.rst +++ b/perception/autoware_radar_tracks_msgs_converter/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_radar_tracks_msgs_converter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_radar_tracks_msgs_converter): fix bugprone-reserved-identifier (`#9658 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/perception/autoware_radar_tracks_msgs_converter/package.xml b/perception/autoware_radar_tracks_msgs_converter/package.xml index 5b2e1c7d6a039..deb438818b6b7 100644 --- a/perception/autoware_radar_tracks_msgs_converter/package.xml +++ b/perception/autoware_radar_tracks_msgs_converter/package.xml @@ -2,7 +2,7 @@ autoware_radar_tracks_msgs_converter - 0.40.0 + 0.41.0 autoware_radar_tracks_msgs_converter Satoshi Tanaka Shunsuke Miura diff --git a/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp index 005138623427e..0e06541f90af1 100644 --- a/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp +++ b/perception/autoware_radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node.cpp @@ -34,7 +34,6 @@ using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -using std::placeholders::_1; namespace { @@ -73,6 +72,7 @@ enum class RadarTrackObjectID { RadarTracksMsgsConverterNode::RadarTracksMsgsConverterNode(const rclcpp::NodeOptions & node_options) : Node("radar_tracks_msgs_converter", node_options) { + using std::placeholders::_1; // Parameter Server set_param_res_ = this->add_on_set_parameters_callback( std::bind(&RadarTracksMsgsConverterNode::onSetParam, this, _1)); diff --git a/perception/autoware_raindrop_cluster_filter/CHANGELOG.rst b/perception/autoware_raindrop_cluster_filter/CHANGELOG.rst index d61173ea1cfea..558501fbc9bf6 100644 --- a/perception/autoware_raindrop_cluster_filter/CHANGELOG.rst +++ b/perception/autoware_raindrop_cluster_filter/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_raindrop_cluster_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9896 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files perception/autoware_raindrop_cluster_filter +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_raindrop_cluster_filter/package.xml b/perception/autoware_raindrop_cluster_filter/package.xml index af3a2b1e9179f..d4d237ff73586 100644 --- a/perception/autoware_raindrop_cluster_filter/package.xml +++ b/perception/autoware_raindrop_cluster_filter/package.xml @@ -2,7 +2,7 @@ autoware_raindrop_cluster_filter - 0.40.0 + 0.41.0 The ROS 2 filter cluster package Yukihiro Saito Dai Nguyen diff --git a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp index e52fc57a5a20c..70ddeadf00b8e 100644 --- a/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp +++ b/perception/autoware_raindrop_cluster_filter/src/low_intensity_cluster_filter_node.cpp @@ -114,9 +114,9 @@ void LowIntensityClusterFilter::objectCallback( if (debug_publisher_ptr_ && stop_watch_ptr_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_ptr_->publish( + debug_publisher_ptr_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/perception/autoware_shape_estimation/CHANGELOG.rst b/perception/autoware_shape_estimation/CHANGELOG.rst index 85af9f1cf54ac..fdd4382218028 100644 --- a/perception/autoware_shape_estimation/CHANGELOG.rst +++ b/perception/autoware_shape_estimation/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package autoware_shape_estimation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_shape_estimation): tier4_debug_msgs chnaged to autoware_internal_debug_msgs in autoware_shape_estimation (`#9897 `_) + feat: tier4_debug_msgs chnaged to autoware_internal_debug_msgs in files perception/autoware_shape_estimation +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* fix(autoware_shape_estimation): fix bugprone-branch-clone (`#9659 `_) + * fix: bugprone-error + * fix: fmt + * fix: pre-commit + * fix: pre-commit + --------- +* Contributors: Amadeusz Szymko, Fumiya Watanabe, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_shape_estimation/CMakeLists.txt b/perception/autoware_shape_estimation/CMakeLists.txt index d0eb2aa5535a8..d7313111e6ade 100644 --- a/perception/autoware_shape_estimation/CMakeLists.txt +++ b/perception/autoware_shape_estimation/CMakeLists.txt @@ -4,6 +4,9 @@ project(autoware_shape_estimation) find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(PCL REQUIRED COMPONENTS common) find_package(pcl_conversions REQUIRED) diff --git a/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp index d39dce65d224d..4328246ff395f 100644 --- a/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp +++ b/perception/autoware_shape_estimation/include/autoware/shape_estimation/tensorrt_shape_estimator.hpp @@ -48,11 +48,7 @@ class TrtShapeEstimator { public: TrtShapeEstimator( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, - const size_t max_workspace_size = (1 << 30), - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0)); + const std::string & model_path, const std::string & precision, const int batch_size); ~TrtShapeEstimator() = default; diff --git a/perception/autoware_shape_estimation/lib/corrector/utils.cpp b/perception/autoware_shape_estimation/lib/corrector/utils.cpp index 5e90c9d54f78a..678eb41c56831 100644 --- a/perception/autoware_shape_estimation/lib/corrector/utils.cpp +++ b/perception/autoware_shape_estimation/lib/corrector/utils.cpp @@ -163,7 +163,7 @@ bool correctWithDefaultValue( (param.min_width < (v_point.at(second_most_distant_index) * 2.0).norm() && (v_point.at(second_most_distant_index) * 2.0).norm() < param.max_width)) // both of edge is within width threshold - { + { // NOLINT correction_vector = v_point.at(first_most_distant_index); if (correction_vector.x() == 0.0) { correction_vector.y() = diff --git a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp index b4b1f18832da7..fa3c3cb09bad3 100644 --- a/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp +++ b/perception/autoware_shape_estimation/lib/tensorrt_shape_estimator.cpp @@ -29,47 +29,41 @@ namespace autoware::shape_estimation { TrtShapeEstimator::TrtShapeEstimator( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size, - const autoware::tensorrt_common::BuildConfig build_config) + const std::string & model_path, const std::string & precision, const int batch_size) { trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); + tensorrt_common::TrtCommonConfig(model_path, precision)); + batch_size_ = batch_size; - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - std::cerr << "Failed to initialize TensorRT" << std::endl; - return; + if (!trt_common_->setup()) { + throw std::runtime_error("Failed to setup TensorRT"); } - const auto pc_input_dims = trt_common_->getBindingDimensions(0); + const auto pc_input_dims = trt_common_->getTensorShape(0); const auto pc_input_size = std::accumulate( pc_input_dims.d + 1, pc_input_dims.d + pc_input_dims.nbDims, 1, std::multiplies()); - input_pc_d_ = autoware::cuda_utils::make_unique(pc_input_size * batch_config[2]); - batch_size_ = batch_config[2]; - const auto one_hot_input_dims = trt_common_->getBindingDimensions(1); + input_pc_d_ = autoware::cuda_utils::make_unique(pc_input_size * batch_size_); + const auto one_hot_input_dims = trt_common_->getTensorShape(1); const auto one_hot_input_size = std::accumulate( one_hot_input_dims.d + 1, one_hot_input_dims.d + one_hot_input_dims.nbDims, 1, std::multiplies()); - input_one_hot_d_ = - autoware::cuda_utils::make_unique(one_hot_input_size * batch_config[2]); + input_one_hot_d_ = autoware::cuda_utils::make_unique(one_hot_input_size * batch_size_); - const auto stage1_center_out_dims = trt_common_->getBindingDimensions(2); + const auto stage1_center_out_dims = trt_common_->getTensorShape(2); out_s1center_elem_num_ = std::accumulate( stage1_center_out_dims.d + 1, stage1_center_out_dims.d + stage1_center_out_dims.nbDims, 1, std::multiplies()); - out_s1center_elem_num_ = out_s1center_elem_num_ * batch_config[2]; - out_s1center_elem_num_per_batch_ = static_cast(out_s1center_elem_num_ / batch_config[2]); + out_s1center_elem_num_ = out_s1center_elem_num_ * batch_size_; + out_s1center_elem_num_per_batch_ = static_cast(out_s1center_elem_num_ / batch_size_); out_s1center_prob_d_ = autoware::cuda_utils::make_unique(out_s1center_elem_num_); out_s1center_prob_h_ = autoware::cuda_utils::make_unique_host(out_s1center_elem_num_, cudaHostAllocPortable); - const auto pred_out_dims = trt_common_->getBindingDimensions(3); + const auto pred_out_dims = trt_common_->getTensorShape(3); out_pred_elem_num_ = std::accumulate( pred_out_dims.d + 1, pred_out_dims.d + pred_out_dims.nbDims, 1, std::multiplies()); - out_pred_elem_num_ = out_pred_elem_num_ * batch_config[2]; - out_pred_elem_num_per_batch_ = static_cast(out_pred_elem_num_ / batch_config[2]); + out_pred_elem_num_ = out_pred_elem_num_ * batch_size_; + out_pred_elem_num_per_batch_ = static_cast(out_pred_elem_num_ / batch_size_); out_pred_prob_d_ = autoware::cuda_utils::make_unique(out_pred_elem_num_); out_pred_prob_h_ = autoware::cuda_utils::make_unique_host(out_pred_elem_num_, cudaHostAllocPortable); @@ -83,10 +77,6 @@ TrtShapeEstimator::TrtShapeEstimator( bool TrtShapeEstimator::inference( const DetectedObjectsWithFeature & input, DetectedObjectsWithFeature & output) { - if (!trt_common_->isInitialized()) { - return false; - } - bool result = false; for (size_t i = 0; i < input.feature_objects.size(); i += batch_size_) { @@ -111,13 +101,13 @@ bool TrtShapeEstimator::inference( void TrtShapeEstimator::preprocess(const DetectedObjectsWithFeature & input) { - auto input_dims_pc = trt_common_->getBindingDimensions(0); + auto input_dims_pc = trt_common_->getTensorShape(0); int batch_size = static_cast(input.feature_objects.size()); const auto input_chan = static_cast(input_dims_pc.d[1]); const auto input_pc_size = static_cast(input_dims_pc.d[2]); - auto input_dims_one_hot = trt_common_->getBindingDimensions(1); + auto input_dims_one_hot = trt_common_->getTensorShape(1); const auto input_one_hot_size = static_cast(input_dims_one_hot.d[1]); int volume_pc = batch_size * input_chan * input_pc_size; @@ -229,7 +219,10 @@ bool TrtShapeEstimator::feed_forward_and_decode( int batch_size = static_cast(input.feature_objects.size()); std::vector buffers = { input_pc_d_.get(), input_one_hot_d_.get(), out_s1center_prob_d_.get(), out_pred_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( out_s1center_prob_h_.get(), out_s1center_prob_d_.get(), out_s1center_elem_num_ * sizeof(float), diff --git a/perception/autoware_shape_estimation/package.xml b/perception/autoware_shape_estimation/package.xml index cfc1d7e829a8a..54fb99ecb4ab6 100644 --- a/perception/autoware_shape_estimation/package.xml +++ b/perception/autoware_shape_estimation/package.xml @@ -2,7 +2,7 @@ autoware_shape_estimation - 0.40.0 + 0.41.0 This package implements a shape estimation algorithm as a ROS 2 node Yukihiro Saito Yoshi Ri diff --git a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp index 67402f68d63ef..28b2a6e398e91 100644 --- a/perception/autoware_shape_estimation/src/shape_estimation_node.cpp +++ b/perception/autoware_shape_estimation/src/shape_estimation_node.cpp @@ -63,9 +63,8 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option min_points_ = declare_parameter("model_params.minimum_points"); std::string precision = declare_parameter("model_params.precision"); int batch_size = declare_parameter("model_params.batch_size"); - autoware::tensorrt_common::BatchConfig batch_config{batch_size, batch_size, batch_size}; tensorrt_shape_estimator_ = - std::make_unique(model_path, precision, batch_config); + std::make_unique(model_path, precision, batch_size); if (this->declare_parameter("model_params.build_only", false)) { RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); @@ -180,9 +179,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Publish pub_->publish(output_msg); published_time_publisher_->publish_if_subscribed(pub_, output_msg.header.stamp); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } } // namespace autoware::shape_estimation diff --git a/perception/autoware_simple_object_merger/CHANGELOG.rst b/perception/autoware_simple_object_merger/CHANGELOG.rst index d45c6a726433d..49953813d302d 100644 --- a/perception/autoware_simple_object_merger/CHANGELOG.rst +++ b/perception/autoware_simple_object_merger/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_simple_object_merger ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/perception/autoware_simple_object_merger/package.xml b/perception/autoware_simple_object_merger/package.xml index db20c03a95640..6398e4e640535 100644 --- a/perception/autoware_simple_object_merger/package.xml +++ b/perception/autoware_simple_object_merger/package.xml @@ -1,7 +1,7 @@ autoware_simple_object_merger - 0.40.0 + 0.41.0 autoware_simple_object_merger Sathshi Tanaka Shunsuke Miura diff --git a/perception/autoware_tensorrt_classifier/CHANGELOG.rst b/perception/autoware_tensorrt_classifier/CHANGELOG.rst index 88311b8ed9515..9cba11673c1b5 100644 --- a/perception/autoware_tensorrt_classifier/CHANGELOG.rst +++ b/perception/autoware_tensorrt_classifier/CHANGELOG.rst @@ -2,6 +2,41 @@ Changelog for package autoware_tensorrt_classifier ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* fix(autoware_tensorrt_classifier): fix bugprone-exception-escape (`#9732 `_) + fix: bugprone-error +* Contributors: Amadeusz Szymko, Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_tensorrt_classifier/CMakeLists.txt b/perception/autoware_tensorrt_classifier/CMakeLists.txt index 88747a1e9240c..59ce7b18afcc3 100644 --- a/perception/autoware_tensorrt_classifier/CMakeLists.txt +++ b/perception/autoware_tensorrt_classifier/CMakeLists.txt @@ -6,6 +6,9 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++17 -O3 -Wno-write-strings -fopen find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(CUDA) find_package(CUDNN) find_package(TENSORRT) diff --git a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp index ee16343b956d1..4f7a5865c72ae 100644 --- a/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp +++ b/perception/autoware_tensorrt_classifier/include/autoware/tensorrt_classifier/tensorrt_classifier.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include @@ -44,22 +45,18 @@ class TrtClassifier * @param[in] mode_path ONNX model_path * @param[in] precision precision for inference * @param[in] calibration_images path for calibration files (only require for quantization) - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine * @param[in] mean mean for preprocess * @param[in] std std for preprocess - * @param[in] buildConfig configuration including precision, calibration method, dla, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder + * @param[in] calib_config calibration configuration * @param[in] cuda whether use cuda gpu for preprocessing */ TrtClassifier( const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, const std::vector & mean = {123.675, 116.28, 103.53}, const std::vector & std = {58.395, 57.12, 57.375}, - const size_t max_workspace_size = (1 << 30), const std::string & calibration_images = "", - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0), + const std::string & calibration_images = "", + const tensorrt_common::CalibrationConfig & calibration_config = + tensorrt_common::CalibrationConfig("MinMax", false, false, 0.0), const bool cuda = false); /** * @brief Deconstruct TrtClassifier @@ -84,6 +81,12 @@ class TrtClassifier */ void initPreprocessBuffer(int width, int height); + /** + * @brief get batch size + * @return batch size + */ + [[nodiscard]] int getBatchSize() const; + private: /** * @brief run preprocess including resizing, letterbox, BGR2RGB, NHWC2NCHW and toFloat on CPU @@ -102,7 +105,7 @@ class TrtClassifier const std::vector & images, std::vector & results, std::vector & probabilities); - std::unique_ptr trt_common_; + std::unique_ptr trt_common_; std::vector input_h_; CudaUniquePtr input_d_; diff --git a/perception/autoware_tensorrt_classifier/package.xml b/perception/autoware_tensorrt_classifier/package.xml index 489f5f2596e25..f752cd723a842 100644 --- a/perception/autoware_tensorrt_classifier/package.xml +++ b/perception/autoware_tensorrt_classifier/package.xml @@ -1,7 +1,7 @@ autoware_tensorrt_classifier - 0.40.0 + 0.41.0 tensorrt classifier wrapper Dan Umeda diff --git a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp index b24e4fe56e8b8..7666ae4f9d068 100644 --- a/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp +++ b/perception/autoware_tensorrt_classifier/src/tensorrt_classifier.cpp @@ -100,11 +100,9 @@ std::vector loadImageList(const std::string & filename, const std:: namespace autoware::tensorrt_classifier { TrtClassifier::TrtClassifier( - const std::string & model_path, const std::string & precision, - const autoware::tensorrt_common::BatchConfig & batch_config, const std::vector & mean, - const std::vector & std, const size_t max_workspace_size, - const std::string & calibration_image_list_path, - autoware::tensorrt_common::BuildConfig build_config, const bool cuda) + const std::string & model_path, const std::string & precision, const std::vector & mean, + const std::vector & std, const std::string & calibration_image_list_path, + const tensorrt_common::CalibrationConfig & calib_config, const bool cuda) { src_width_ = -1; src_height_ = -1; @@ -114,22 +112,36 @@ TrtClassifier::TrtClassifier( for (size_t i = 0; i < inv_std_.size(); i++) { inv_std_[i] = 1.0 / inv_std_[i]; } - batch_size_ = batch_config[2]; + trt_common_ = std::make_unique( + tensorrt_common::TrtCommonConfig(model_path, precision)); + + const auto network_input_dims = trt_common_->getTensorShape(0); + batch_size_ = network_input_dims.d[0]; + const auto input_channel = network_input_dims.d[1]; + const auto input_height = network_input_dims.d[2]; + const auto input_width = network_input_dims.d[3]; + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + 0, {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}})}; + auto profile_dims_ptr = + std::make_unique>(profile_dims); + if (precision == "int8") { - int max_batch_size = batch_config[2]; - nvinfer1::Dims input_dims = autoware::tensorrt_common::get_input_dims(model_path); std::vector calibration_images; if (calibration_image_list_path != "") { calibration_images = loadImageList(calibration_image_list_path, ""); } autoware::tensorrt_classifier::ImageStream stream( - max_batch_size, input_dims, calibration_images); + batch_size_, network_input_dims, calibration_images); fs::path calibration_table{model_path}; std::string ext = ""; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { ext = "EntropyV2-"; } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { ext = "Legacy-"; } else { ext = "MinMax-"; @@ -141,11 +153,11 @@ TrtClassifier::TrtClassifier( histogram_table.replace_extension(ext); std::unique_ptr calibrator; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { calibrator.reset(new autoware::tensorrt_classifier::Int8EntropyCalibrator( stream, calibration_table, mean_, std_)); } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { double quantile = 0.999999; double cutoff = 0.999999; calibrator.reset(new autoware::tensorrt_classifier::Int8LegacyCalibrator( @@ -154,29 +166,27 @@ TrtClassifier::TrtClassifier( calibrator.reset(new autoware::tensorrt_classifier::Int8MinMaxCalibrator( stream, calibration_table, mean_, std_)); } - trt_common_ = std::make_unique( - model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); + if (!trt_common_->setupWithCalibrator( + std::move(calibrator), calib_config, std::move((profile_dims_ptr)))) { + throw std::runtime_error("Failed to setup TensorRT engine with calibrator"); + } } else { - trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); - } - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - return; + if (!trt_common_->setup(std::move(profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TensorRT engine"); + } } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); - const auto output_dims = trt_common_->getBindingDimensions(1); - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); + const auto output_dims = trt_common_->getTensorShape(1); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); out_elem_num_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num_ = out_elem_num_ * batch_config[2]; - out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_config[2]); + out_elem_num_ = out_elem_num_ * batch_size_; + out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_size_); out_prob_d_ = autoware::cuda_utils::make_unique(out_elem_num_); out_prob_h_ = autoware::cuda_utils::make_unique_host(out_elem_num_, cudaHostAllocPortable); @@ -193,9 +203,15 @@ TrtClassifier::TrtClassifier( TrtClassifier::~TrtClassifier() { - if (m_cuda) { - if (h_img_) CHECK_CUDA_ERROR(cudaFreeHost(h_img_)); - if (d_img_) CHECK_CUDA_ERROR(cudaFree(d_img_)); + try { + if (m_cuda) { + if (h_img_) CHECK_CUDA_ERROR(cudaFreeHost(h_img_)); + if (d_img_) CHECK_CUDA_ERROR(cudaFree(d_img_)); + } + } catch (const std::exception & e) { + std::cerr << "Exception in TrtClassifier destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in TrtClassifier destructor" << std::endl; } } @@ -218,7 +234,7 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) src_width_ = width; src_height_ = height; if (m_cuda) { - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); bool const hasRuntimeDim = std::any_of( input_dims.d, input_dims.d + input_dims.nbDims, [](int32_t input_dim) { return input_dim == -1; }); @@ -226,7 +242,9 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) input_dims.d[0] = batch_size_; } if (!h_img_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } if (!h_img_) { CHECK_CUDA_ERROR(cudaMallocHost( @@ -239,10 +257,15 @@ void TrtClassifier::initPreprocessBuffer(int width, int height) } } +int TrtClassifier::getBatchSize() const +{ + return batch_size_; +} + void TrtClassifier::preprocessGpu(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -266,7 +289,9 @@ void TrtClassifier::preprocessGpu(const std::vector & images) src_height_ = height; } if (!h_img_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -299,9 +324,11 @@ void TrtClassifier::preprocessGpu(const std::vector & images) void TrtClassifier::preprocess_opt(const std::vector & images) { int batch_size = static_cast(images.size()); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_chan = static_cast(input_dims.d[1]); const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -348,9 +375,6 @@ bool TrtClassifier::doInference( const std::vector & images, std::vector & results, std::vector & probabilities) { - if (!trt_common_->isInitialized()) { - return false; - } preprocess_opt(images); return feedforwardAndDecode(images, results, probabilities); @@ -363,7 +387,10 @@ bool TrtClassifier::feedforwardAndDecode( results.clear(); probabilities.clear(); std::vector buffers = {input_d_.get(), out_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); int batch_size = static_cast(images.size()); diff --git a/perception/autoware_tensorrt_common/CHANGELOG.rst b/perception/autoware_tensorrt_common/CHANGELOG.rst index ab3b7187c0169..771f9189c3092 100644 --- a/perception/autoware_tensorrt_common/CHANGELOG.rst +++ b/perception/autoware_tensorrt_common/CHANGELOG.rst @@ -2,6 +2,41 @@ Changelog for package autoware_tensorrt_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* fix(autoware_tensorrt_common): fix bugprone-integer-division (`#9660 `_) + fix: bugprone-error +* Contributors: Amadeusz Szymko, Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_tensorrt_common/CMakeLists.txt b/perception/autoware_tensorrt_common/CMakeLists.txt index 3105523a509e3..d7d8026866124 100644 --- a/perception/autoware_tensorrt_common/CMakeLists.txt +++ b/perception/autoware_tensorrt_common/CMakeLists.txt @@ -16,9 +16,14 @@ if(NOT (CUDAToolkit_FOUND AND CUDNN_FOUND AND TENSORRT_FOUND)) return() endif() +if(TENSORRT_VERSION VERSION_LESS 8.5) + message(WARNING "Unsupported version TensorRT ${TENSORRT_VERSION} detected. This package requires TensorRT 8.5 or later.") + return() +endif() + add_library(${PROJECT_NAME} SHARED src/tensorrt_common.cpp - src/simple_profiler.cpp + src/profiler.cpp ) target_link_libraries(${PROJECT_NAME} @@ -42,7 +47,7 @@ set_target_properties(${PROJECT_NAME} CXX_EXTENSIONS NO ) -# TODO(autoware_tensorrt_common): Remove -Wno-deprecated-declarations once upgrading to TensorRT 8.5 is complete +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization target_compile_options(${PROJECT_NAME} PRIVATE -Wall -Wextra -Wpedantic -Werror -Wno-deprecated-declarations ) diff --git a/perception/autoware_tensorrt_common/README.md b/perception/autoware_tensorrt_common/README.md index c828be58c1c1c..54bfccdfcb561 100644 --- a/perception/autoware_tensorrt_common/README.md +++ b/perception/autoware_tensorrt_common/README.md @@ -1,6 +1,70 @@ # autoware_tensorrt_common -## Purpose +This package provides a high-level API to work with TensorRT. This library simplifies the process of loading, building, and executing TensorRT inference engines using ONNX models. It also includes utilities for profiling and managing TensorRT execution contexts, making it easier to integrate TensorRT-based packages in Autoware. -This package contains a library of common functions related to TensorRT. -This package may include functions for handling TensorRT engine and calibration algorithm used for quantization +## Usage + +Here is an example usage of the library. For the full API documentation, please refer to the doxygen documentation (see header [file](include/autoware/tensorrt_common/tensorrt_common.hpp)). + +```c++ +#include + +#include +#include +#include + +using autoware::tensorrt_common::TrtCommon; +using autoware::tensorrt_common::TrtCommonConfig; +using autoware::tensorrt_common::TensorInfo; +using autoware::tensorrt_common::NetworkIO; +using autoware::tensorrt_common::ProfileDims; + +std::unique_ptr trt_common_; +``` + +### Create a tensorrt common instance and setup engine + +- With minimal configuration. + +```c++ +trt_common_ = std::make_unique(TrtCommonConfig("/path/to/onnx/model.onnx")); +trt_common_->setup(); +``` + +- With full configuration. + +```c++ +trt_common_ = std::make_unique(TrtCommonConfig("/path/to/onnx/model.onnx", "fp16", "/path/to/engine/model.engine", (1ULL << 30U), -1, false)); + +std::vector network_io{ + NetworkIO("sample_input", {3, {-1, 64, 512}}), NetworkIO("sample_output", {1, {50}})}; +std::vector profile_dims{ + ProfileDims("sample_input", {3, {1, 64, 512}}, {3, {3, 64, 512}}, {3, {9, 64, 512}})}; + +auto network_io_ptr = std::make_unique>(network_io); +auto profile_dims_ptr = std::make_unique>(profile_dims); + +trt_common_->setup(std::move(profile_dims_ptr), std::move(network_io_ptr)); +``` + +By defining network IO names and dimensions, an extra shapes validation will be performed after building / loading engine. This is useful to ensure the model is compatible with current code for preprocessing as it might consists of operations dependent on tensor shapes. + +Profile dimension is used to specify the min, opt, and max dimensions for dynamic shapes. + +Network IO or / and profile dimensions can be omitted if not needed. + +### Setting input and output tensors + +```c++ +bool success = true; +success &= trt_common_->setTensor("sample_input", sample_input_d_.get(), nvinfer1::Dims{3, {var_size, 64, 512}}); +success &= trt_common_->setTensor("sample_output", sample_output_d_.get()); +return success; +``` + +### Execute inference + +```c++ +auto success = trt_common_->enqueueV3(stream_); +return success; +``` diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp new file mode 100644 index 0000000000000..0b58a797d8b9b --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/conv_profiler.hpp @@ -0,0 +1,108 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ +#define AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ + +#include + +#include + +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +/** + * @struct ConvLayerInfo + * @brief Information of a layer. + */ +struct ConvLayerInfo +{ + //! @brief Input channel. + int in_c; + //! @brief Output channel. + int out_c; + //! @brief Width. + int w; + //! @brief Height. + int h; + //! @brief Kernel size. + int k; + //! @brief Stride. + int stride; + //! @brief Number of groups. + int groups; + //! @brief Layer type. + nvinfer1::LayerType type; +}; + +/** + * @class ConvProfiler + * @brief Collect per-layer profile information, assuming times are reported in the same order. + */ +class ConvProfiler : public tensorrt_common::Profiler +{ +public: + /** + * @brief Construct Profiler for convolutional layers. + * + * @param[in] src_profilers Source profilers to merge. + */ + explicit ConvProfiler( + const std::vector & src_profilers = std::vector()) + : Profiler(src_profilers) + { + } + + /** + * @brief Set per-layer profile information for model. + * + * @param[in] layer Layer to set profile information. + */ + void setProfDict(nvinfer1::ILayer * const layer) noexcept final + { + if (const auto type = layer->getType(); type == nvinfer1::LayerType::kCONVOLUTION) { + const auto name = layer->getName(); + auto conv = dynamic_cast(layer); + + nvinfer1::ITensor * in = layer->getInput(0); + nvinfer1::Dims in_dim = in->getDimensions(); + + nvinfer1::ITensor * out = layer->getOutput(0); + nvinfer1::Dims out_dim = out->getDimensions(); + + nvinfer1::Dims k_dims = conv->getKernelSizeNd(); + nvinfer1::Dims s_dims = conv->getStrideNd(); + + int32_t kernel = k_dims.d[0]; + int32_t stride = s_dims.d[0]; + int32_t groups = conv->getNbGroups(); + + layer_dict_.insert_or_assign( + name, ConvLayerInfo{ + in_dim.d[1], out_dim.d[1], in_dim.d[3], in_dim.d[2], kernel, stride, groups, type}); + } + } + +private: + //! @brief Per-layer information. + std::map layer_dict_; +}; +} // namespace tensorrt_common +} // namespace autoware +#endif // AUTOWARE__TENSORRT_COMMON__CONV_PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp index 5aa897983af72..b9b9048cd8204 100644 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/logger.hpp @@ -17,18 +17,19 @@ #ifndef AUTOWARE__TENSORRT_COMMON__LOGGER_HPP_ #define AUTOWARE__TENSORRT_COMMON__LOGGER_HPP_ -#include "NvInferRuntimeCommon.h" +#include #include #include +#include #include #include -#include #include #include #include #include #include +#include namespace autoware { @@ -46,7 +47,7 @@ class LogStreamConsumerBuffer : public std::stringbuf LogStreamConsumerBuffer(LogStreamConsumerBuffer && other) : mOutput(other.mOutput) {} - ~LogStreamConsumerBuffer() + ~LogStreamConsumerBuffer() override { // std::streambuf::pbase() gives a pointer to the beginning of the buffered part of the output // sequence std::streambuf::pptr() gives a pointer to the current position of the output @@ -60,7 +61,7 @@ class LogStreamConsumerBuffer : public std::stringbuf // synchronizes the stream buffer and returns 0 on success // synchronizing the stream buffer consists of inserting the buffer contents into the stream, // resetting the buffer and flushing the stream - virtual int sync() + int sync() override { putOutput(); return 0; @@ -252,6 +253,37 @@ class Logger : public nvinfer1::ILogger // NOLINT } } + void log(Severity severity, const char * msg, ...) const noexcept + { + if (mVerbose) { + va_list args; + va_start(args, msg); + + // Buffer size + va_list args_copy; + va_copy(args_copy, args); + int required_size = std::vsnprintf(nullptr, 0, msg, args_copy); + va_end(args_copy); + + // Formatting error + if (required_size < 0) { + log(Severity::kINTERNAL_ERROR, "Error formatting log message"); + va_end(args); + return; + } + + // Format msg + std::vector buffer(required_size + 1); + std::vsnprintf(buffer.data(), buffer.size(), msg, args); + + va_end(args); // End variadic argument processing + + // Send the formatted message to LogStreamConsumer + LogStreamConsumer(mReportableSeverity, severity) + << "[TRT] " << std::string(buffer.data()) << std::endl; + } + } + /** * @brief Logging with throttle. * diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp new file mode 100644 index 0000000000000..d45351f97fd01 --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/profiler.hpp @@ -0,0 +1,96 @@ +// 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__TENSORRT_COMMON__PROFILER_HPP_ +#define AUTOWARE__TENSORRT_COMMON__PROFILER_HPP_ + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ + +/** + * @class Profiler + * @brief Collect per-layer profile information, assuming times are reported in the same order. + */ +class Profiler : public nvinfer1::IProfiler +{ +public: + /** + * @struct Record + * @brief Record of layer profile information. + */ + struct Record + { + float time{0}; + int count{0}; + float min_time{-1.0}; + int index; + }; + + /** + * @brief Construct Profiler. + * + * @param[in] src_profilers Source profilers to merge. + */ + explicit Profiler(const std::vector & src_profilers = std::vector()); + + /** + * @brief Report layer time. + * + * @param[in] layerName Layer name. + * @param[in] ms Time in milliseconds. + */ + void reportLayerTime(const char * layerName, float ms) noexcept final; + + /** + * @brief Get printable representation of Profiler. + * + * @return String representation for current state of Profiler. + */ + [[nodiscard]] std::string toString() const; + + /** + * @brief Set per-layer profile information for model. + * + * @param[in] layer Layer to set profile information. + */ + virtual void setProfDict([[maybe_unused]] nvinfer1::ILayer * layer) noexcept {}; + + /** + * @brief Output Profiler to ostream. + * + * @param[out] out Output stream. + * @param[in] value Profiler to output. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & out, const Profiler & value); + +private: + //!< @brief Profile information for layers. + std::map profile_; + + //!< @brief Index for layer. + int index_; +}; +} // namespace tensorrt_common +} // namespace autoware +#endif // AUTOWARE__TENSORRT_COMMON__PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp deleted file mode 100644 index a8d504fb2861a..0000000000000 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/simple_profiler.hpp +++ /dev/null @@ -1,73 +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 AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ -#define AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ - -#include - -#include -#include -#include -#include - -namespace autoware -{ -namespace tensorrt_common -{ -struct LayerInfo -{ - int in_c; - int out_c; - int w; - int h; - int k; - int stride; - int groups; - nvinfer1::LayerType type; -}; - -/** - * @class Profiler - * @brief Collect per-layer profile information, assuming times are reported in the same order - */ -class SimpleProfiler : public nvinfer1::IProfiler -{ -public: - struct Record - { - float time{0}; - int count{0}; - float min_time{-1.0}; - int index; - }; - SimpleProfiler( - std::string name, - const std::vector & src_profilers = std::vector()); - - void reportLayerTime(const char * layerName, float ms) noexcept override; - - void setProfDict(nvinfer1::ILayer * layer) noexcept; - - friend std::ostream & operator<<(std::ostream & out, const SimpleProfiler & value); - -private: - std::string m_name; - std::map m_profile; - int m_index; - std::map m_layer_dict; -}; -} // namespace tensorrt_common -} // namespace autoware -#endif // AUTOWARE__TENSORRT_COMMON__SIMPLE_PROFILER_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp index 2b3b3f02f315f..9355732962e23 100644 --- a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_common.hpp @@ -15,226 +15,367 @@ #ifndef AUTOWARE__TENSORRT_COMMON__TENSORRT_COMMON_HPP_ #define AUTOWARE__TENSORRT_COMMON__TENSORRT_COMMON_HPP_ -#ifndef YOLOX_STANDALONE -#include - -#include -#endif +#include "autoware/tensorrt_common/logger.hpp" +#include "autoware/tensorrt_common/profiler.hpp" +#include "autoware/tensorrt_common/utils.hpp" #include #include -#if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) -#include -namespace fs = ::std::filesystem; -#else -#include -namespace fs = ::std::experimental::filesystem; -#endif - -#include -#include - #include -#include #include +#include +#include #include namespace autoware { namespace tensorrt_common { -/** - * @struct BuildConfig - * @brief Configuration to provide fine control regarding TensorRT builder - */ -struct BuildConfig -{ - // type for calibration - std::string calib_type_str; - - // DLA core ID that the process uses - int dla_core_id; - - // flag for partial quantization in first layer - bool quantize_first_layer; // For partial quantization - - // flag for partial quantization in last layer - bool quantize_last_layer; // For partial quantization - - // flag for per-layer profiler using IProfiler - bool profile_per_layer; - - // clip value for implicit quantization - double clip_value; // For implicit quantization - - // Supported calibration type - const std::array valid_calib_type = {"Entropy", "Legacy", "Percentile", "MinMax"}; - - BuildConfig() - : calib_type_str("MinMax"), - dla_core_id(-1), - quantize_first_layer(false), - quantize_last_layer(false), - profile_per_layer(false), - clip_value(0.0) - { - } - - explicit BuildConfig( - const std::string & calib_type_str, const int dla_core_id = -1, - const bool quantize_first_layer = false, const bool quantize_last_layer = false, - const bool profile_per_layer = false, const double clip_value = 0.0) - : calib_type_str(calib_type_str), - dla_core_id(dla_core_id), - quantize_first_layer(quantize_first_layer), - quantize_last_layer(quantize_last_layer), - profile_per_layer(profile_per_layer), - clip_value(clip_value) - { -#ifndef YOLOX_STANDALONE - if ( - std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) == - valid_calib_type.end()) { - std::stringstream message; - message << "Invalid calibration type was specified: " << calib_type_str << std::endl - << "Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]" << std::endl - << "Default calibration type will be used: MinMax" << std::endl; - std::cerr << message.str(); - } -#endif - } -}; - -nvinfer1::Dims get_input_dims(const std::string & onnx_file_path); - -const std::array valid_precisions = {"fp32", "fp16", "int8"}; -bool is_valid_precision_string(const std::string & precision); - template struct InferDeleter // NOLINT { - void operator()(T * obj) const - { - if (obj) { -#if TENSORRT_VERSION_MAJOR >= 8 - delete obj; -#else - obj->destroy(); -#endif - } - } + void operator()(T * obj) const { delete obj; } }; template using TrtUniquePtr = std::unique_ptr>; -using BatchConfig = std::array; +using NetworkIOPtr = std::unique_ptr>; +using ProfileDimsPtr = std::unique_ptr>; +using TensorsVec = std::vector>; +using TensorsMap = std::unordered_map>; /** * @class TrtCommon - * @brief TensorRT common library + * @brief TensorRT common library. */ class TrtCommon // NOLINT { public: /** * @brief Construct TrtCommon. - * @param[in] mode_path ONNX model_path - * @param[in] precision precision for inference - * @param[in] calibrator pointer for any type of INT8 calibrator - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine - * @param[in] buildConfig configuration including precision, calibration method, dla, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder - * @param[in] plugin_paths path for custom plugin + * + * @param[in] trt_config Base configuration with ONNX model path as minimum required. + * parameter. + * @param[in] profiler Per-layer profiler. + * @param[in] plugin_paths Paths for TensorRT plugins. */ TrtCommon( - const std::string & model_path, const std::string & precision, - std::unique_ptr calibrator = nullptr, - const BatchConfig & batch_config = {1, 1, 1}, const size_t max_workspace_size = (16 << 20), - const BuildConfig & buildConfig = BuildConfig(), + const TrtCommonConfig & trt_config, + const std::shared_ptr & profiler = std::make_shared(), const std::vector & plugin_paths = {}); - /** * @brief Deconstruct TrtCommon */ ~TrtCommon(); /** - * @brief Load TensorRT engine - * @param[in] engine_file_path path for a engine file - * @return flag for whether loading are succeeded or failed + * @brief Setup for TensorRT execution including building and loading engine. + * + * @param[in] profile_dims Optimization profile of tensors for dynamic shapes. + * @param[in] network_io Network input/output tensors information. + * @return Whether setup is successful. + */ + [[nodiscard]] virtual bool setup( + ProfileDimsPtr profile_dims = nullptr, NetworkIOPtr network_io = nullptr); + + /** + * @brief Get TensorRT engine precision. + * + * @return string representation of TensorRT engine precision. + */ + [[nodiscard]] std::string getPrecision() const; + + /** + * @brief Get tensor name by index from TensorRT engine with fallback from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor name. + */ + [[nodiscard]] const char * getIOTensorName(const int32_t index) const; + + /** + * @brief Get number of IO tensors from TensorRT engine with fallback from TensorRT network. + * + * @return Number of IO tensors. + */ + [[nodiscard]] int32_t getNbIOTensors() const; + + /** + * @brief Get tensor shape by index from TensorRT engine with fallback from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getTensorShape(const int32_t index) const; + + /** + * @brief Get tensor shape by name from TensorRT engine. + * + * @param[in] tensor_name Tensor name. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getTensorShape(const char * tensor_name) const; + + /** + * @brief Get input tensor shape by index from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getInputDims(const int32_t index) const; + + /** + * @brief Get output tensor shape by index from TensorRT network. + * + * @param[in] index Tensor index. + * @return Tensor shape. + */ + [[nodiscard]] nvinfer1::Dims getOutputDims(const int32_t index) const; + + /** + * @brief Set tensor address by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] data Tensor pointer. + * @return Whether setting tensor address is successful. + */ + bool setTensorAddress(const int32_t index, void * data); + + /** + * @brief Set tensor address by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] data Tensor pointer. + * @return Whether setting tensor address is successful. + */ + bool setTensorAddress(const char * tensor_name, void * data); + + /** + * @brief Set tensors addresses by indices via TensorRT context. + * + * @param[in] tensors Tensors pointers. + * @return Whether setting tensors addresses is successful. + */ + bool setTensorsAddresses(std::vector & tensors); + + /** + * @brief Set tensors addresses by names via TensorRT context. + * + * @param[in] tensors Tensors pointers. + * @return Whether setting tensors addresses is successful. + */ + bool setTensorsAddresses(std::unordered_map & tensors); + + /** + * @brief Set input shape by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting input shape is successful. + */ + bool setInputShape(const int32_t index, const nvinfer1::Dims & dimensions); + + /** + * @brief Set input shape by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting input shape is successful. + */ + bool setInputShape(const char * tensor_name, const nvinfer1::Dims & dimensions); + + /** + * @brief Set inputs shapes by indices via TensorRT context. + * + * @param[in] dimensions Vector of tensor dimensions with corresponding indices. + * @return Whether setting input shapes is successful. + */ + bool setInputsShapes(const std::vector & dimensions); + + /** + * @brief Set inputs shapes by names via TensorRT context. + * + * @param[in] dimensions Map of tensor dimensions with corresponding names as keys. + * @return Whether setting input shapes is successful. + */ + bool setInputsShapes(const std::unordered_map & dimensions); + + /** + * @brief Set tensor (address and shape) by index via TensorRT context. + * + * @param[in] index Tensor index. + * @param[in] data Tensor pointer. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting tensor is successful. + */ + bool setTensor(const int32_t index, void * data, nvinfer1::Dims dimensions = {}); + + /** + * @brief Set tensor (address and shape) by name via TensorRT context. + * + * @param[in] tensor_name Tensor name. + * @param[in] data Tensor pointer. + * @param[in] dimensions Tensor dimensions. + * @return Whether setting tensor is successful. + */ + bool setTensor(const char * tensor_name, void * data, nvinfer1::Dims dimensions = {}); + + /** + * @brief Set tensors (addresses and shapes) by indices via TensorRT context. + * + * @param[in] tensors Vector of tensor pointers and dimensions with corresponding indices. + * @return Whether setting tensors is successful. */ - bool loadEngine(const std::string & engine_file_path); + bool setTensors(TensorsVec & tensors); /** - * @brief Output layer information including GFLOPs and parameters - * @param[in] onnx_file_path path for a onnx file - * @warning This function is based on darknet log. + * @brief Set tensors (addresses and shapes) by names via TensorRT context. + * + * @param[in] tensors Map of tensor pointers and dimensions with corresponding names as keys. + * @return Whether setting tensors is successful. */ - void printNetworkInfo(const std::string & onnx_file_path); + bool setTensors(TensorsMap & tensors); /** - * @brief build TensorRT engine from ONNX - * @param[in] onnx_file_path path for a onnx file - * @param[in] output_engine_file_path path for a engine file + * @brief Get per-layer profiler for model. + * + * @return Per-layer profiler. */ - bool buildEngineFromOnnx( - const std::string & onnx_file_path, const std::string & output_engine_file_path); + [[nodiscard]] std::shared_ptr getModelProfiler(); /** - * @brief setup for TensorRT execution including building and loading engine + * @brief Get per-layer profiler for host. + * + * @return Per-layer profiler. */ - void setup(); + [[nodiscard]] std::shared_ptr getHostProfiler(); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 - void setupBindings(std::vector & bindings); -#endif + /** + * @brief Get TensorRT common configuration. + * + * @return TensorRT common configuration. + */ + [[nodiscard]] std::shared_ptr getTrtCommonConfig(); - bool isInitialized(); + /** + * @brief Get TensorRT builder configuration. + * + * @return TensorRT builder configuration. + */ + [[nodiscard]] std::shared_ptr getBuilderConfig(); + + /** + * @brief Get TensorRT network definition. + * + * @return TensorRT network definition. + */ + [[nodiscard]] std::shared_ptr getNetwork(); - nvinfer1::Dims getBindingDimensions(const int32_t index) const; - int32_t getNbBindings(); - bool setBindingDimensions(const int32_t index, const nvinfer1::Dims & dimensions) const; -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 + /** + * @brief Get TensorRT logger. + * + * @return TensorRT logger. + */ + [[nodiscard]] std::shared_ptr getLogger(); + + /** + * @brief Execute inference via TensorRT context. + * + * @param[in] stream CUDA stream. + * @return Whether inference is successful. + */ bool enqueueV3(cudaStream_t stream); -#endif -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 - bool enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * input_consumed); -#endif /** - * @brief output per-layer information + * @brief Print per-layer information. + */ + void printProfiling() const; + +private: + /** + * @brief Initialize TensorRT common. + * + * @return Whether initialization is successful. */ - void printProfiling(void); + [[nodiscard]] bool initialize(); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 /** - * @brief get per-layer information for trt-engine-profiler + * @brief Get per-layer information for trt-engine-profiler. + * + * @param[in] format Format for layer information. + * @return Layer information. */ std::string getLayerInformation(nvinfer1::LayerInformationFormat format); -#endif -private: - Logger logger_; - fs::path model_file_path_; + /** + + * @brief Build TensorRT engine from ONNX. + * + * @return Whether building engine is successful. + */ + bool buildEngineFromOnnx(); + + /** + * @brief Load TensorRT engine. + * + * @return Whether loading engine is successful. + */ + bool loadEngine(); + + /** + * @brief Validate network input/output names and dimensions. + * + * @return Whether network input/output is valid. + */ + bool validateNetworkIO(); + + /** + * @brief Validate optimization profile. + * + * @return Whether optimization profile is valid. + */ + bool validateProfileDims(); + + //! @brief TensorRT runtime. TrtUniquePtr runtime_; + + //! @brief TensorRT engine. TrtUniquePtr engine_; + + //! @brief TensorRT execution context. TrtUniquePtr context_; - std::unique_ptr calibrator_; - std::string precision_; - BatchConfig batch_config_; - size_t max_workspace_size_; - bool is_initialized_{false}; + //! @brief TensorRT builder. + TrtUniquePtr builder_; + + //! @brief TensorRT engine parser. + TrtUniquePtr parser_; + + //! @brief TensorRT builder configuration. + std::shared_ptr builder_config_; + + //! @brief TensorRT network definition. + std::shared_ptr network_; + + //! @brief TrtCommon library base configuration. + std::shared_ptr trt_config_; + + //! @brief TensorRT logger. + std::shared_ptr logger_; + + //! @brief Per-layer profiler for host. + std::shared_ptr host_profiler_; + + //! @brief Per-layer profiler for model. + std::shared_ptr model_profiler_; - // profiler for per-layer - SimpleProfiler model_profiler_; - // profiler for whole model - SimpleProfiler host_profiler_; + //! @brief Model optimization profile. + ProfileDimsPtr profile_dims_; - std::unique_ptr build_config_; + //! @brief Model network input/output tensors information. + NetworkIOPtr network_io_; }; } // namespace tensorrt_common diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp new file mode 100644 index 0000000000000..44a944031086e --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/tensorrt_conv_calib.hpp @@ -0,0 +1,241 @@ +// 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__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ +#define AUTOWARE__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ + +#include +#include +#include + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +template +bool contain(const std::string & s, const T & v) +{ + return s.find(v) != std::string::npos; +} + +using autoware::tensorrt_common::CalibrationConfig; +using autoware::tensorrt_common::NetworkIOPtr; +using autoware::tensorrt_common::ProfileDimsPtr; +using autoware::tensorrt_common::Profiler; +using autoware::tensorrt_common::TrtCommonConfig; + +/** + * @class TrtConvCalib + * @brief TensorRT common library with calibration. + */ +class TrtConvCalib : public tensorrt_common::TrtCommon +{ +public: + /** + * @brief Construct TrtCommonCalib. + * + * @param[in] trt_config base trt common configuration, ONNX model path is mandatory + * @param[in] profiler per-layer profiler + * @param[in] plugin_paths paths for TensorRT plugins + */ + explicit TrtConvCalib( + const TrtCommonConfig & trt_config, + const std::shared_ptr & profiler = std::make_shared(), + const std::vector & plugin_paths = {}) + : TrtCommon(trt_config, profiler, plugin_paths) + { + } + + /** + * @brief Setup for TensorRT execution including calibration, building and loading engine. + * + * @param[in] calibrator Pointer for any type of INT8 calibrator. + * @param[in] calib_config Calibration configuration. + * @param[in] profile_dims Optimization profile of input tensors for dynamic shapes. + * @param[in] network_io Network input/output tensors information. + * @return Whether setup is successful. + */ + [[nodiscard]] bool setupWithCalibrator( + std::unique_ptr calibrator, const CalibrationConfig & calib_config, + ProfileDimsPtr profile_dims = nullptr, NetworkIOPtr network_io = nullptr) + { + calibrator_ = std::move(calibrator); + calib_config_ = std::make_unique(calib_config); + + auto builder_config = getBuilderConfig(); + builder_config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + builder_config->setInt8Calibrator(calibrator_.get()); + + // Model specific quantization + auto logger = getLogger(); + auto quantization_log = quantization(); + logger->log(nvinfer1::ILogger::Severity::kINFO, quantization_log.c_str()); + + return setup(std::move(profile_dims), std::move(network_io)); + } + +private: + /** + * @brief Implicit quantization for TensorRT. + * + * @return Output log for TensorRT logger. + */ + std::string quantization() + { + auto network = getNetwork(); + auto trt_config = getTrtCommonConfig(); + auto model_profiler = getModelProfiler(); + + const int num = network->getNbLayers(); + bool first = calib_config_->quantize_first_layer; + bool last = calib_config_->quantize_last_layer; + std::stringstream ss; + + // Partial Quantization + if (getPrecision() == "int8") { + auto builder_config = getBuilderConfig(); + builder_config->setFlag(nvinfer1::BuilderFlag::kFP16); + network->getInput(0)->setDynamicRange(0, 255.0); + for (int i = 0; i < num; i++) { + nvinfer1::ILayer * layer = network->getLayer(i); + auto layer_type = layer->getType(); + std::string name = layer->getName(); + nvinfer1::ITensor * out = layer->getOutput(0); + if (calib_config_->clip_value > 0.0) { + ss << "Set max value for outputs: " << calib_config_->clip_value << " " << name + << std::endl; + out->setDynamicRange(0.0, calib_config_->clip_value); + } + + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { + if (first) { + layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << name << std::endl; + first = false; + } + if (last) { + // cspell: ignore preds + if ( + contain(name, "reg_preds") || contain(name, "cls_preds") || + contain(name, "obj_preds")) { + layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << name << std::endl; + } + for (int j = num - 1; j >= 0; j--) { + nvinfer1::ILayer * inner_layer = network->getLayer(j); + auto inner_layer_type = inner_layer->getType(); + std::string inner_name = inner_layer->getName(); + if (inner_layer_type == nvinfer1::LayerType::kCONVOLUTION) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << inner_name << std::endl; + break; + } + if (inner_layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { + inner_layer->setPrecision(nvinfer1::DataType::kHALF); + ss << "Set kHALF in " << inner_name << std::endl; + break; + } + } + } + } + } + builder_config->setFlag(nvinfer1::BuilderFlag::kINT8); + } + + // Print layer information + float total_gflops = 0.0; + int total_params = 0; + + for (int i = 0; i < num; i++) { + nvinfer1::ILayer * layer = network->getLayer(i); + auto layer_type = layer->getType(); + if (trt_config->profile_per_layer) { + model_profiler->setProfDict(layer); + } + if (layer_type == nvinfer1::LayerType::kCONSTANT) { + continue; + } + nvinfer1::ITensor * in = layer->getInput(0); + nvinfer1::Dims dim_in = in->getDimensions(); + nvinfer1::ITensor * out = layer->getOutput(0); + nvinfer1::Dims dim_out = out->getDimensions(); + + if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { + auto conv = dynamic_cast(layer); + nvinfer1::Dims k_dims = conv->getKernelSizeNd(); + nvinfer1::Dims s_dims = conv->getStrideNd(); + int groups = conv->getNbGroups(); + int stride = s_dims.d[0]; + int num_weights = (dim_in.d[1] / groups) * dim_out.d[1] * k_dims.d[0] * k_dims.d[1]; + float gflops = (2.0 * num_weights) * (static_cast(dim_in.d[3]) / stride * + static_cast(dim_in.d[2]) / stride / 1e9); + total_gflops += gflops; + total_params += num_weights; + ss << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups << ") " + << ") " << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" + << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" << dim_out.d[1]; + ss << " weights:" << num_weights; + ss << " GFLOPs:" << gflops; + ss << std::endl; + } else if (layer_type == nvinfer1::LayerType::kPOOLING) { + auto pool = dynamic_cast(layer); + auto p_type = pool->getPoolingType(); + nvinfer1::Dims dim_stride = pool->getStrideNd(); + nvinfer1::Dims dim_window = pool->getWindowSizeNd(); + + ss << "L" << i << " ["; + if (p_type == nvinfer1::PoolingType::kMAX) { + ss << "max "; + } else if (p_type == nvinfer1::PoolingType::kAVERAGE) { + ss << "avg "; + } else if (p_type == nvinfer1::PoolingType::kMAX_AVERAGE_BLEND) { + ss << "max avg blend "; + } + float gflops = static_cast(dim_in.d[1]) * + (static_cast(dim_window.d[0]) / static_cast(dim_stride.d[0])) * + (static_cast(dim_window.d[1]) / static_cast(dim_stride.d[1])) * + static_cast(dim_in.d[2]) * static_cast(dim_in.d[3]) / 1e9; + total_gflops += gflops; + ss << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; + ss << " GFLOPs:" << gflops; + ss << std::endl; + } else if (layer_type == nvinfer1::LayerType::kRESIZE) { + ss << "L" << i << " [resize]" << std::endl; + } + } + ss << "Total " << total_gflops << " GFLOPs" << std::endl; + ss << "Total " << total_params / 1000.0 / 1000.0 << " M params" << std::endl; + + return ss.str(); + }; + +private: + //!< Calibration configuration + std::unique_ptr calib_config_; + + //!< Calibrator used for implicit quantization + std::unique_ptr calibrator_; +}; + +} // namespace tensorrt_common +} // namespace autoware + +#endif // AUTOWARE__TENSORRT_COMMON__TENSORRT_CONV_CALIB_HPP_ diff --git a/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp new file mode 100644 index 0000000000000..be640f52147d9 --- /dev/null +++ b/perception/autoware_tensorrt_common/include/autoware/tensorrt_common/utils.hpp @@ -0,0 +1,408 @@ +// 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__TENSORRT_COMMON__UTILS_HPP_ +#define AUTOWARE__TENSORRT_COMMON__UTILS_HPP_ + +#include +#include + +#if (defined(_MSC_VER) or (defined(__GNUC__) and (7 <= __GNUC_MAJOR__))) +#include +namespace fs = ::std::filesystem; +#else +#include +namespace fs = ::std::experimental::filesystem; +#endif + +#include +#include +#include +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ +constexpr std::array valid_precisions = {"fp32", "fp16", "int8"}; + +/** + * @struct TrtCommonConfig + * @brief Configuration to provide fine control regarding TensorRT builder + */ +struct TrtCommonConfig +{ + /** @brief Construct TrtCommonConfig, ONNX model path is mandatory. + * + * @param[in] onnx_path ONNX model path + * @param[in] precision precision for inference + * @param[in] engine_path TensorRT engine path + * @param[in] max_workspace_size maximum workspace size for TensorRT + * @param[in] dla_core_id DLA core ID + * @param[in] profile_per_layer flag for per-layer profiler using IProfiler + */ + explicit TrtCommonConfig( + const std::string onnx_path, const std::string precision = "fp16", + const std::string engine_path = "", const size_t max_workspace_size = (1ULL << 30U), + const int32_t dla_core_id = -1, const bool profile_per_layer = false) + : onnx_path(onnx_path), + precision(precision), + engine_path(engine_path), + max_workspace_size(max_workspace_size), + dla_core_id(dla_core_id), + profile_per_layer(profile_per_layer) + { + validatePrecision(); + + if (engine_path.empty()) { + this->engine_path = onnx_path; + this->engine_path.replace_extension(".engine"); + } + } + + /** + * @brief Validate configured TensorRT engine precision. + */ + void validatePrecision() const + { + if ( + std::find(valid_precisions.begin(), valid_precisions.end(), precision) == + valid_precisions.end()) { + std::stringstream message; + message << "Invalid precision was specified: " << precision << std::endl + << "Valid string is one of: [" << valid_precisions[0]; + for (size_t i = 1; i < valid_precisions.size(); ++i) { + message << ", " << valid_precisions[i]; + } + message << "] (case sensitive)" << std::endl; + + throw std::runtime_error(message.str()); + } + } + + //!< @brief ONNX model path. + const fs::path onnx_path; + + //!< @brief Precision for inference. + const std::string precision; + + //!< @brief TensorRT engine path. + fs::path engine_path; + + //!< @brief TensorRT max workspace size. + const size_t max_workspace_size; + + //!< @brief DLA core ID that the process uses. + const int32_t dla_core_id; + + //!< @brief Flag for per-layer profiler using IProfiler. + const bool profile_per_layer; +}; + +/** + * @brief Represents a tensor with its name or index. + */ +struct TensorInfo +{ + /** + * @brief Construct TensorInfo with tensor name. + * + * @param[in] name Tensor name. + */ + explicit TensorInfo(std::string name) : tensor_name(std::move(name)), tensor_index(-1) {} + + /** + * @brief Construct TensorInfo with tensor index. + * + * @param[in] index Tensor index. + */ + explicit TensorInfo(int32_t index) : tensor_index(index) {} + + /** + * @brief Check if dimensions are equal. + * + * @param lhs Left-hand side tensor dimensions. + * @param rhs Right-hand side tensor dimensions. + * @return Whether dimensions are equal. + */ + static bool dimsEqual(const nvinfer1::Dims & lhs, const nvinfer1::Dims & rhs) + { + if (lhs.nbDims != rhs.nbDims) return false; + return std::equal(lhs.d, lhs.d + lhs.nbDims, rhs.d); // NOLINT + } + + /** + * @brief Get printable representation of tensor dimensions. + * + * @param[in] dims Tensor dimensions. + * @return String representation of tensor dimensions. + */ + static std::string dimsRepr(const nvinfer1::Dims & dims) + { + if (dims.nbDims == 0) return "[]"; + std::string repr = "[" + std::to_string(dims.d[0]); + for (int i = 1; i < dims.nbDims; ++i) { + repr += ", " + std::to_string(dims.d[i]); + } + repr += "]"; + return repr; + } + + //!< @brief Tensor name. + std::string tensor_name; + + //!< @brief Tensor index. + int32_t tensor_index; +}; + +/** + * @brief Represents a network input/output tensor with its dimensions. + * + * Example usage: + * @code + * nvinfer1::Dims tensor_dims = {3, {1, 2, 3}}; + * NetworkIO input("input_tensor", tensor_dims); + * NetworkIO output("output_tensor", tensor_dims); + * bool is_equal = input == output; // false + * @endcode + */ +struct NetworkIO : public TensorInfo +{ + /** + * @brief Construct NetworkIO with tensor name and dimensions. + * + * @param[in] name Tensor name. + * @param[in] tensor_dims Tensor dimensions. + */ + NetworkIO(std::string name, const nvinfer1::Dims & tensor_dims) + : TensorInfo(std::move(name)), dims(tensor_dims) + { + } + + /** + * @brief Construct NetworkIO with tensor index and dimensions. + * + * @param[in] index Tensor index. + * @param[in] tensor_dims Tensor dimensions. + */ + NetworkIO(int32_t index, const nvinfer1::Dims & tensor_dims) + : TensorInfo(index), dims(tensor_dims) + { + } + + /** + * @brief Get printable representation of NetworkIO. + * + * @return String representation of NetworkIO. + */ + [[nodiscard]] std::string toString() const + { + std::stringstream ss; + ss << tensor_name << " {" << TensorInfo::dimsRepr(dims) << "}"; + return ss.str(); + } + + /** + * @brief Check if NetworkIO is equal to another NetworkIO. + * + * @param rhs Right-hand side NetworkIO. + * @return Whether NetworkIO is equal to another NetworkIO. + */ + bool operator==(const NetworkIO & rhs) const + { + if (tensor_name != rhs.tensor_name) return false; + return dimsEqual(dims, rhs.dims); + } + + /** + * @brief Check if NetworkIO is not equal to another NetworkIO. + * + * @param rhs Right-hand side NetworkIO. + * @return Whether NetworkIO is not equal to another NetworkIO. + */ + bool operator!=(const NetworkIO & rhs) const { return !(*this == rhs); } + + /** + * @brief Output NetworkIO to ostream. + * + * @param os Output stream. + * @param io NetworkIO. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & os, const NetworkIO & io) + { + os << io.toString(); + return os; + } + + //!< @brief Tensor dimensions. + nvinfer1::Dims dims; +}; + +/** + * @brief Represents a tensor optimization profile with minimum, optimal, and maximum dimensions. + * + * Example usage: + * @code + * nvinfer1::Dims min = {3, {1, 2, 3}}; + * nvinfer1::Dims opt = {3, {1, 3, 4}}; + * nvinfer1::Dims max = {3, {1, 4, 5}}; + * ProfileDims entry_1("tensor_name", min, opt, max); + * ProfileDims entry_2("tensor_name", {3, {1, 2, 3}}, {3, {1, 3, 4}}, {3, {1, 4, 5}}); + * bool is_equal = entry_1 == entry_2; // true + * @endcode + */ +struct ProfileDims : public TensorInfo +{ + /** + * @brief Construct ProfileDims with tensor name and dimensions. + * + * @param[in] name Tensor name. + * @param[in] min Minimum dimensions for optimization profile. + * @param[in] opt Optimal dimensions for optimization profile. + * @param[in] max Maximum dimensions for optimization profile. + */ + ProfileDims( + std::string name, const nvinfer1::Dims & min, const nvinfer1::Dims & opt, + const nvinfer1::Dims & max) + : TensorInfo(std::move(name)), min_dims(min), opt_dims(opt), max_dims(max) + { + } + + /** + * @brief Construct ProfileDims with tensor index and dimensions. + * + * @param[in] index Tensor index. + * @param[in] min Minimum dimensions for optimization profile. + * @param[in] opt Optimal dimensions for optimization profile. + * @param[in] max Maximum dimensions for optimization profile. + */ + ProfileDims( + int32_t index, const nvinfer1::Dims & min, const nvinfer1::Dims & opt, + const nvinfer1::Dims & max) + : TensorInfo(index), min_dims(min), opt_dims(opt), max_dims(max) + { + } + + /** + * @brief Get printable representation of ProfileDims. + * + * @return String representation of ProfileDims. + */ + [[nodiscard]] std::string toString() const + { + std::ostringstream oss; + oss << tensor_name << " {min " << TensorInfo::dimsRepr(min_dims) << ", opt " + << TensorInfo::dimsRepr(opt_dims) << ", max " << TensorInfo::dimsRepr(max_dims) << "}"; + return oss.str(); + } + + /** + * @brief Check if ProfileDims is equal to another ProfileDims. + * + * @param rhs Right-hand side ProfileDims. + * @return Whether ProfileDims is equal to another ProfileDims. + */ + bool operator==(const ProfileDims & rhs) const + { + if (tensor_name != rhs.tensor_name) return false; + return dimsEqual(min_dims, rhs.min_dims) && dimsEqual(opt_dims, rhs.opt_dims) && + dimsEqual(max_dims, rhs.max_dims); + } + + /** + * @brief Check if ProfileDims is not equal to another ProfileDims. + * + * @param rhs Right-hand side ProfileDims. + * @return Whether ProfileDims is not equal to another ProfileDims. + */ + bool operator!=(const ProfileDims & rhs) const { return !(*this == rhs); } + + /** + * @brief Output ProfileDims to ostream. + * + * @param os Output stream. + * @param profile ProfileDims. + * @return Output stream. + */ + friend std::ostream & operator<<(std::ostream & os, const ProfileDims & profile) + { + os << profile.toString(); + return os; + } + + //!< @brief Minimum dimensions for optimization profile. + nvinfer1::Dims min_dims; + + //!< @brief Optimal dimensions for optimization profile. + nvinfer1::Dims opt_dims; + + //!< @brief Maximum dimensions for optimization profile. + nvinfer1::Dims max_dims; +}; + +//!< @brief Valid calibration types for TensorRT. +constexpr std::array valid_calib_type = { + "Entropy", "Legacy", "Percentile", "MinMax"}; + +/** + * @brief Configuration for implicit calibration. + */ +struct CalibrationConfig +{ + /** + * @brief Construct CalibrationConfig with its parameters. + * + * @param[in] calib_type_str Calibration type. + * @param[in] quantize_first_layer Flag for partial quantization in first layer. + * @param[in] quantize_last_layer Flag for partial quantization in last layer. + * @param[in] clip_value Clip value for implicit quantization. + */ + explicit CalibrationConfig( + const std::string & calib_type_str = "MinMax", const bool quantize_first_layer = false, + const bool quantize_last_layer = false, const double clip_value = 0.0) + : calib_type_str(calib_type_str), + quantize_first_layer(quantize_first_layer), + quantize_last_layer(quantize_last_layer), + clip_value(clip_value) + { + if ( + std::find(valid_calib_type.begin(), valid_calib_type.end(), calib_type_str) == + valid_calib_type.end()) { + throw std::runtime_error( + "Invalid calibration type was specified: " + std::string(calib_type_str) + + "\nValid value is one of: [Entropy, (Legacy | Percentile), MinMax]"); + } + } + + //!< @brief type of calibration + const std::string calib_type_str; + + //!< @brief flag for partial quantization in first layer + const bool quantize_first_layer; + + //!< @brief flag for partial quantization in last layer + const bool quantize_last_layer; + + //!< @brief clip value for implicit quantization + const double clip_value; +}; + +} // namespace tensorrt_common +} // namespace autoware + +#endif // AUTOWARE__TENSORRT_COMMON__UTILS_HPP_ diff --git a/perception/autoware_tensorrt_common/package.xml b/perception/autoware_tensorrt_common/package.xml index 300a7b263c063..475332d5795be 100644 --- a/perception/autoware_tensorrt_common/package.xml +++ b/perception/autoware_tensorrt_common/package.xml @@ -1,7 +1,7 @@ autoware_tensorrt_common - 0.40.0 + 0.41.0 tensorrt utility wrapper Taichi Higashide diff --git a/perception/autoware_tensorrt_common/src/profiler.cpp b/perception/autoware_tensorrt_common/src/profiler.cpp new file mode 100644 index 0000000000000..433cca51f2b4f --- /dev/null +++ b/perception/autoware_tensorrt_common/src/profiler.cpp @@ -0,0 +1,113 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +namespace autoware +{ +namespace tensorrt_common +{ + +Profiler::Profiler(const std::vector & src_profilers) +{ + index_ = 0; + for (const auto & src_profiler : src_profilers) { + for (const auto & [name, record] : src_profiler.profile_) { + auto it = profile_.find(name); + if (it == profile_.end()) { + profile_.emplace(name, record); + } else { + it->second.time += record.time; + it->second.count += record.count; + } + } + } +} + +void Profiler::reportLayerTime(const char * layerName, float ms) noexcept +{ + if (profile_.find(layerName) == profile_.end()) { + return; + } + profile_[layerName].count++; + profile_[layerName].time += ms; + if (profile_[layerName].min_time == -1.0) { + profile_[layerName].min_time = ms; + profile_[layerName].index = index_; + index_++; + } else if (profile_[layerName].min_time > ms) { + profile_[layerName].min_time = ms; + } +} + +std::string Profiler::toString() const +{ + std::ostringstream out; + float total_time = 0.0; + std::string layer_name = "Operation"; + + int max_layer_name_length = static_cast(layer_name.size()); + for (const auto & elem : profile_) { + total_time += elem.second.time; + max_layer_name_length = std::max(max_layer_name_length, static_cast(elem.first.size())); + } + + auto old_settings = out.flags(); + auto old_precision = out.precision(); + // Output header + { + out << "index, " << std::setw(12); + out << std::setw(max_layer_name_length) << layer_name << " "; + out << std::setw(12) << "Runtime" << "%," << " "; + out << std::setw(12) << "Invocations" << " , "; + out << std::setw(12) << "Runtime[ms]" << " , "; + out << std::setw(12) << "Avg Runtime[ms]" << " ,"; + out << std::setw(12) << "Min Runtime[ms]" << std::endl; + } + int index = index_; + for (int i = 0; i < index; i++) { + for (const auto & elem : profile_) { + if (elem.second.index == i) { + out << i << ", "; + out << std::setw(max_layer_name_length) << elem.first << ","; + out << std::setw(12) << std::fixed << std::setprecision(1) + << (elem.second.time * 100.0F / total_time) << "%" << ","; + out << std::setw(12) << elem.second.count << ","; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.time << ", "; + out << std::setw(12) << std::fixed << std::setprecision(2) + << elem.second.time / elem.second.count << ", "; + out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.min_time + << std::endl; + } + } + } + out.flags(old_settings); + out.precision(old_precision); + out << "========== total runtime = " << total_time << " ms ==========" << std::endl; + + return out.str(); +} + +std::ostream & operator<<(std::ostream & out, const Profiler & value) +{ + out << value.toString(); + return out; +} +} // namespace tensorrt_common +} // namespace autoware diff --git a/perception/autoware_tensorrt_common/src/simple_profiler.cpp b/perception/autoware_tensorrt_common/src/simple_profiler.cpp deleted file mode 100644 index 2bcc1c4f9da06..0000000000000 --- a/perception/autoware_tensorrt_common/src/simple_profiler.cpp +++ /dev/null @@ -1,138 +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 - -#include -#include -#include -#include - -namespace autoware -{ -namespace tensorrt_common -{ - -SimpleProfiler::SimpleProfiler(std::string name, const std::vector & src_profilers) -: m_name(name) -{ - m_index = 0; - for (const auto & src_profiler : src_profilers) { - for (const auto & rec : src_profiler.m_profile) { - auto it = m_profile.find(rec.first); - if (it == m_profile.end()) { - m_profile.insert(rec); - } else { - it->second.time += rec.second.time; - it->second.count += rec.second.count; - } - } - } -} - -void SimpleProfiler::reportLayerTime(const char * layerName, float ms) noexcept -{ - m_profile[layerName].count++; - m_profile[layerName].time += ms; - if (m_profile[layerName].min_time == -1.0) { - m_profile[layerName].min_time = ms; - m_profile[layerName].index = m_index; - m_index++; - } else if (m_profile[layerName].min_time > ms) { - m_profile[layerName].min_time = ms; - } -} - -void SimpleProfiler::setProfDict(nvinfer1::ILayer * layer) noexcept -{ - std::string name = layer->getName(); - m_layer_dict[name]; - m_layer_dict[name].type = layer->getType(); - if (layer->getType() == nvinfer1::LayerType::kCONVOLUTION) { - nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; - nvinfer1::ITensor * in = layer->getInput(0); - nvinfer1::Dims dim_in = in->getDimensions(); - nvinfer1::ITensor * out = layer->getOutput(0); - nvinfer1::Dims dim_out = out->getDimensions(); - nvinfer1::Dims k_dims = conv->getKernelSizeNd(); - nvinfer1::Dims s_dims = conv->getStrideNd(); - int groups = conv->getNbGroups(); - int stride = s_dims.d[0]; - int kernel = k_dims.d[0]; - m_layer_dict[name].in_c = dim_in.d[1]; - m_layer_dict[name].out_c = dim_out.d[1]; - m_layer_dict[name].w = dim_in.d[3]; - m_layer_dict[name].h = dim_in.d[2]; - m_layer_dict[name].k = kernel; - ; - m_layer_dict[name].stride = stride; - m_layer_dict[name].groups = groups; - } -} - -std::ostream & operator<<(std::ostream & out, const SimpleProfiler & value) -{ - out << "========== " << value.m_name << " profile ==========" << std::endl; - float totalTime = 0; - std::string layerNameStr = "Operation"; - - int maxLayerNameLength = static_cast(layerNameStr.size()); - for (const auto & elem : value.m_profile) { - totalTime += elem.second.time; - maxLayerNameLength = std::max(maxLayerNameLength, static_cast(elem.first.size())); - } - - auto old_settings = out.flags(); - auto old_precision = out.precision(); - // Output header - { - out << "index, " << std::setw(12); - out << std::setw(maxLayerNameLength) << layerNameStr << " "; - out << std::setw(12) << "Runtime" - << "%," - << " "; - out << std::setw(12) << "Invocations" - << " , "; - out << std::setw(12) << "Runtime[ms]" - << " , "; - out << std::setw(12) << "Avg Runtime[ms]" - << " ,"; - out << std::setw(12) << "Min Runtime[ms]" << std::endl; - } - int index = value.m_index; - for (int i = 0; i < index; i++) { - for (const auto & elem : value.m_profile) { - if (elem.second.index == i) { - out << i << ", "; - out << std::setw(maxLayerNameLength) << elem.first << ","; - out << std::setw(12) << std::fixed << std::setprecision(1) - << (elem.second.time * 100.0F / totalTime) << "%" - << ","; - out << std::setw(12) << elem.second.count << ","; - out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.time << ", "; - out << std::setw(12) << std::fixed << std::setprecision(2) - << elem.second.time / elem.second.count << ", "; - out << std::setw(12) << std::fixed << std::setprecision(2) << elem.second.min_time - << std::endl; - } - } - } - out.flags(old_settings); - out.precision(old_precision); - out << "========== " << value.m_name << " total runtime = " << totalTime - << " ms ==========" << std::endl; - return out; -} -} // namespace tensorrt_common -} // namespace autoware diff --git a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp index 897010d22bb4b..ba422277416ab 100644 --- a/perception/autoware_tensorrt_common/src/tensorrt_common.cpp +++ b/perception/autoware_tensorrt_common/src/tensorrt_common.cpp @@ -12,102 +12,38 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "autoware/tensorrt_common/tensorrt_common.hpp" +#include "autoware/tensorrt_common/logger.hpp" +#include "autoware/tensorrt_common/utils.hpp" + +#include #include +#include #include +#include #include -#include #include #include #include +#include #include #include -namespace -{ -template -bool contain(const std::string & s, const T & v) -{ - return s.find(v) != std::string::npos; -} -} // anonymous namespace - namespace autoware { namespace tensorrt_common { -nvinfer1::Dims get_input_dims(const std::string & onnx_file_path) -{ - Logger logger_; - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); - } - - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - } - - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - } - - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Failed to parse onnx file"); - } - - const auto input = network->getInput(0); - return input->getDimensions(); -} - -bool is_valid_precision_string(const std::string & precision) -{ - if ( - std::find(valid_precisions.begin(), valid_precisions.end(), precision) == - valid_precisions.end()) { - std::stringstream message; - message << "Invalid precision was specified: " << precision << std::endl - << "Valid string is one of: ["; - for (const auto & s : valid_precisions) { - message << s << ", "; - } - message << "] (case sensitive)" << std::endl; - std::cerr << message.str(); - return false; - } else { - return true; - } -} TrtCommon::TrtCommon( - const std::string & model_path, const std::string & precision, - std::unique_ptr calibrator, const BatchConfig & batch_config, - const size_t max_workspace_size, const BuildConfig & build_config, + const TrtCommonConfig & trt_config, const std::shared_ptr & profiler, const std::vector & plugin_paths) -: model_file_path_(model_path), - calibrator_(std::move(calibrator)), - precision_(precision), - batch_config_(batch_config), - max_workspace_size_(max_workspace_size), - model_profiler_("Model"), - host_profiler_("Host") +: trt_config_(std::make_shared(trt_config)), + host_profiler_(profiler), + model_profiler_(profiler) { - // Check given precision is valid one - if (!is_valid_precision_string(precision)) { - return; - } - build_config_ = std::make_unique(build_config); - + logger_ = std::make_shared(); for (const auto & plugin_path : plugin_paths) { int32_t flags{RTLD_LAZY}; // cspell: ignore asan @@ -120,519 +56,595 @@ TrtCommon::TrtCommon( #endif // ENABLE_ASAN void * handle = dlopen(plugin_path.c_str(), flags); if (!handle) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Could not load plugin library"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Could not load plugin library"); + } else { + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Loaded plugin library: %s", plugin_path.c_str()); } } - runtime_ = TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); - if (build_config_->dla_core_id != -1) { - runtime_->setDLACore(build_config_->dla_core_id); + runtime_ = TrtUniquePtr(nvinfer1::createInferRuntime(*logger_)); + if (trt_config_->dla_core_id != -1) { + runtime_->setDLACore(trt_config_->dla_core_id); } - initLibNvInferPlugins(&logger_, ""); -} + initLibNvInferPlugins(&*logger_, ""); -TrtCommon::~TrtCommon() -{ + if (!initialize()) { + throw std::runtime_error("Failed to initialize TensorRT"); + } } -void TrtCommon::setup() +TrtCommon::~TrtCommon() = default; + +bool TrtCommon::setup(ProfileDimsPtr profile_dims, NetworkIOPtr network_io) { - if (!fs::exists(model_file_path_)) { - is_initialized_ = false; - return; - } - // cppcheck-suppress unreadVariable - std::string engine_path = model_file_path_; - if (model_file_path_.extension() == ".engine") { - std::cout << "Load ... " << model_file_path_ << std::endl; - loadEngine(model_file_path_); - } else if (model_file_path_.extension() == ".onnx") { - fs::path cache_engine_path{model_file_path_}; - std::string ext; - std::string calib_name = ""; - if (precision_ == "int8") { - if (build_config_->calib_type_str == "Entropy") { - calib_name = "EntropyV2-"; - } else if ( - build_config_->calib_type_str == "Legacy" || - build_config_->calib_type_str == "Percentile") { - calib_name = "Legacy-"; - } else { - calib_name = "MinMax-"; + profile_dims_ = std::move(profile_dims); + network_io_ = std::move(network_io); + + // Set input profile + if (profile_dims_ && !profile_dims_->empty()) { + auto profile = builder_->createOptimizationProfile(); + for (auto & profile_dim : *profile_dims_) { + if (profile_dim.tensor_name.empty()) { + profile_dim.tensor_name = getIOTensorName(profile_dim.tensor_index); } + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Setting optimization profile for tensor: %s", + profile_dim.toString().c_str()); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kMIN, profile_dim.min_dims); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kOPT, profile_dim.opt_dims); + profile->setDimensions( + profile_dim.tensor_name.c_str(), nvinfer1::OptProfileSelector::kMAX, profile_dim.max_dims); } - if (build_config_->dla_core_id != -1) { - ext = "DLA" + std::to_string(build_config_->dla_core_id) + "-" + calib_name + precision_; - if (build_config_->quantize_first_layer) { - ext += "-firstFP16"; - } - if (build_config_->quantize_last_layer) { - ext += "-lastFP16"; - } - ext += "-batch" + std::to_string(batch_config_[0]) + ".engine"; - } else { - ext = calib_name + precision_; - if (build_config_->quantize_first_layer) { - ext += "-firstFP16"; - } - if (build_config_->quantize_last_layer) { - ext += "-lastFP16"; - } - ext += "-batch" + std::to_string(batch_config_[0]) + ".engine"; + builder_config_->addOptimizationProfile(profile); + } + + auto build_engine_with_log = [this]() -> bool { + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Starting to build engine"); + auto log_thread = logger_->log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TensorRT CUDA engine. Please wait for a few minutes...", + 5); + auto success = buildEngineFromOnnx(); + logger_->stop_throttle(log_thread); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Engine build completed"); + return success; + }; + + // Load engine file if it exists + if (fs::exists(trt_config_->engine_path)) { + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Loading engine"); + if (!loadEngine()) { + return false; } - cache_engine_path.replace_extension(ext); - - // Output Network Information - printNetworkInfo(model_file_path_); - - if (fs::exists(cache_engine_path)) { - std::cout << "Loading... " << cache_engine_path << std::endl; - loadEngine(cache_engine_path); - } else { - std::cout << "Building... " << cache_engine_path << std::endl; - logger_.log(nvinfer1::ILogger::Severity::kINFO, "Start build engine"); - auto log_thread = logger_.log_throttle( - nvinfer1::ILogger::Severity::kINFO, - "Applying optimizations and building TRT CUDA engine. Please wait for a few minutes...", 5); - buildEngineFromOnnx(model_file_path_, cache_engine_path); - logger_.stop_throttle(log_thread); - logger_.log(nvinfer1::ILogger::Severity::kINFO, "End build engine"); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Network validation"); + // Validate engine tensor shapes and optimization profile + if (!validateNetworkIO() || !validateProfileDims()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Network validation failed for loaded engine from file. Rebuilding engine"); + // Rebuild engine if the tensor shapes or optimization profile mismatch + if (!build_engine_with_log()) { + return false; + } } - // cppcheck-suppress unreadVariable - engine_path = cache_engine_path; } else { - is_initialized_ = false; - return; + // Build engine if engine has not been cached + if (!build_engine_with_log()) { + return false; + } } - context_ = TrtUniquePtr(engine_->createExecutionContext()); - if (!context_) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); - is_initialized_ = false; - return; + // Validate engine nevertheless is loaded or rebuilt + if (!validateNetworkIO() || !validateProfileDims()) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Final network validation failed. Possibly the input / output of the currently " + "deployed model has changed. Check your configuration file with the current model."); + return false; } - if (build_config_->profile_per_layer) { - context_->setProfiler(&model_profiler_); + logger_->log(nvinfer1::ILogger::Severity::kINFO, "Engine setup completed"); + return true; +} + +std::string TrtCommon::getPrecision() const +{ + return trt_config_->precision; +} + +const char * TrtCommon::getIOTensorName(const int32_t index) const +{ + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return nullptr; + } + auto num_inputs = network_->getNbInputs(); + auto num_outputs = network_->getNbOutputs(); + if (index < 0 || index >= num_inputs + num_outputs) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid index for I/O tensor: %d. Total I/O tensors: %d", index, num_inputs + num_outputs); + return nullptr; + } + if (index < num_inputs) { + return network_->getInput(index)->getName(); + } + return network_->getOutput(index - num_inputs)->getName(); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - // Write profiles for trt-engine-explorer - // See: https://github.com/NVIDIA/TensorRT/tree/main/tools/experimental/trt-engine-explorer - std::string j_ext = ".json"; - fs::path json_path{engine_path}; - json_path.replace_extension(j_ext); - std::string ret = getLayerInformation(nvinfer1::LayerInformationFormat::kJSON); - std::ofstream os(json_path, std::ofstream::trunc); - os << ret << std::flush; -#endif - is_initialized_ = true; + return engine_->getIOTensorName(index); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 -void TrtCommon::setupBindings(std::vector & bindings) +int32_t TrtCommon::getNbIOTensors() const { - for (int32_t i = 0, e = engine_->getNbIOTensors(); i < e; i++) { - auto const name = engine_->getIOTensorName(i); - context_->setTensorAddress(name, bindings.at(i)); + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return 0; + } + return network_->getNbInputs() + network_->getNbOutputs(); } + return engine_->getNbIOTensors(); } -#endif -bool TrtCommon::loadEngine(const std::string & engine_file_path) +nvinfer1::Dims TrtCommon::getTensorShape(const int32_t index) const { - std::ifstream engine_file(engine_file_path); - std::stringstream engine_buffer; - engine_buffer << engine_file.rdbuf(); - std::string engine_str = engine_buffer.str(); - engine_ = TrtUniquePtr(runtime_->deserializeCudaEngine( - reinterpret_cast(engine_str.data()), engine_str.size())); - return true; + if (!engine_) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Engine is not initialized. Retrieving data from network"); + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return nvinfer1::Dims{}; + } + auto num_inputs = network_->getNbInputs(); + auto num_outputs = network_->getNbOutputs(); + if (index < 0 || index >= num_inputs + num_outputs) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid index for I/O tensor: %d. Total I/O tensors: %d", index, num_inputs + num_outputs); + return nvinfer1::Dims{}; + } + if (index < num_inputs) { + return network_->getInput(index)->getDimensions(); + } + return network_->getOutput(index - num_inputs)->getDimensions(); + } + auto const & name = getIOTensorName(index); + return getTensorShape(name); } -void TrtCommon::printNetworkInfo(const std::string & onnx_file_path) +nvinfer1::Dims TrtCommon::getTensorShape(const char * tensor_name) const { - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); - return; + if (!engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Engine is not initialized"); + return nvinfer1::Dims{}; } + return engine_->getTensorShape(tensor_name); +} - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); +nvinfer1::Dims TrtCommon::getInputDims(const int32_t index) const +{ + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return {}; + } + const auto input = network_->getInput(index); + return input->getDimensions(); +} - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - return; +nvinfer1::Dims TrtCommon::getOutputDims(const int32_t index) const +{ + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Network is not initialized"); + return {}; } + const auto output = network_->getOutput(index); + return output->getDimensions(); +} + +bool TrtCommon::setTensorAddress(const int32_t index, void * data) +{ + auto const & name = getIOTensorName(index); + return setTensorAddress(name, data); +} - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - return; +bool TrtCommon::setTensorAddress(const char * tensor_name, void * data) +{ + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); + return false; + } + auto success = context_->setTensorAddress(tensor_name, data); + if (!success) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Failed to set tensor address for tensor: ", tensor_name); } + return success; +} - if (precision_ == "fp16" || precision_ == "int8") { - config->setFlag(nvinfer1::BuilderFlag::kFP16); +bool TrtCommon::setTensorsAddresses(std::vector & tensors) +{ + bool success = true; + for (std::size_t i = 0, e = tensors.size(); i < e; i++) { + auto const name = getIOTensorName(i); + success &= setTensorAddress(name, tensors.at(i)); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, max_workspace_size_); -#else - config->setMaxWorkspaceSize(max_workspace_size_); -#endif + return success; +} - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - return; - } - int num = network->getNbLayers(); - float total_gflops = 0.0; - int total_params = 0; - for (int i = 0; i < num; i++) { - nvinfer1::ILayer * layer = network->getLayer(i); - auto layer_type = layer->getType(); - if (build_config_->profile_per_layer) { - model_profiler_.setProfDict(layer); - } - if (layer_type == nvinfer1::LayerType::kCONSTANT) { - continue; - } - nvinfer1::ITensor * in = layer->getInput(0); - nvinfer1::Dims dim_in = in->getDimensions(); - nvinfer1::ITensor * out = layer->getOutput(0); - nvinfer1::Dims dim_out = out->getDimensions(); - - if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { - nvinfer1::IConvolutionLayer * conv = (nvinfer1::IConvolutionLayer *)layer; - nvinfer1::Dims k_dims = conv->getKernelSizeNd(); - nvinfer1::Dims s_dims = conv->getStrideNd(); - int groups = conv->getNbGroups(); - int stride = s_dims.d[0]; - int num_weights = (dim_in.d[1] / groups) * dim_out.d[1] * k_dims.d[0] * k_dims.d[1]; - float gflops = (2 * num_weights) * (dim_in.d[3] / stride * dim_in.d[2] / stride / 1e9); - ; - total_gflops += gflops; - total_params += num_weights; - std::cout << "L" << i << " [conv " << k_dims.d[0] << "x" << k_dims.d[1] << " (" << groups - << ") " << "/" << s_dims.d[0] << "] " << dim_in.d[3] << "x" << dim_in.d[2] << "x" - << dim_in.d[1] << " -> " << dim_out.d[3] << "x" << dim_out.d[2] << "x" - << dim_out.d[1]; - std::cout << " weights:" << num_weights; - std::cout << " GFLOPs:" << gflops; - std::cout << std::endl; - } else if (layer_type == nvinfer1::LayerType::kPOOLING) { - nvinfer1::IPoolingLayer * pool = (nvinfer1::IPoolingLayer *)layer; - auto p_type = pool->getPoolingType(); - nvinfer1::Dims dim_stride = pool->getStrideNd(); - nvinfer1::Dims dim_window = pool->getWindowSizeNd(); - - std::cout << "L" << i << " ["; - if (p_type == nvinfer1::PoolingType::kMAX) { - std::cout << "max "; - } else if (p_type == nvinfer1::PoolingType::kAVERAGE) { - std::cout << "avg "; - } else if (p_type == nvinfer1::PoolingType::kMAX_AVERAGE_BLEND) { - std::cout << "max avg blend "; - } - float gflops = dim_in.d[1] * dim_window.d[0] / dim_stride.d[0] * dim_window.d[1] / - dim_stride.d[1] * dim_in.d[2] * dim_in.d[3] / 1e9; - total_gflops += gflops; - std::cout << "pool " << dim_window.d[0] << "x" << dim_window.d[1] << "]"; - std::cout << " GFLOPs:" << gflops; - std::cout << std::endl; - } else if (layer_type == nvinfer1::LayerType::kRESIZE) { - std::cout << "L" << i << " [resize]" << std::endl; - } +bool TrtCommon::setTensorsAddresses(std::unordered_map & tensors) +{ + bool success = true; + for (auto const & tensor : tensors) { + success &= setTensorAddress(tensor.first, tensor.second); } - std::cout << "Total " << total_gflops << " GFLOPs" << std::endl; - std::cout << "Total " << total_params / 1000.0 / 1000.0 << " M params" << std::endl; - return; + return success; +} + +bool TrtCommon::setInputShape(const int32_t index, const nvinfer1::Dims & dimensions) +{ + auto const & name = getIOTensorName(index); + return setInputShape(name, dimensions); } -bool TrtCommon::buildEngineFromOnnx( - const std::string & onnx_file_path, const std::string & output_engine_file_path) +bool TrtCommon::setInputShape(const char * tensor_name, const nvinfer1::Dims & dimensions) { - auto builder = TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); - if (!builder) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); return false; } + auto success = context_->setInputShape(tensor_name, dimensions); + if (!success) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, "Failed to set input shape for tensor: ", tensor_name); + } + return success; +} - const auto explicitBatch = - 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); +bool TrtCommon::setInputsShapes(const std::vector & dimensions) +{ + bool success = true; + for (std::size_t i = 0, e = dimensions.size(); i < e; i++) { + success &= setInputShape(i, dimensions.at(i)); + } + return success; +} - auto network = - TrtUniquePtr(builder->createNetworkV2(explicitBatch)); - if (!network) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); - return false; +bool TrtCommon::setInputsShapes(const std::unordered_map & dimensions) +{ + bool success = true; + for (auto const & dim : dimensions) { + success &= setInputShape(dim.first, dim.second); } + return success; +} - auto config = TrtUniquePtr(builder->createBuilderConfig()); - if (!config) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); - return false; +bool TrtCommon::setTensor(const int32_t index, void * data, nvinfer1::Dims dimensions) +{ + auto success = setTensorAddress(index, data); + if (dimensions.nbDims > 0) { + success &= setInputShape(index, dimensions); } + return success; +} - int num_available_dla = builder->getNbDLACores(); - if (build_config_->dla_core_id != -1) { - if (num_available_dla > 0) { - std::cout << "###" << num_available_dla << " DLAs are supported! ###" << std::endl; - } else { - std::cout << "###Warning : " << "No DLA is supported! ###" << std::endl; - } - config->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); - config->setDLACore(build_config_->dla_core_id); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); -#else - config->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); -#endif - config->setFlag(nvinfer1::BuilderFlag::kGPU_FALLBACK); +bool TrtCommon::setTensor(const char * tensor_name, void * data, nvinfer1::Dims dimensions) +{ + auto success = setTensorAddress(tensor_name, data); + if (dimensions.nbDims > 0) { + success &= setInputShape(tensor_name, dimensions); } - if (precision_ == "fp16" || precision_ == "int8") { - config->setFlag(nvinfer1::BuilderFlag::kFP16); + return success; +} + +bool TrtCommon::setTensors(TensorsVec & tensors) +{ + bool success = true; + for (std::size_t i = 0, e = tensors.size(); i < e; i++) { + success &= setTensor(i, tensors.at(i).first, tensors.at(i).second); } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 - config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, max_workspace_size_); -#else - config->setMaxWorkspaceSize(max_workspace_size_); -#endif + return success; +} - auto parser = TrtUniquePtr(nvonnxparser::createParser(*network, logger_)); - if (!parser->parseFromFile( - onnx_file_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR))) { - std::cout << "Failed to parse onnx file" << std::endl; - return false; +bool TrtCommon::setTensors(TensorsMap & tensors) +{ + bool success = true; + for (auto const & tensor : tensors) { + success &= setTensor(tensor.first, tensor.second.first, tensor.second.second); } + return success; +} - const int num = network->getNbLayers(); - bool first = build_config_->quantize_first_layer; - bool last = build_config_->quantize_last_layer; - // Partial Quantization - if (precision_ == "int8") { - network->getInput(0)->setDynamicRange(0, 255.0); - for (int i = 0; i < num; i++) { - nvinfer1::ILayer * layer = network->getLayer(i); - auto layer_type = layer->getType(); - std::string name = layer->getName(); - nvinfer1::ITensor * out = layer->getOutput(0); - if (build_config_->clip_value > 0.0) { - std::cout << "Set max value for outputs : " << build_config_->clip_value << " " << name - << std::endl; - out->setDynamicRange(0.0, build_config_->clip_value); - } +std::shared_ptr TrtCommon::getModelProfiler() +{ + return model_profiler_; +} - if (layer_type == nvinfer1::LayerType::kCONVOLUTION) { - if (first) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; - first = false; - } - if (last) { - // cspell: ignore preds - if ( - contain(name, "reg_preds") || contain(name, "cls_preds") || - contain(name, "obj_preds")) { - layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << name << std::endl; - } - for (int j = num - 1; j >= 0; j--) { - nvinfer1::ILayer * inner_layer = network->getLayer(j); - auto inner_layer_type = inner_layer->getType(); - std::string inner_name = inner_layer->getName(); - if (inner_layer_type == nvinfer1::LayerType::kCONVOLUTION) { - inner_layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << inner_name << std::endl; - break; - } - if (inner_layer_type == nvinfer1::LayerType::kMATRIX_MULTIPLY) { - inner_layer->setPrecision(nvinfer1::DataType::kHALF); - std::cout << "Set kHALF in " << inner_name << std::endl; - break; - } - } - } - } - } +std::shared_ptr TrtCommon::getHostProfiler() +{ + return host_profiler_; +} + +std::shared_ptr TrtCommon::getTrtCommonConfig() +{ + return trt_config_; +} + +std::shared_ptr TrtCommon::getBuilderConfig() +{ + return builder_config_; +} + +std::shared_ptr TrtCommon::getNetwork() +{ + return network_; +} + +std::shared_ptr TrtCommon::getLogger() +{ + return logger_; +} + +bool TrtCommon::enqueueV3(cudaStream_t stream) +{ + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context is not initialized"); + return false; + } + if (trt_config_->profile_per_layer) { + auto inference_start = std::chrono::high_resolution_clock::now(); + auto success = context_->enqueueV3(stream); + auto inference_end = std::chrono::high_resolution_clock::now(); + host_profiler_->reportLayerTime( + "inference_host", + std::chrono::duration(inference_end - inference_start).count()); + return success; } + return context_->enqueueV3(stream); +} - const auto input = network->getInput(0); - const auto input_dims = input->getDimensions(); - const auto input_channel = input_dims.d[1]; - const auto input_height = input_dims.d[2]; - const auto input_width = input_dims.d[3]; - const auto input_batch = input_dims.d[0]; +void TrtCommon::printProfiling() const +{ + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Host Profiling\n", host_profiler_->toString().c_str()); + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Model Profiling\n", model_profiler_->toString().c_str()); +} + +std::string TrtCommon::getLayerInformation(nvinfer1::LayerInformationFormat format) +{ + if (!context_ || !engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Context or engine are not initialized"); + return {}; + } + auto inspector = std::unique_ptr(engine_->createEngineInspector()); + inspector->setExecutionContext(&(*context_)); + std::string result = inspector->getEngineInformation(format); + return result; +} - if (input_batch > 1) { - batch_config_[0] = input_batch; +bool TrtCommon::initialize() +{ + if (!fs::exists(trt_config_->onnx_path) || trt_config_->onnx_path.extension() != ".onnx") { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Invalid ONNX file path or extension"); + return false; } - if (batch_config_.at(0) > 1 && (batch_config_.at(0) == batch_config_.at(2))) { - // Attention : below API is deprecated in TRT8.4 - builder->setMaxBatchSize(batch_config_.at(2)); - } else { - if (build_config_->profile_per_layer) { - auto profile = builder->createOptimizationProfile(); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMIN, - nvinfer1::Dims4{batch_config_.at(0), input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kOPT, - nvinfer1::Dims4{batch_config_.at(1), input_channel, input_height, input_width}); - profile->setDimensions( - network->getInput(0)->getName(), nvinfer1::OptProfileSelector::kMAX, - nvinfer1::Dims4{batch_config_.at(2), input_channel, input_height, input_width}); - config->addOptimizationProfile(profile); - } + builder_ = TrtUniquePtr(nvinfer1::createInferBuilder(*logger_)); + if (!builder_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder"); + return false; } - if (precision_ == "int8" && calibrator_) { - config->setFlag(nvinfer1::BuilderFlag::kINT8); -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 + const auto explicit_batch = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + network_ = TrtUniquePtr(builder_->createNetworkV2(explicit_batch)); #else - config->setFlag(nvinfer1::BuilderFlag::kSTRICT_TYPES); + network_ = TrtUniquePtr(builder_->createNetworkV2(0)); #endif - // QAT requires no calibrator. - // assert((calibrator != nullptr) && "Invalid calibrator for INT8 precision"); - config->setInt8Calibrator(calibrator_.get()); + + if (!network_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create network"); + return false; } - if (build_config_->profile_per_layer) { -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 - config->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kDETAILED); -#else - config->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kVERBOSE); -#endif + + builder_config_ = TrtUniquePtr(builder_->createBuilderConfig()); + if (!builder_config_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create builder config"); + return false; + } + + auto num_available_dla = builder_->getNbDLACores(); + if (trt_config_->dla_core_id != -1) { + logger_->log( + nvinfer1::ILogger::Severity::kINFO, "Number of DLAs supported: %d", num_available_dla); + builder_config_->setDefaultDeviceType(nvinfer1::DeviceType::kDLA); + builder_config_->setDLACore(trt_config_->dla_core_id); + builder_config_->setFlag(nvinfer1::BuilderFlag::kPREFER_PRECISION_CONSTRAINTS); + builder_config_->setFlag(nvinfer1::BuilderFlag::kGPU_FALLBACK); } + if (trt_config_->precision == "fp16") { + builder_config_->setFlag(nvinfer1::BuilderFlag::kFP16); + } else if (trt_config_->precision == "int8") { + builder_config_->setFlag(nvinfer1::BuilderFlag::kINT8); + } + builder_config_->setMemoryPoolLimit( + nvinfer1::MemoryPoolType::kWORKSPACE, trt_config_->max_workspace_size); + + parser_ = TrtUniquePtr(nvonnxparser::createParser(*network_, *logger_)); + if (!parser_->parseFromFile( + trt_config_->onnx_path.c_str(), + static_cast(nvinfer1::ILogger::Severity::kERROR))) { + return false; + } + + if (trt_config_->profile_per_layer) { + builder_config_->setProfilingVerbosity(nvinfer1::ProfilingVerbosity::kDETAILED); + } + + return true; +} -#if TENSORRT_VERSION_MAJOR >= 8 - auto plan = - TrtUniquePtr(builder->buildSerializedNetwork(*network, *config)); +bool TrtCommon::buildEngineFromOnnx() +{ + // Build engine + auto plan = TrtUniquePtr( + builder_->buildSerializedNetwork(*network_, *builder_config_)); if (!plan) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create host memory"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create host memory"); return false; } engine_ = TrtUniquePtr( runtime_->deserializeCudaEngine(plan->data(), plan->size())); -#else - engine_ = TrtUniquePtr(builder->buildEngineWithConfig(*network, *config)); -#endif if (!engine_) { - logger_.log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); return false; } - // save engine -#if TENSORRT_VERSION_MAJOR < 8 - auto data = TrtUniquePtr(engine_->serialize()); -#endif + // Save engine std::ofstream file; - file.open(output_engine_file_path, std::ios::binary | std::ios::out); + file.open(trt_config_->engine_path, std::ios::binary | std::ios::out); if (!file.is_open()) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to open engine file"); return false; } -#if TENSORRT_VERSION_MAJOR < 8 - file.write(reinterpret_cast(data->data()), data->size()); -#else - file.write(reinterpret_cast(plan->data()), plan->size()); -#endif - + file.write(reinterpret_cast(plan->data()), plan->size()); // NOLINT file.close(); - return true; -} - -bool TrtCommon::isInitialized() -{ - return is_initialized_; -} - -nvinfer1::Dims TrtCommon::getBindingDimensions(const int32_t index) const -{ -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + (NV_TENSOR_PATCH * 10) >= 8500 - auto const & name = engine_->getIOTensorName(index); - auto dims = context_->getTensorShape(name); - bool const has_runtime_dim = - std::any_of(dims.d, dims.d + dims.nbDims, [](int32_t dim) { return dim == -1; }); + context_ = TrtUniquePtr(engine_->createExecutionContext()); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); + return false; + } - if (has_runtime_dim) { - return dims; - } else { - return context_->getBindingDimensions(index); + if (trt_config_->profile_per_layer) { + context_->setProfiler(&*model_profiler_); } -#else - return context_->getBindingDimensions(index); -#endif -} -int32_t TrtCommon::getNbBindings() -{ - return engine_->getNbBindings(); -} + fs::path json_path = trt_config_->engine_path.replace_extension(".json"); + auto ret = getLayerInformation(nvinfer1::LayerInformationFormat::kJSON); + std::ofstream os(json_path, std::ofstream::trunc); + os << ret << std::flush; + os.close(); -bool TrtCommon::setBindingDimensions(const int32_t index, const nvinfer1::Dims & dimensions) const -{ - return context_->setBindingDimensions(index, dimensions); + return true; } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8500 -bool TrtCommon::enqueueV3(cudaStream_t stream) +bool TrtCommon::loadEngine() { - if (build_config_->profile_per_layer) { - auto inference_start = std::chrono::high_resolution_clock::now(); - - bool ret = context_->enqueueV3(stream); + std::ifstream engine_file(trt_config_->engine_path); + std::stringstream engine_buffer; + engine_buffer << engine_file.rdbuf(); + std::string engine_str = engine_buffer.str(); - auto inference_end = std::chrono::high_resolution_clock::now(); - host_profiler_.reportLayerTime( - "inference", - std::chrono::duration(inference_end - inference_start).count()); - return ret; + engine_ = TrtUniquePtr(runtime_->deserializeCudaEngine( + reinterpret_cast( // NOLINT + engine_str.data()), + engine_str.size())); + if (!engine_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create engine"); + return false; } - return context_->enqueueV3(stream); -} -#endif -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH < 10000 -bool TrtCommon::enqueueV2(void ** bindings, cudaStream_t stream, cudaEvent_t * input_consumed) -{ - if (build_config_->profile_per_layer) { - auto inference_start = std::chrono::high_resolution_clock::now(); - bool ret = context_->enqueueV2(bindings, stream, input_consumed); + context_ = TrtUniquePtr(engine_->createExecutionContext()); + if (!context_) { + logger_->log(nvinfer1::ILogger::Severity::kERROR, "Fail to create context"); + return false; + } - auto inference_end = std::chrono::high_resolution_clock::now(); - host_profiler_.reportLayerTime( - "inference", - std::chrono::duration(inference_end - inference_start).count()); - return ret; - } else { - return context_->enqueueV2(bindings, stream, input_consumed); + if (trt_config_->profile_per_layer) { + context_->setProfiler(&*model_profiler_); } + + return true; } -#endif -void TrtCommon::printProfiling() +bool TrtCommon::validateProfileDims() { - std::cout << host_profiler_; - std::cout << std::endl; - std::cout << model_profiler_; + auto success = true; + if (!profile_dims_ || profile_dims_->empty()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Input profile is empty, skipping validation. If network has dynamic shapes, it might lead " + "to undefined behavior."); + return success; + } + if (engine_->getNbOptimizationProfiles() != 1) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Number of optimization profiles in the engine (%d) is not equal to 1. Selecting the first " + "cached profile.", + engine_->getNbOptimizationProfiles()); + } + + for (const auto & profile_dim : *profile_dims_) { + nvinfer1::Dims min_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kMIN); + nvinfer1::Dims opt_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kOPT); + nvinfer1::Dims max_dims = engine_->getProfileShape( + profile_dim.tensor_name.c_str(), 0, nvinfer1::OptProfileSelector::kMAX); + ProfileDims profile_from_engine{profile_dim.tensor_name, min_dims, opt_dims, max_dims}; + if (profile_dim != profile_from_engine) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Invalid profile. Current configuration: %s. Cached engine: %s", + profile_dim.toString().c_str(), profile_from_engine.toString().c_str()); + success = false; + } + } + return success; } -#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8200 -std::string TrtCommon::getLayerInformation(nvinfer1::LayerInformationFormat format) +bool TrtCommon::validateNetworkIO() { - auto runtime = std::unique_ptr(nvinfer1::createInferRuntime(logger_)); - auto inspector = std::unique_ptr(engine_->createEngineInspector()); - if (context_ != nullptr) { - inspector->setExecutionContext(&(*context_)); + auto success = true; + if (!network_io_ || network_io_->empty()) { + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Network IO is empty, skipping validation. It might lead to undefined behavior"); + return success; + } + + if (network_io_->size() != static_cast(getNbIOTensors())) { + std::string tensor_names = "[" + std::string(getIOTensorName(0)); + for (int32_t i = 1; i < getNbIOTensors(); ++i) { + tensor_names += ", " + std::string(getIOTensorName(i)); + } + tensor_names += "]"; + logger_->log( + nvinfer1::ILogger::Severity::kWARNING, + "Number of tensors in the engine (%d) does not match number of tensors in the config (%d). " + "Tensors in the built engine: %s", + getNbIOTensors(), network_io_->size(), tensor_names.c_str()); + success = false; + } + for (const auto & io : *network_io_) { + NetworkIO tensor_from_engine{io.tensor_name, getTensorShape(io.tensor_name.c_str())}; + if (io != tensor_from_engine) { + logger_->log( + nvinfer1::ILogger::Severity::kERROR, + "Invalid tensor. Current configuration: %s. Cached engine: %s", io.toString().c_str(), + tensor_from_engine.toString().c_str()); + success = false; + } } - std::string result = inspector->getEngineInformation(format); - return result; + + return success; } -#endif } // namespace tensorrt_common } // namespace autoware diff --git a/perception/autoware_tensorrt_yolox/CHANGELOG.rst b/perception/autoware_tensorrt_yolox/CHANGELOG.rst index 969f1df4b5496..2b7959f6234c9 100644 --- a/perception/autoware_tensorrt_yolox/CHANGELOG.rst +++ b/perception/autoware_tensorrt_yolox/CHANGELOG.rst @@ -2,6 +2,53 @@ Changelog for package autoware_tensorrt_yolox ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_tensorrt_yolox)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_tensorrt_yolox (`#9898 `_) +* feat(tensorrt_yolox): add launch for tlr model (`#9828 `_) + * feat(tensorrt_yolox): add launch for tlr model + * chore: typo + * docs: update readme and description + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* fix(autoware_tensorrt_yolox): modify tensorrt_yolox_node name (`#9156 `_) +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* fix(autoware_tensorrt_yolox): fix bugprone-exception-escape (`#9734 `_) + * fix: bugprone-error + * fix: fmt + * fix: fmt + --------- +* Contributors: Amadeusz Szymko, Fumiya Watanabe, Vishal Chauhan, badai nguyen, cyn-liu, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_tensorrt_yolox/CMakeLists.txt b/perception/autoware_tensorrt_yolox/CMakeLists.txt index 17b31fc7e8df1..cc0d793151781 100644 --- a/perception/autoware_tensorrt_yolox/CMakeLists.txt +++ b/perception/autoware_tensorrt_yolox/CMakeLists.txt @@ -10,6 +10,9 @@ endif() find_package(autoware_cmake REQUIRED) autoware_package() +# TODO(amadeuszsz): Remove -Wno-deprecated-declarations once removing implicit quantization +add_compile_options(-Wno-deprecated-declarations) + find_package(OpenCV REQUIRED) option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) diff --git a/perception/autoware_tensorrt_yolox/README.md b/perception/autoware_tensorrt_yolox/README.md index cc7113e7dfcee..b211219e60ac6 100644 --- a/perception/autoware_tensorrt_yolox/README.md +++ b/perception/autoware_tensorrt_yolox/README.md @@ -4,6 +4,8 @@ This package detects target objects e.g., cars, trucks, bicycles, and pedestrians and segment target objects such as cars, trucks, buses and pedestrian, building, vegetation, road, sidewalk on a image based on [YOLOX](https://github.com/Megvii-BaseDetection/YOLOX) model with multi-header structure. +Additionally, the package also supports traffic light detection by switching onnx file which target classes listed on respective `label_file`. Currently 0: `unknown`, 1: `car_traffic_light` and 2: `pedestrian_traffic_light`. + ## Inner-workings / Algorithms ### Cite @@ -22,12 +24,12 @@ Zheng Ge, Songtao Liu, Feng Wang, Zeming Li, Jian Sun, "YOLOX: Exceeding YOLO Se ### Output -| Name | Type | Description | -| ---------------- | -------------------------------------------------- | ------------------------------------------------------------------- | -| `out/objects` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects with 2D bounding boxes | -| `out/image` | `sensor_msgs/Image` | The image with 2D bounding boxes for visualization | -| `out/mask` | `sensor_msgs/Image` | The semantic segmentation mask | -| `out/color_mask` | `sensor_msgs/Image` | The colorized image of semantic segmentation mask for visualization | +| Name | Type | Description | +| ---------------- | -------------------------------------------------- | ----------------------------------------------------------------------------------------------------- | +| `out/objects` | `tier4_perception_msgs/DetectedObjectsWithFeature` | The detected objects or traffic light with 2D bounding boxes | +| `out/image` | `sensor_msgs/Image` | The image with 2D bounding boxes for visualization | +| `out/mask` | `sensor_msgs/Image` | The semantic segmentation mask (only effective for semseg model) | +| `out/color_mask` | `sensor_msgs/Image` | The colorized image of semantic segmentation mask for visualization (only effective for semseg model) | ## Parameters @@ -45,6 +47,14 @@ The label contained in detected 2D bounding boxes (i.e., `out/objects`) will be - BICYCLE - MOTORCYCLE +or + +- UNKNOWN +- CAR_TRAFFIC_LIGHT +- PEDESTRIAN_TRAFFIC_LIGHT + +for traffic light detector onnx model. + If other labels (case insensitive) are contained in the file specified via the `label_file` parameter, those are labeled as `UNKNOWN`, while detected rectangles are drawn in the visualization result (`out/image`). diff --git a/perception/autoware_tensorrt_yolox/config/yolox_traffic_light_detector.param.yaml b/perception/autoware_tensorrt_yolox/config/yolox_traffic_light_detector.param.yaml new file mode 100644 index 0000000000000..4982204892488 --- /dev/null +++ b/perception/autoware_tensorrt_yolox/config/yolox_traffic_light_detector.param.yaml @@ -0,0 +1,35 @@ +# cspell: ignore semseg +/**: + ros__parameters: + + model_path: "$(var data_path)/tensorrt_yolox/$(var model_name).onnx" + label_path: "$(var data_path)/tensorrt_yolox/car_ped_tl_detector_labels.txt" + score_threshold: 0.35 + nms_threshold: 0.7 + + precision: "fp16" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8]. + calibration_algorithm: "Entropy" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax]. + dla_core_id: -1 # If positive ID value is specified, the node assign inference task to the DLA core. + quantize_first_layer: false # If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8. + quantize_last_layer: false # If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8. + profile_per_layer: false # If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose. + clip_value: 6.0 # If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration. + preprocess_on_gpu: true # If true, pre-processing is performed on GPU. + gpu_id: 0 # GPU ID to select CUDA Device + calibration_image_list_path: "" # Path to a file which contains path to images. Those images will be used for int8 quantization. + + + # default params of tensorrt_yolox package, only effective for semseg model + color_map_path: "$(var data_path)/tensorrt_yolox/semseg_color_map.csv" + is_roi_overlap_segment: false + overlap_roi_score_threshold: 0.3 + is_publish_color_mask: false + roi_overlay_segment_label: + UNKNOWN : false + CAR : false + TRUCK : false + BUS : false + MOTORCYCLE : false + BICYCLE : false + PEDESTRIAN : false + ANIMAL: false diff --git a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp index 483adfbdf2757..7d49df145b72e 100644 --- a/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/autoware_tensorrt_yolox/include/autoware/tensorrt_yolox/tensorrt_yolox.hpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include @@ -44,6 +46,13 @@ struct Object using ObjectArray = std::vector; using ObjectArrays = std::vector; +using autoware::tensorrt_common::CalibrationConfig; +using autoware::tensorrt_common::NetworkIOPtr; +using autoware::tensorrt_common::ProfileDimsPtr; +using autoware::tensorrt_common::Profiler; +using autoware::tensorrt_common::TrtCommon; +using autoware::tensorrt_common::TrtCommonConfig; +using autoware::tensorrt_common::TrtConvCalib; struct GridAndStride { @@ -70,31 +79,26 @@ class TrtYoloX public: /** * @brief Construct TrtYoloX. - * @param[in] mode_path ONNX model_path - * @param[in] precision precision for inference + * @param[in] trt_config base trt common configuration * @param[in] num_class classifier-ed num * @param[in] score_threshold threshold for detection * @param[in] nms_threshold threshold for NMS - * @param[in] build_config configuration including precision, calibration method, DLA, remaining - * fp16 for first layer, remaining fp16 for last layer and profiler for builder * @param[in] use_gpu_preprocess whether use cuda gpu for preprocessing - * @param[in] calibration_image_list_file path for calibration files (only require for + * @param[in] gpu_id GPU id for inference + * @param[in] calibration_image_list_path path for calibration files (only require for * quantization) * @param[in] norm_factor scaling factor for preprocess * @param[in] cache_dir unused variable - * @param[in] batch_config configuration for batched execution - * @param[in] max_workspace_size maximum workspace for building TensorRT engine + * @param[in] color_map_path path for colormap for masks + * @param[in] calib_config calibration configuration */ TrtYoloX( - const std::string & model_path, const std::string & precision, const int num_class = 8, - const float score_threshold = 0.3, const float nms_threshold = 0.7, - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig(), - const bool use_gpu_preprocess = false, const uint8_t gpu_id = 0, - std::string calibration_image_list_file = std::string(), const double norm_factor = 1.0, - [[maybe_unused]] const std::string & cache_dir = "", - const autoware::tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, - const size_t max_workspace_size = (1 << 30), const std::string & color_map_path = ""); + TrtCommonConfig & trt_config, const int num_class = 8, const float score_threshold = 0.3, + const float nms_threshold = 0.7, const bool use_gpu_preprocess = false, + const uint8_t gpu_id = 0, std::string calibration_image_list_path = std::string(), + const double norm_factor = 1.0, [[maybe_unused]] const std::string & cache_dir = "", + const std::string & color_map_path = "", + const CalibrationConfig & calib_config = CalibrationConfig()); /** * @brief Deconstruct TrtYoloX */ @@ -168,6 +172,12 @@ class TrtYoloX cv::Mat & colorized_mask); inline std::vector getColorMap() { return sematic_color_map_; } + /** + * @brief get batch size + * @return batch size + */ + [[nodiscard]] int getBatchSize() const; + private: /** * @brief run preprocess including resizing, letterbox, NHWC2NCHW and toFloat on CPU @@ -266,7 +276,7 @@ class TrtYoloX */ cv::Mat getMaskImageGpu(float * d_prob, nvinfer1::Dims dims, int out_w, int out_h, int b); - std::unique_ptr trt_common_; + std::unique_ptr trt_common_; std::vector input_h_; CudaUniquePtr input_d_; @@ -288,7 +298,7 @@ class TrtYoloX int num_class_; float score_threshold_; float nms_threshold_; - int batch_size_; + int32_t batch_size_; CudaUniquePtrHost out_prob_h_; // flag whether preprocess are performed on GPU diff --git a/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml index a7952b12414b1..606fd1299cf35 100644 --- a/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/multiple_yolox.launch.xml @@ -11,34 +11,50 @@ + + + + + + + + + + + + + + + + diff --git a/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 0096a219c8573..6d40905d78127 100644 --- a/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/autoware_tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,6 +1,8 @@ + + @@ -15,13 +17,13 @@ - + - + diff --git a/perception/autoware_tensorrt_yolox/launch/yolox_traffic_light_detector.launch.xml b/perception/autoware_tensorrt_yolox/launch/yolox_traffic_light_detector.launch.xml new file mode 100644 index 0000000000000..67aae61b54376 --- /dev/null +++ b/perception/autoware_tensorrt_yolox/launch/yolox_traffic_light_detector.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/autoware_tensorrt_yolox/package.xml b/perception/autoware_tensorrt_yolox/package.xml index e04d839282fd8..5c1b00906285b 100644 --- a/perception/autoware_tensorrt_yolox/package.xml +++ b/perception/autoware_tensorrt_yolox/package.xml @@ -1,7 +1,7 @@ autoware_tensorrt_yolox - 0.40.0 + 0.41.0 tensorrt library implementation for yolox Daisuke Nishimatsu @@ -20,6 +20,7 @@ tensorrt_cmake_module autoware_cuda_utils + autoware_internal_debug_msgs autoware_object_recognition_utils autoware_perception_msgs autoware_tensorrt_common diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp index 06540f2b7cd19..74d8d73ecaed4 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox.cpp @@ -155,13 +155,11 @@ std::vector get_seg_colormap(const std::stri namespace autoware::tensorrt_yolox { TrtYoloX::TrtYoloX( - const std::string & model_path, const std::string & precision, const int num_class, - const float score_threshold, const float nms_threshold, - autoware::tensorrt_common::BuildConfig build_config, const bool use_gpu_preprocess, - const uint8_t gpu_id, std::string calibration_image_list_path, const double norm_factor, - [[maybe_unused]] const std::string & cache_dir, - const autoware::tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size, - const std::string & color_map_path) + TrtCommonConfig & trt_config, const int num_class, const float score_threshold, + const float nms_threshold, const bool use_gpu_preprocess, const uint8_t gpu_id, + std::string calibration_image_list_path, const double norm_factor, + [[maybe_unused]] const std::string & cache_dir, const std::string & color_map_path, + const CalibrationConfig & calib_config) : gpu_id_(gpu_id), is_gpu_initialized_(false) { if (!setCudaDeviceId(gpu_id_)) { @@ -172,13 +170,29 @@ TrtYoloX::TrtYoloX( src_width_ = -1; src_height_ = -1; norm_factor_ = norm_factor; - batch_size_ = batch_config[2]; multitask_ = 0; sematic_color_map_ = get_seg_colormap(color_map_path); stream_ = makeCudaStream(); - if (precision == "int8") { - if (build_config.clip_value <= 0.0) { + trt_common_ = std::make_unique(trt_config); + + const auto network_input_dims = trt_common_->getTensorShape(0); + batch_size_ = network_input_dims.d[0]; + const auto input_channel = network_input_dims.d[1]; + const auto input_height = network_input_dims.d[2]; + const auto input_width = network_input_dims.d[3]; + + auto profile_dims_ptr = std::make_unique>(); + + std::vector profile_dims{ + autoware::tensorrt_common::ProfileDims( + 0, {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}}, + {4, {batch_size_, input_channel, input_height, input_width}})}; + *profile_dims_ptr = profile_dims; + + if (trt_config.precision == "int8") { + if (calib_config.clip_value <= 0.0) { if (calibration_image_list_path.empty()) { throw std::runtime_error( "calibration_image_list_path should be passed to generate int8 engine " @@ -189,19 +203,17 @@ TrtYoloX::TrtYoloX( calibration_image_list_path = ""; } - int max_batch_size = batch_size_; - nvinfer1::Dims input_dims = autoware::tensorrt_common::get_input_dims(model_path); std::vector calibration_images; if (calibration_image_list_path != "") { calibration_images = loadImageList(calibration_image_list_path, ""); } - tensorrt_yolox::ImageStream stream(max_batch_size, input_dims, calibration_images); - fs::path calibration_table{model_path}; + tensorrt_yolox::ImageStream stream(batch_size_, network_input_dims, calibration_images); + fs::path calibration_table{trt_config.onnx_path}; std::string ext = ""; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { ext = "EntropyV2-"; } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { ext = "Legacy-"; } else { ext = "MinMax-"; @@ -209,17 +221,17 @@ TrtYoloX::TrtYoloX( ext += "calibration.table"; calibration_table.replace_extension(ext); - fs::path histogram_table{model_path}; + fs::path histogram_table{trt_config.onnx_path}; ext = "histogram.table"; histogram_table.replace_extension(ext); std::unique_ptr calibrator; - if (build_config.calib_type_str == "Entropy") { + if (calib_config.calib_type_str == "Entropy") { calibrator.reset( new tensorrt_yolox::Int8EntropyCalibrator(stream, calibration_table, norm_factor_)); } else if ( - build_config.calib_type_str == "Legacy" || build_config.calib_type_str == "Percentile") { + calib_config.calib_type_str == "Legacy" || calib_config.calib_type_str == "Percentile") { const double quantile = 0.999999; const double cutoff = 0.999999; calibrator.reset(new tensorrt_yolox::Int8LegacyCalibrator( @@ -228,22 +240,20 @@ TrtYoloX::TrtYoloX( calibrator.reset( new tensorrt_yolox::Int8MinMaxCalibrator(stream, calibration_table, norm_factor_)); } - trt_common_ = std::make_unique( - model_path, precision, std::move(calibrator), batch_config, max_workspace_size, build_config); + if (!trt_common_->setupWithCalibrator( + std::move(calibrator), calib_config, std::move((profile_dims_ptr)))) { + throw std::runtime_error("Failed to setup TensorRT engine with calibrator"); + } } else { - trt_common_ = std::make_unique( - model_path, precision, nullptr, batch_config, max_workspace_size, build_config); - } - trt_common_->setup(); - - if (!trt_common_->isInitialized()) { - return; + if (!trt_common_->setup(std::move(profile_dims_ptr))) { + throw std::runtime_error("Failed to setup TensorRT engine"); + } } // Judge whether decoding output is required // Plain models require decoding, while models with EfficientNMS_TRT module don't. // If getNbBindings == 5, the model contains EfficientNMS_TRT - switch (trt_common_->getNbBindings()) { + switch (trt_common_->getNbIOTensors()) { case 2: // Specified model is plain one. // Bindings are: [input, output] @@ -273,16 +283,16 @@ TrtYoloX::TrtYoloX( */ } // GPU memory allocation - const auto input_dims = trt_common_->getBindingDimensions(0); + const auto input_dims = trt_common_->getTensorShape(0); const auto input_size = std::accumulate(input_dims.d + 1, input_dims.d + input_dims.nbDims, 1, std::multiplies()); if (needs_output_decode_) { - const auto output_dims = trt_common_->getBindingDimensions(1); - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); + const auto output_dims = trt_common_->getTensorShape(1); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); out_elem_num_ = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num_ = out_elem_num_ * batch_config[2]; - out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_config[2]); + out_elem_num_ = out_elem_num_ * batch_size_; + out_elem_num_per_batch_ = static_cast(out_elem_num_ / batch_size_); out_prob_d_ = autoware::cuda_utils::make_unique(out_elem_num_); out_prob_h_ = autoware::cuda_utils::make_unique_host(out_elem_num_, cudaHostAllocPortable); @@ -298,29 +308,27 @@ TrtYoloX::TrtYoloX( output_strides_ = {8, 16, 32, 4}; } } else { - const auto out_scores_dims = trt_common_->getBindingDimensions(3); + const auto out_scores_dims = trt_common_->getTensorShape(3); max_detections_ = out_scores_dims.d[1]; - input_d_ = autoware::cuda_utils::make_unique(batch_config[2] * input_size); - out_num_detections_d_ = autoware::cuda_utils::make_unique(batch_config[2]); - out_boxes_d_ = - autoware::cuda_utils::make_unique(batch_config[2] * max_detections_ * 4); - out_scores_d_ = autoware::cuda_utils::make_unique(batch_config[2] * max_detections_); - out_classes_d_ = - autoware::cuda_utils::make_unique(batch_config[2] * max_detections_); + input_d_ = autoware::cuda_utils::make_unique(batch_size_ * input_size); + out_num_detections_d_ = autoware::cuda_utils::make_unique(batch_size_); + out_boxes_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_ * 4); + out_scores_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_); + out_classes_d_ = autoware::cuda_utils::make_unique(batch_size_ * max_detections_); } if (multitask_) { // Allocate buffer for segmentation segmentation_out_elem_num_ = 0; for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections size_t out_elem_num = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); - out_elem_num = out_elem_num * batch_config[2]; + out_elem_num = out_elem_num * batch_size_; segmentation_out_elem_num_ += out_elem_num; } segmentation_out_elem_num_per_batch_ = - static_cast(segmentation_out_elem_num_ / batch_config[2]); + static_cast(segmentation_out_elem_num_ / batch_size_); segmentation_out_prob_d_ = autoware::cuda_utils::make_unique(segmentation_out_elem_num_); segmentation_out_prob_h_ = autoware::cuda_utils::make_unique_host( @@ -341,16 +349,22 @@ TrtYoloX::TrtYoloX( TrtYoloX::~TrtYoloX() { - if (use_gpu_preprocess_) { - if (image_buf_h_) { - image_buf_h_.reset(); - } - if (image_buf_d_) { - image_buf_d_.reset(); - } - if (argmax_buf_d_) { - argmax_buf_d_.reset(); + try { + if (use_gpu_preprocess_) { + if (image_buf_h_) { + image_buf_h_.reset(); + } + if (image_buf_d_) { + image_buf_d_.reset(); + } + if (argmax_buf_d_) { + argmax_buf_d_.reset(); + } } + } catch (const std::exception & e) { + std::cerr << "Exception in TrtYoloX destructor: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in TrtYoloX destructor" << std::endl; } } @@ -381,7 +395,7 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) src_width_ = width; src_height_ = height; if (use_gpu_preprocess_) { - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); bool const hasRuntimeDim = std::any_of( input_dims.d, input_dims.d + input_dims.nbDims, [](int32_t input_dim) { return input_dim == -1; }); @@ -389,7 +403,9 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) input_dims.d[0] = batch_size_; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } scales_.clear(); } const float input_height = static_cast(input_dims.d[2]); @@ -408,7 +424,7 @@ void TrtYoloX::initPreprocessBuffer(int width, int height) size_t argmax_out_elem_num = 0; for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections const float scale = std::min( output_dims.d[3] / static_cast(width), output_dims.d[2] / static_cast(height)); @@ -435,7 +451,7 @@ void TrtYoloX::printProfiling(void) void TrtYoloX::preprocessGpu(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -463,7 +479,9 @@ void TrtYoloX::preprocessGpu(const std::vector & images) src_height_ = height; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } scales_.clear(); } const float input_height = static_cast(input_dims.d[2]); @@ -489,7 +507,7 @@ void TrtYoloX::preprocessGpu(const std::vector & images) if (multitask_) { for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0: input, 1: output for detections + trt_common_->getTensorShape(m + 2); // 0: input, 1: output for detections const float scale = std::min( output_dims.d[3] / static_cast(image.cols), output_dims.d[2] / static_cast(image.rows)); @@ -524,9 +542,11 @@ void TrtYoloX::preprocessGpu(const std::vector & images) void TrtYoloX::preprocess(const std::vector & images) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -561,9 +581,6 @@ bool TrtYoloX::doInference( if (!setCudaDeviceId(gpu_id_)) { return false; } - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { preprocessGpu(images); @@ -582,7 +599,7 @@ void TrtYoloX::preprocessWithRoiGpu( const std::vector & images, const std::vector & rois) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; for (const auto & image : images) { @@ -604,7 +621,9 @@ void TrtYoloX::preprocessWithRoiGpu( src_height_ = height; } if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -656,9 +675,11 @@ void TrtYoloX::preprocessWithRoi( const std::vector & images, const std::vector & rois) { const auto batch_size = images.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -694,7 +715,7 @@ void TrtYoloX::preprocessWithRoi( void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector & rois) { const auto batch_size = rois.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; @@ -716,7 +737,9 @@ void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector< src_height_ = height; if (!image_buf_h_) { - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); @@ -764,9 +787,11 @@ void TrtYoloX::multiScalePreprocessGpu(const cv::Mat & image, const std::vector< void TrtYoloX::multiScalePreprocess(const cv::Mat & image, const std::vector & rois) { const auto batch_size = rois.size(); - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); input_dims.d[0] = batch_size; - trt_common_->setBindingDimensions(0, input_dims); + if (!trt_common_->setInputShape(0, input_dims)) { + return; + } const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector dst_images; @@ -800,9 +825,6 @@ void TrtYoloX::multiScalePreprocess(const cv::Mat & image, const std::vector & images, ObjectArrays & objects, const std::vector & rois) { - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { preprocessWithRoiGpu(images, rois); } else { @@ -821,9 +843,6 @@ bool TrtYoloX::doInferenceWithRoi( bool TrtYoloX::doMultiScaleInference( const cv::Mat & image, ObjectArrays & objects, const std::vector & rois) { - if (!trt_common_->isInitialized()) { - return false; - } if (use_gpu_preprocess_) { multiScalePreprocessGpu(image, rois); } else { @@ -843,8 +862,10 @@ bool TrtYoloX::feedforward(const std::vector & images, ObjectArrays & o std::vector buffers = { input_d_.get(), out_num_detections_d_.get(), out_boxes_d_.get(), out_scores_d_.get(), out_classes_d_.get()}; - - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); const auto batch_size = images.size(); auto out_num_detections = std::make_unique(batch_size); @@ -897,7 +918,11 @@ bool TrtYoloX::feedforwardAndDecode( if (multitask_) { buffers = {input_d_.get(), out_prob_d_.get(), segmentation_out_prob_d_.get()}; } - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); const auto batch_size = images.size(); @@ -927,7 +952,7 @@ bool TrtYoloX::feedforwardAndDecode( static_cast(segmentation_out_elem_num_ / segmentation_out_elem_num_per_batch_); for (int m = 0; m < multitask_; m++) { const auto output_dims = - trt_common_->getBindingDimensions(m + 2); // 0 : input, 1 : output for detections + trt_common_->getTensorShape(m + 2); // 0 : input, 1 : output for detections size_t out_elem_num = std::accumulate( output_dims.d + 1, output_dims.d + output_dims.nbDims, 1, std::multiplies()); out_elem_num = out_elem_num * batch; @@ -966,7 +991,10 @@ bool TrtYoloX::multiScaleFeedforward(const cv::Mat & image, int batch_size, Obje input_d_.get(), out_num_detections_d_.get(), out_boxes_d_.get(), out_scores_d_.get(), out_classes_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); auto out_num_detections = std::make_unique(batch_size); auto out_boxes = std::make_unique(4 * batch_size * max_detections_); @@ -1014,7 +1042,10 @@ bool TrtYoloX::multiScaleFeedforwardAndDecode( const cv::Mat & image, int batch_size, ObjectArrays & objects) { std::vector buffers = {input_d_.get(), out_prob_d_.get()}; - trt_common_->enqueueV2(buffers.data(), *stream_, nullptr); + if (!trt_common_->setTensorsAddresses(buffers)) { + return false; + } + trt_common_->enqueueV3(*stream_); CHECK_CUDA_ERROR(cudaMemcpyAsync( out_prob_h_.get(), out_prob_d_.get(), sizeof(float) * out_elem_num_, cudaMemcpyDeviceToHost, @@ -1036,7 +1067,7 @@ void TrtYoloX::decodeOutputs( float * prob, ObjectArray & objects, float scale, cv::Size & img_size) const { ObjectArray proposals; - auto input_dims = trt_common_->getBindingDimensions(0); + auto input_dims = trt_common_->getTensorShape(0); const float input_height = static_cast(input_dims.d[2]); const float input_width = static_cast(input_dims.d[3]); std::vector grid_strides; @@ -1292,4 +1323,9 @@ void TrtYoloX::getColorizedMask( } } +int TrtYoloX::getBatchSize() const +{ + return batch_size_; +} + } // namespace autoware::tensorrt_yolox diff --git a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp index 94a3a37a4d08f..c8a6b676e6c32 100644 --- a/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -83,19 +83,18 @@ TrtYoloXNode::TrtYoloXNode(const rclcpp::NodeOptions & node_options) roi_overlay_segment_labels_.ANIMAL = declare_parameter("roi_overlay_segment_label.ANIMAL"); replaceLabelMap(); - autoware::tensorrt_common::BuildConfig build_config( - calibration_algorithm, dla_core_id, quantize_first_layer, quantize_last_layer, - profile_per_layer, clip_value); + TrtCommonConfig trt_config( + model_path, precision, "", (1ULL << 30U), dla_core_id, profile_per_layer); + + CalibrationConfig calib_config( + calibration_algorithm, quantize_first_layer, quantize_last_layer, clip_value); const double norm_factor = 1.0; const std::string cache_dir = ""; - const autoware::tensorrt_common::BatchConfig batch_config{1, 1, 1}; - const size_t max_workspace_size = (1 << 30); trt_yolox_ = std::make_unique( - model_path, precision, label_map_.size(), score_threshold, nms_threshold, build_config, - preprocess_on_gpu, gpu_id, calibration_image_list_path, norm_factor, cache_dir, batch_config, - max_workspace_size, color_map_path); + trt_config, label_map_.size(), score_threshold, nms_threshold, preprocess_on_gpu, gpu_id, + calibration_image_list_path, norm_factor, cache_dir, color_map_path, calib_config); if (!trt_yolox_->isGPUInitialized()) { RCLCPP_ERROR(this->get_logger(), "GPU %d does not exist or is not suitable.", gpu_id); @@ -213,11 +212,11 @@ void TrtYoloXNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) std::chrono::nanoseconds( (this->get_clock()->now() - out_objects.header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } diff --git a/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp index 243e82c65dab9..63c0ee0cd3e8a 100644 --- a/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp +++ b/perception/autoware_tensorrt_yolox/src/yolox_single_image_inference_node.cpp @@ -46,7 +46,9 @@ class YoloXSingleImageInferenceNode : public rclcpp::Node const auto output_image_path = declare_parameter("output_image_path", p.string() + "_detect" + ext); - auto trt_yolox = std::make_unique(model_path, precision); + auto trt_config = tensorrt_common::TrtCommonConfig(model_path, precision); + + auto trt_yolox = std::make_unique(trt_config); auto image = cv::imread(image_path); tensorrt_yolox::ObjectArrays objects; std::vector masks; diff --git a/perception/autoware_tracking_object_merger/CHANGELOG.rst b/perception/autoware_tracking_object_merger/CHANGELOG.rst index 337e7028ee61f..765f639a58918 100644 --- a/perception/autoware_tracking_object_merger/CHANGELOG.rst +++ b/perception/autoware_tracking_object_merger/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package autoware_tracking_object_merger ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_tracking_object_merger)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_tracking_object_merger (`#9899 `_) +* feat(autoware_object_merger, autoware_tracking_object_merger): enable anonymized node names to be configurable (`#9733 `_) + feat: enable anonymized node names to be configurable +* fix(autoware_tracking_object_merger): fix bugprone-branch-clone (`#9662 `_) + * fix: bugprone-error + * fix: fmt + --------- +* Contributors: Fumiya Watanabe, Taekjin LEE, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml index cd609a0fa612a..e2892bc75dfe0 100644 --- a/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml +++ b/perception/autoware_tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -1,5 +1,6 @@ + @@ -7,7 +8,7 @@ - + diff --git a/perception/autoware_tracking_object_merger/package.xml b/perception/autoware_tracking_object_merger/package.xml index 2b19ee381258f..d61479f44f31d 100644 --- a/perception/autoware_tracking_object_merger/package.xml +++ b/perception/autoware_tracking_object_merger/package.xml @@ -2,7 +2,7 @@ autoware_tracking_object_merger - 0.40.0 + 0.41.0 merge tracking object Yukihiro Saito Yoshi Ri diff --git a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp index 44912e2242eb2..91345c2d10685 100644 --- a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp @@ -247,9 +247,9 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( published_time_publisher_->publish_if_subscribed( merged_object_pub_, tracked_objects.header.stamp); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/cyclic_time_ms", stop_watch_ptr_->toc("cyclic_time", true)); - processing_time_publisher_->publish( + processing_time_publisher_->publish( "debug/processing_time_ms", stop_watch_ptr_->toc("processing_time", true)); } diff --git a/perception/autoware_tracking_object_merger/src/utils/utils.cpp b/perception/autoware_tracking_object_merger/src/utils/utils.cpp index 15d130b4fcd50..2d7b930da9e66 100644 --- a/perception/autoware_tracking_object_merger/src/utils/utils.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/utils.cpp @@ -98,10 +98,10 @@ TrackedObject linearInterpolationForTrackedObject( output_shape.dimensions.x = shape1.dimensions.x * (1 - weight) + shape2.dimensions.x * weight; output_shape.dimensions.y = shape1.dimensions.y * (1 - weight) + shape2.dimensions.y * weight; output_shape.dimensions.z = shape1.dimensions.z * (1 - weight) + shape2.dimensions.z * weight; - } else if (shape1.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - // (TODO) implement - } else if (shape1.type == autoware_perception_msgs::msg::Shape::POLYGON) { - // (TODO) implement + } else if (shape1.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // NOLINT + // (TODO) implement and remove NOLINT + } else if (shape1.type == autoware_perception_msgs::msg::Shape::POLYGON) { // NOLINT + // (TODO) implement and remove NOLINT } else { // when type is unknown, print warning and do nothing std::cerr << "unknown shape type in linearInterpolationForTrackedObject function." diff --git a/perception/autoware_traffic_light_arbiter/CHANGELOG.rst b/perception/autoware_traffic_light_arbiter/CHANGELOG.rst index 0b3182907a8ce..a086c29ae65f7 100644 --- a/perception/autoware_traffic_light_arbiter/CHANGELOG.rst +++ b/perception/autoware_traffic_light_arbiter/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package autoware_traffic_light_arbiter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_traffic_light_arbiter): add current time validation (`#9747 `_) + * add current time validation + * style(pre-commit): autofix + * change ros parameter name + * style(pre-commit): autofix + * add validation with absolute function + * add timestamp of topic in test + * fix ci error + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Masato Saeki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_traffic_light_arbiter/README.md b/perception/autoware_traffic_light_arbiter/README.md index 619154e1e183b..f27afab62818e 100644 --- a/perception/autoware_traffic_light_arbiter/README.md +++ b/perception/autoware_traffic_light_arbiter/README.md @@ -43,7 +43,8 @@ The table below outlines how the matching process determines the output based on | Name | Type | Default Value | Description | | --------------------------- | ------ | ------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging | -| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging | +| `external_delay_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging in comparison with current time | +| `external_time_tolerance` | double | 5.0 | The duration in seconds an external message is considered valid for merging in comparison with a timestamp of perception message | +| `perception_time_tolerance` | double | 1.0 | The duration in seconds a perception message is considered valid for merging in comparison with a timestamp of external message | | `external_priority` | bool | false | Whether or not externals signals take precedence over perception-based ones. If false, the merging uses confidence as a criteria | | `enable_signal_matching` | bool | false | Decide whether to validate the match between perception signals and external signals. If set to true, verify that the colors match and only publish them if they are identical | diff --git a/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml b/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml index dfe12ff352f16..36e1b8593bebc 100644 --- a/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml +++ b/perception/autoware_traffic_light_arbiter/config/traffic_light_arbiter.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + external_delay_tolerance: 5.0 external_time_tolerance: 5.0 perception_time_tolerance: 1.0 external_priority: false diff --git a/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp index ccd928d52b367..916bd04a6bdd0 100644 --- a/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp +++ b/perception/autoware_traffic_light_arbiter/include/autoware/traffic_light_arbiter/traffic_light_arbiter.hpp @@ -51,6 +51,7 @@ class TrafficLightArbiter : public rclcpp::Node std::unique_ptr> map_regulatory_elements_set_; + double external_delay_tolerance_; double external_time_tolerance_; double perception_time_tolerance_; bool external_priority_; diff --git a/perception/autoware_traffic_light_arbiter/package.xml b/perception/autoware_traffic_light_arbiter/package.xml index 03d6ad5cb6cf4..abf0e2c2204cd 100644 --- a/perception/autoware_traffic_light_arbiter/package.xml +++ b/perception/autoware_traffic_light_arbiter/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_arbiter - 0.40.0 + 0.41.0 The autoware_traffic_light_arbiter package Kenzo Lobos-Tsunekawa Shunsuke Miura diff --git a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp index e71629fa5dd28..8ce002780813f 100644 --- a/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp +++ b/perception/autoware_traffic_light_arbiter/src/traffic_light_arbiter.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -70,6 +71,7 @@ namespace autoware TrafficLightArbiter::TrafficLightArbiter(const rclcpp::NodeOptions & options) : Node("traffic_light_arbiter", options) { + external_delay_tolerance_ = this->declare_parameter("external_delay_tolerance", 5.0); external_time_tolerance_ = this->declare_parameter("external_time_tolerance", 5.0); perception_time_tolerance_ = this->declare_parameter("perception_time_tolerance", 1.0); external_priority_ = this->declare_parameter("external_priority", false); @@ -119,7 +121,7 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP latest_perception_msg_ = *msg; if ( - (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds() > + std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_external_msg_.stamp)).seconds()) > external_time_tolerance_) { latest_external_msg_.traffic_light_groups.clear(); } @@ -129,10 +131,16 @@ void TrafficLightArbiter::onPerceptionMsg(const TrafficSignalArray::ConstSharedP void TrafficLightArbiter::onExternalMsg(const TrafficSignalArray::ConstSharedPtr msg) { + if (std::abs((this->now() - rclcpp::Time(msg->stamp)).seconds()) > external_delay_tolerance_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "Received outdated V2X traffic signal messages"); + return; + } + latest_external_msg_ = *msg; if ( - (rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds() > + std::abs((rclcpp::Time(msg->stamp) - rclcpp::Time(latest_perception_msg_.stamp)).seconds()) > perception_time_tolerance_) { latest_perception_msg_.traffic_light_groups.clear(); } @@ -229,6 +237,13 @@ void TrafficLightArbiter::arbitrateAndPublish(const builtin_interfaces::msg::Tim } pub_->publish(output_signals_msg); + + const auto latest_time = + std::max(rclcpp::Time(latest_perception_msg_.stamp), rclcpp::Time(latest_external_msg_.stamp)); + if (rclcpp::Time(output_signals_msg.stamp) < latest_time) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), 5000, "Published traffic signal messages are not latest"); + } } } // namespace autoware diff --git a/perception/autoware_traffic_light_arbiter/test/test_node.cpp b/perception/autoware_traffic_light_arbiter/test/test_node.cpp index f993ad7cec84d..44b3ca7925fd0 100644 --- a/perception/autoware_traffic_light_arbiter/test/test_node.cpp +++ b/perception/autoware_traffic_light_arbiter/test/test_node.cpp @@ -196,6 +196,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyPerceptionMsg) }; test_manager->set_subscriber(output_topic, callback); + // stamp preparation + perception_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg( @@ -229,6 +232,9 @@ TEST(TrafficLightArbiterTest, testTrafficSignalOnlyExternalMsg) }; test_manager->set_subscriber(output_topic, callback); + // stamp preparation + external_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg( @@ -268,6 +274,10 @@ TEST(TrafficLightArbiterTest, testTrafficSignalBothMsg) }; test_manager->set_subscriber(output_topic, callback); + // stamp preparation + external_msg.stamp = test_target_node->now(); + perception_msg.stamp = test_target_node->now(); + test_manager->test_pub_msg( test_target_node, input_map_topic, vector_map_msg, rclcpp::QoS(1).transient_local()); test_manager->test_pub_msg( diff --git a/perception/autoware_traffic_light_classifier/CHANGELOG.rst b/perception/autoware_traffic_light_classifier/CHANGELOG.rst index db6ddb6b18f87..ee564098cc8f0 100644 --- a/perception/autoware_traffic_light_classifier/CHANGELOG.rst +++ b/perception/autoware_traffic_light_classifier/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package autoware_traffic_light_classifier ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(autoware_traffic_light_classifier): modify docs (`#9819 `_) + * modify docs + * style(pre-commit): autofix + * fix docs + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* Contributors: Amadeusz Szymko, Fumiya Watanabe, Masato Saeki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_traffic_light_classifier/CMakeLists.txt b/perception/autoware_traffic_light_classifier/CMakeLists.txt index d132577743ab0..181037caebfa5 100644 --- a/perception/autoware_traffic_light_classifier/CMakeLists.txt +++ b/perception/autoware_traffic_light_classifier/CMakeLists.txt @@ -34,7 +34,6 @@ if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) if(CUDA_VERBOSE) message(STATUS "TensorRT is available!") message(STATUS "NVINFER: ${NVINFER}") - message(STATUS "NVPARSERS: ${NVPARSERS}") message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") endif() diff --git a/perception/autoware_traffic_light_classifier/README.md b/perception/autoware_traffic_light_classifier/README.md index 6e720aabc7593..7dcd4a73380bb 100644 --- a/perception/autoware_traffic_light_classifier/README.md +++ b/perception/autoware_traffic_light_classifier/README.md @@ -1,22 +1,33 @@ -# traffic_light_classifier +# autoware_traffic_light_classifier ## Purpose -traffic_light_classifier is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. +`autoware_traffic_light_classifier` is a package for classifying traffic light labels using cropped image around a traffic light. This package has two classifier models: `cnn_classifier` and `hsv_classifier`. ## Inner-workings / Algorithms +If height and width of `~/input/rois` is `0`, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `CIRCLE`, and `0.0`. +If `~/input/rois` is judged as backlight, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `UNKNOWN`, and `0.0`. + ### cnn_classifier -Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. -Totally 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. -The information of the models is listed here: +Traffic light labels are classified by EfficientNet-b1 or MobileNet-v2. +We trained classifiers for vehicular signals and pedestrian signals separately. +For vehicular signals, a total of 83400 (58600 for training, 14800 for evaluation and 10000 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. | Name | Input Size | Test Accuracy | | --------------- | ---------- | ------------- | | EfficientNet-b1 | 128 x 128 | 99.76% | | MobileNet-v2 | 224 x 224 | 99.81% | +For pedestrian signals, a total of 21199 (17860 for training, 2114 for evaluation and 1225 for test) TIER IV internal images of Japanese traffic lights were used for fine-tuning. +The information of the models is listed here: + +| Name | Input Size | Test Accuracy | +| --------------- | ---------- | ------------- | +| EfficientNet-b1 | 128 x 128 | 97.89% | +| MobileNet-v2 | 224 x 224 | 99.10% | + ### hsv_classifier Traffic light colors (green, yellow and red) are classified in HSV model. @@ -52,11 +63,12 @@ These colors and shapes are assigned to the message as follows: ### Node Parameters -| Name | Type | Description | -| --------------------- | ----- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `classifier_type` | int | if the value is `1`, cnn_classifier is used | -| `data_path` | str | packages data and artifacts directory path | -| `backlight_threshold` | float | If the intensity get grater than this overwrite with UNKNOWN in corresponding RoI. Note that, if the value is much higher, the node only overwrites in the harsher backlight situations. Therefore, If you wouldn't like to use this feature set this value to `1.0`. The value can be `[0.0, 1.0]`. The confidence of overwritten signal is set to `0.0`. | +| Name | Type | Description | +| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `classifier_type` | int | If the value is `1`, cnn_classifier is used | +| `data_path` | str | Packages data and artifacts directory path | +| `backlight_threshold` | double | If the intensity of light is grater than this threshold, the color and shape of the corresponding ROI will be overwritten with UNKNOWN, and the confidence of the overwritten signal will be set to `0.0`. The value should be set in the range of `[0.0, 1.0]`. If you wouldn't like to use this feature, please set it to `1.0`. | +| `classify_traffic_light_type` | int | If the value is `0`, vehicular signals are classified. If the value is `1`, pedestrian signals are classified. | ### Core Parameters diff --git a/perception/autoware_traffic_light_classifier/package.xml b/perception/autoware_traffic_light_classifier/package.xml index d83791916d4b9..8ca6f1685bdcb 100644 --- a/perception/autoware_traffic_light_classifier/package.xml +++ b/perception/autoware_traffic_light_classifier/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_classifier - 0.40.0 + 0.41.0 The autoware_traffic_light_classifier package Yukihiro Saito Shunsuke Miura diff --git a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp index f9bf2509aa2ae..d47cb1500fffd 100644 --- a/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp +++ b/perception/autoware_traffic_light_classifier/src/classifier/cnn_classifier.cpp @@ -51,13 +51,10 @@ CNNClassifier::CNNClassifier(rclcpp::Node * node_ptr) : node_ptr_(node_ptr) } readLabelfile(label_file_path, labels_); - nvinfer1::Dims input_dim = autoware::tensorrt_common::get_input_dims(model_file_path); - assert(input_dim.d[0] > 0); - batch_size_ = input_dim.d[0]; - autoware::tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; classifier_ = std::make_unique( - model_file_path, precision, batch_config, mean_, std_); + model_file_path, precision, mean_, std_); + batch_size_ = classifier_->getBatchSize(); if (node_ptr_->declare_parameter("build_only", false)) { RCLCPP_INFO(node_ptr_->get_logger(), "TensorRT engine is built and shutdown node."); rclcpp::shutdown(); diff --git a/perception/autoware_traffic_light_fine_detector/CHANGELOG.rst b/perception/autoware_traffic_light_fine_detector/CHANGELOG.rst index 80b1fa60f9e1f..3d6d2ccedea8b 100644 --- a/perception/autoware_traffic_light_fine_detector/CHANGELOG.rst +++ b/perception/autoware_traffic_light_fine_detector/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package autoware_traffic_light_fine_detector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(traffic_light_fine_detector)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in traffic_light_fine_detector (`#9900 `_) +* chore(autoware_traffic_light_fine_detector): modify docs and related params (`#9818 `_) + * modify readme and related params + * fix typo + * fix + --------- +* refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components (`#9762 `_) + * refactor(autoware_tensorrt_common): multi-TensorRT compatibility & tensorrt_common as unified lib for all perception components + * style(pre-commit): autofix + * style(autoware_tensorrt_common): linting + * style(autoware_lidar_centerpoint): typo + Co-authored-by: Kenzo Lobos Tsunekawa + * docs(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_lidar_transfusion): reuse cast variable + * fix(autoware_tensorrt_common): remove deprecated inference API + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * style(autoware_tensorrt_common): grammar + Co-authored-by: Kenzo Lobos Tsunekawa + * fix(autoware_tensorrt_common): const pointer + * fix(autoware_tensorrt_common): remove unused method declaration + * style(pre-commit): autofix + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): return if layer not registered + * refactor(autoware_tensorrt_common): readability + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> + * fix(autoware_tensorrt_common): rename struct + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Kenzo Lobos Tsunekawa + Co-authored-by: Kotaro Uetake <60615504+ktro2828@users.noreply.github.com> +* Contributors: Amadeusz Szymko, Fumiya Watanabe, Masato Saeki, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_traffic_light_fine_detector/CMakeLists.txt b/perception/autoware_traffic_light_fine_detector/CMakeLists.txt index 80d6e43c098ed..dffbab0e77681 100644 --- a/perception/autoware_traffic_light_fine_detector/CMakeLists.txt +++ b/perception/autoware_traffic_light_fine_detector/CMakeLists.txt @@ -39,7 +39,6 @@ if(NVINFER AND NVONNXPARSER AND NVINFER_PLUGIN) if(CUDA_VERBOSE) message(STATUS "TensorRT is available!") message(STATUS "NVINFER: ${NVINFER}") - message(STATUS "NVPARSERS: ${NVPARSERS}") message(STATUS "NVINFER_PLUGIN: ${NVINFER_PLUGIN}") message(STATUS "NVONNXPARSER: ${NVONNXPARSER}") endif() diff --git a/perception/autoware_traffic_light_fine_detector/README.md b/perception/autoware_traffic_light_fine_detector/README.md index 35b55c84aa087..05a3da5d82e64 100644 --- a/perception/autoware_traffic_light_fine_detector/README.md +++ b/perception/autoware_traffic_light_fine_detector/README.md @@ -1,8 +1,8 @@ -# traffic_light_fine_detector +# autoware_traffic_light_fine_detector ## Purpose -It is a package for traffic light detection using YoloX-s. +It is a package for traffic light detection using YOLOX-s. ## Training Information @@ -21,24 +21,25 @@ Please visit [autoware-documentation](https://github.com/autowarefoundation/auto ## Inner-workings / Algorithms -Based on the camera image and the global ROI array detected by `map_based_detection` node, a CNN-based detection method enables highly accurate traffic light detection. +Based on the camera image and the global ROI array detected by `map_based_detector` node, a CNN-based detection method enables highly accurate traffic light detection. If can not detect traffic light, x_offset, y_offset, height and width of output ROI become `0`. +ROIs detected from YOLOX will be selected by a combination of `expect/rois`. At this time, evaluate the whole as ROIs, not just the ROI alone. ## Inputs / Outputs ### Input -| Name | Type | Description | -| --------------- | -------------------------------------------------- | ------------------------------------------------------------------- | -| `~/input/image` | `sensor_msgs/Image` | The full size camera image | -| `~/input/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector | -| `~/expect/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector without any offset | +| Name | Type | Description | +| --------------- | -------------------------------------------------- | -------------------------------------------------------------------------------------------------------------- | +| `~/input/image` | `sensor_msgs::msg::Image` | The full size camera image | +| `~/input/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector | +| `~/expect/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The array of ROIs detected by map_based_detector without any offset, used to select the best detection results | ### Output -| Name | Type | Description | -| --------------------- | -------------------------------------------------- | ---------------------------- | -| `~/output/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The detected accurate rois | -| `~/debug/exe_time_ms` | `tier4_debug_msgs::msg::Float32Stamped` | The time taken for inference | +| Name | Type | Description | +| --------------------- | --------------------------------------------------- | ---------------------------- | +| `~/output/rois` | `tier4_perception_msgs::msg::TrafficLightRoiArray` | The detected accurate rois | +| `~/debug/exe_time_ms` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The time taken for inference | ## Parameters @@ -56,7 +57,7 @@ Based on the camera image and the global ROI array detected by `map_based_detect | `data_path` | string | "$(env HOME)/autoware_data" | packages data and artifacts directory path | | `fine_detector_model_path` | string | "" | The onnx file name for yolo model | | `fine_detector_label_path` | string | "" | The label file with label names for detected objects written on it | -| `fine_detector_precision` | string | "fp32" | The inference mode: "fp32", "fp16" | +| `fine_detector_precision` | string | "fp16" | The inference mode: "fp32", "fp16" | | `approximate_sync` | bool | false | Flag for whether to ues approximate sync policy | | `gpu_id` | integer | 0 | ID for the selecting CUDA GPU device | diff --git a/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml b/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml index 36d4413472a0b..b647729a27de5 100644 --- a/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml +++ b/perception/autoware_traffic_light_fine_detector/config/traffic_light_fine_detector.param.yaml @@ -5,4 +5,5 @@ fine_detector_precision: fp16 fine_detector_score_thresh: 0.3 fine_detector_nms_thresh: 0.65 + approximate_sync: false gpu_id: 0 diff --git a/perception/autoware_traffic_light_fine_detector/package.xml b/perception/autoware_traffic_light_fine_detector/package.xml index c9db06669ea99..719d2442b799e 100644 --- a/perception/autoware_traffic_light_fine_detector/package.xml +++ b/perception/autoware_traffic_light_fine_detector/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_fine_detector - 0.40.0 + 0.41.0 The autoware_traffic_light_fine_detector package Tao Zhong Shunsuke Miura @@ -14,6 +14,7 @@ autoware_cmake + autoware_internal_debug_msgs autoware_tensorrt_yolox cv_bridge image_transport @@ -21,7 +22,6 @@ rclcpp rclcpp_components sensor_msgs - tier4_debug_msgs tier4_perception_msgs autoware_lint_common diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp index 37ffca4a9526c..8e2b2d5b3cfbb 100644 --- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.cpp @@ -61,7 +61,7 @@ TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOpt std::string model_path = declare_parameter("fine_detector_model_path", ""); std::string label_path = declare_parameter("fine_detector_label_path", ""); - std::string precision = declare_parameter("fine_detector_precision", "fp32"); + std::string precision = declare_parameter("fine_detector_precision", "fp16"); const uint8_t gpu_id = declare_parameter("gpu_id", 0); // Objects with a score lower than this value will be ignored. // This threshold will be ignored if specified model contains EfficientNMS_TRT module in it @@ -75,21 +75,18 @@ TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOpt RCLCPP_ERROR(this->get_logger(), "Could not find tlr id"); } - const autoware::tensorrt_common::BuildConfig build_config = - autoware::tensorrt_common::BuildConfig("MinMax", -1, false, false, false, 0.0); - const bool cuda_preprocess = true; const std::string calib_image_list = ""; const double scale = 1.0; const std::string cache_dir = ""; - nvinfer1::Dims input_dim = autoware::tensorrt_common::get_input_dims(model_path); - assert(input_dim.d[0] > 0); - batch_size_ = input_dim.d[0]; - const autoware::tensorrt_common::BatchConfig batch_config{batch_size_, batch_size_, batch_size_}; + + auto trt_config = autoware::tensorrt_common::TrtCommonConfig(model_path, precision); trt_yolox_ = std::make_unique( - model_path, precision, num_class, score_thresh_, nms_threshold, build_config, cuda_preprocess, - gpu_id, calib_image_list, scale, cache_dir, batch_config); + trt_config, num_class, score_thresh_, nms_threshold, cuda_preprocess, gpu_id, calib_image_list, + scale, cache_dir); + + batch_size_ = trt_yolox_->getBatchSize(); if (!trt_yolox_->isGPUInitialized()) { RCLCPP_ERROR(this->get_logger(), "GPU %d does not exist or is not suitable.", gpu_id); @@ -104,8 +101,8 @@ TrafficLightFineDetectorNode::TrafficLightFineDetectorNode(const rclcpp::NodeOpt std::lock_guard lock(connect_mutex_); output_roi_pub_ = this->create_publisher("~/output/rois", 1); - exe_time_pub_ = - this->create_publisher("~/debug/exe_time_ms", 1); + exe_time_pub_ = this->create_publisher( + "~/debug/exe_time_ms", 1); if (is_approximate_sync_) { approximate_sync_.reset( new ApproximateSync(ApproximateSyncPolicy(10), image_sub_, rough_roi_sub_, expect_roi_sub_)); @@ -211,7 +208,7 @@ void TrafficLightFineDetectorNode::callback( const auto exe_end_time = high_resolution_clock::now(); const double exe_time = std::chrono::duration_cast(exe_end_time - exe_start_time).count(); - tier4_debug_msgs::msg::Float32Stamped exe_time_msg; + autoware_internal_debug_msgs::msg::Float32Stamped exe_time_msg; exe_time_msg.data = exe_time; exe_time_msg.stamp = this->now(); exe_time_pub_->publish(exe_time_msg); diff --git a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.hpp b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.hpp index ada6be3844d77..a1450c0c5230a 100644 --- a/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.hpp +++ b/perception/autoware_traffic_light_fine_detector/src/traffic_light_fine_detector_node.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #include #if __has_include() @@ -148,7 +148,7 @@ class TrafficLightFineDetectorNode : public rclcpp::Node message_filters::Subscriber expect_roi_sub_; std::mutex connect_mutex_; rclcpp::Publisher::SharedPtr output_roi_pub_; - rclcpp::Publisher::SharedPtr exe_time_pub_; + rclcpp::Publisher::SharedPtr exe_time_pub_; rclcpp::TimerBase::SharedPtr timer_; typedef message_filters::sync_policies::ExactTime< diff --git a/perception/autoware_traffic_light_map_based_detector/CHANGELOG.rst b/perception/autoware_traffic_light_map_based_detector/CHANGELOG.rst index 60f84da7131f2..c020c1439900a 100644 --- a/perception/autoware_traffic_light_map_based_detector/CHANGELOG.rst +++ b/perception/autoware_traffic_light_map_based_detector/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package autoware_traffic_light_map_based_detector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(autoware_traffic_light_map_based_detector): modify docs (`#9817 `_) + * modify docs + * fix title + * fix docs + * fix word + * add comment about debug markers + * fix docs + --------- +* Contributors: Fumiya Watanabe, Masato Saeki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_traffic_light_map_based_detector/README.md b/perception/autoware_traffic_light_map_based_detector/README.md index 8a59db19ae64d..6ef54d448b415 100644 --- a/perception/autoware_traffic_light_map_based_detector/README.md +++ b/perception/autoware_traffic_light_map_based_detector/README.md @@ -1,4 +1,4 @@ -# The `autoware_traffic_light_map_based_detector` Package +# autoware_traffic_light_map_based_detector ## Overview @@ -9,34 +9,36 @@ Calibration and vibration errors can be entered as parameters, and the size of t ![traffic_light_map_based_detector_result](./docs/traffic_light_map_based_detector_result.svg) If the node receives route information, it only looks at traffic lights on that route. -If the node receives no route information, it looks at a radius of 200 meters and the angle between the traffic light and the camera is less than 40 degrees. +If the node receives no route information, it looks at them within a radius of `max_detection_range` and the angle between the traffic light and the camera is less than `car_traffic_light_max_angle_range` or `pedestrian_traffic_light_max_angle_range`. ## Input topics -| Name | Type | Description | -| -------------------- | ------------------------------------- | ----------------------- | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter | -| `~input/route` | autoware_planning_msgs::LaneletRoute | optional: route | +| Name | Type | Description | +| --------------------- | ----------------------------------------- | ----------------------- | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/camera_info` | sensor_msgs::msg::CameraInfo | target camera parameter | +| `~/input/route` | autoware_planning_msgs::msg::LaneletRoute | optional: route | ## Output topics -| Name | Type | Description | -| ---------------- | ------------------------------------------- | -------------------------------------------------------------------- | -| `~output/rois` | tier4_perception_msgs::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | -| `~expect/rois` | tier4_perception_msgs::TrafficLightRoiArray | location of traffic lights in image without any offset | -| `~debug/markers` | visualization_msgs::MarkerArray | visualization to debug | +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | ------------------------------------------------------------------------- | +| `~/output/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image corresponding to the camera info | +| `~/expect/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | location of traffic lights in image without any offset | +| `~/debug/markers` | visualization_msgs::msg::MarkerArray | markers which show a line that combines from camera to each traffic light | ## Node parameters -| Parameter | Type | Description | -| ---------------------- | ------ | --------------------------------------------------------------------- | -| `max_vibration_pitch` | double | Maximum error in pitch direction. If -5~+5, it will be 10. | -| `max_vibration_yaw` | double | Maximum error in yaw direction. If -5~+5, it will be 10. | -| `max_vibration_height` | double | Maximum error in height direction. If -5~+5, it will be 10. | -| `max_vibration_width` | double | Maximum error in width direction. If -5~+5, it will be 10. | -| `max_vibration_depth` | double | Maximum error in depth direction. If -5~+5, it will be 10. | -| `max_detection_range` | double | Maximum detection range in meters. Must be positive | -| `min_timestamp_offset` | double | Minimum timestamp offset when searching for corresponding tf | -| `max_timestamp_offset` | double | Maximum timestamp offset when searching for corresponding tf | -| `timestamp_sample_len` | double | sampling length between min_timestamp_offset and max_timestamp_offset | +| Parameter | Type | Description | +| ------------------------------------------ | ------ | ----------------------------------------------------------------------------------------------- | +| `max_vibration_pitch` | double | Maximum error in pitch direction. If -5~+5, it will be 10. | +| `max_vibration_yaw` | double | Maximum error in yaw direction. If -5~+5, it will be 10. | +| `max_vibration_height` | double | Maximum error in height direction. If -5~+5, it will be 10. | +| `max_vibration_width` | double | Maximum error in width direction. If -5~+5, it will be 10. | +| `max_vibration_depth` | double | Maximum error in depth direction. If -5~+5, it will be 10. | +| `max_detection_range` | double | Maximum detection range in meters. Must be positive. | +| `min_timestamp_offset` | double | Minimum timestamp offset when searching for corresponding tf. | +| `max_timestamp_offset` | double | Maximum timestamp offset when searching for corresponding tf. | +| `timestamp_sample_len` | double | Sampling length between min_timestamp_offset and max_timestamp_offset. | +| `car_traffic_light_max_angle_range` | double | Maximum angle between the vehicular traffic light and the camera in degrees. Must be positive. | +| `pedestrian_traffic_light_max_angle_range` | double | Maximum angle between the pedestrian traffic light and the camera in degrees. Must be positive. | diff --git a/perception/autoware_traffic_light_map_based_detector/package.xml b/perception/autoware_traffic_light_map_based_detector/package.xml index c5caca47a0c39..a21374652c7cb 100644 --- a/perception/autoware_traffic_light_map_based_detector/package.xml +++ b/perception/autoware_traffic_light_map_based_detector/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_map_based_detector - 0.40.0 + 0.41.0 The autoware_traffic_light_map_based_detector package Yukihiro Saito Shunsuke Miura diff --git a/perception/autoware_traffic_light_multi_camera_fusion/CHANGELOG.rst b/perception/autoware_traffic_light_multi_camera_fusion/CHANGELOG.rst index 74a7fa049c486..1f2fdfa5b5145 100644 --- a/perception/autoware_traffic_light_multi_camera_fusion/CHANGELOG.rst +++ b/perception/autoware_traffic_light_multi_camera_fusion/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_traffic_light_multi_camera_fusion ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(autoware_traffic_light_multi_camera_fusion): modify docs (`#9821 `_) + * fix docs + * add condition + --------- +* Contributors: Fumiya Watanabe, Masato Saeki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_traffic_light_multi_camera_fusion/README.md b/perception/autoware_traffic_light_multi_camera_fusion/README.md index f7ee294cda147..b54c623f5d750 100644 --- a/perception/autoware_traffic_light_multi_camera_fusion/README.md +++ b/perception/autoware_traffic_light_multi_camera_fusion/README.md @@ -1,29 +1,36 @@ -# The `traffic_light_multi_camera_fusion` Package +# autoware_traffic_light_multi_camera_fusion ## Overview -`traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: +`autoware_traffic_light_multi_camera_fusion` performs traffic light signal fusion which can be summarized as the following two tasks: -1. Multi-Camera-Fusion: performed on single traffic light signal detected by different cameras. -2. Group-Fusion: performed on traffic light signals within the same group, which means traffic lights sharing the same regulatory element id defined in lanelet2 map. +1. Multi-Camera-Fusion: fusion each traffic light signal detected by different cameras. +2. Group-Fusion: Fusion each traffic light signal within the same group, which means traffic lights share the same regulatory element ID defined in lanelet2 map. + +The fusion method is below. + +1. Use the results of the new timestamp if the results are from the same sensor +2. Use the results that are not `elements.size() == 1 && color == UNKNOWN && shape == UNKNOWN` +3. Use the results that each vertex of ROI is not at the edge of the image +4. Use the results of high confidence ## Input topics For every camera, the following three topics are subscribed: -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------- | --------------------------------------------------- | -| `~//camera_info` | sensor_msgs::CameraInfo | camera info from traffic_light_map_based_detector | -| `~//rois` | tier4_perception_msgs::TrafficLightRoiArray | detection roi from traffic_light_fine_detector | -| `~//traffic_signals` | tier4_perception_msgs::TrafficLightSignalArray | classification result from traffic_light_classifier | +| Name | Type | Description | +| ----------------------------------------------------- | ------------------------------------------------ | ------------------------------------- | +| `~//camera_info` | sensor_msgs::msg::CameraInfo | camera info from map_based_detector | +| `~//detection/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | detection roi from fine_detector | +| `~//classification/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | classification result from classifier | You don't need to configure these topics manually. Just provide the `camera_namespaces` parameter and the node will automatically extract the `` and create the subscribers. ## Output topics -| Name | Type | Description | -| -------------------------- | ------------------------------------------------- | ---------------------------------- | -| `~/output/traffic_signals` | autoware_perception_msgs::TrafficLightSignalArray | traffic light signal fusion result | +| Name | Type | Description | +| -------------------------- | ----------------------------------------------------- | ---------------------------------- | +| `~/output/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light signal fusion result | ## Node parameters diff --git a/perception/autoware_traffic_light_multi_camera_fusion/package.xml b/perception/autoware_traffic_light_multi_camera_fusion/package.xml index dfe8657577582..4dd1f53a2e8fa 100644 --- a/perception/autoware_traffic_light_multi_camera_fusion/package.xml +++ b/perception/autoware_traffic_light_multi_camera_fusion/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_multi_camera_fusion - 0.40.0 + 0.41.0 The autoware_traffic_light_multi_camera_fusion package Tao Zhong Shunsuke Miura diff --git a/perception/autoware_traffic_light_occlusion_predictor/CHANGELOG.rst b/perception/autoware_traffic_light_occlusion_predictor/CHANGELOG.rst index 8dec833763cb7..8833224d5b69f 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/CHANGELOG.rst +++ b/perception/autoware_traffic_light_occlusion_predictor/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_traffic_light_occlusion_predictor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(autoware_traffic_light_occlusion_predictor): modify docs (`#9820 `_) + * fix docs + * fix docs + --------- +* Contributors: Fumiya Watanabe, Masato Saeki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_traffic_light_occlusion_predictor/README.md b/perception/autoware_traffic_light_occlusion_predictor/README.md index bc57dbea76c97..5d2e320dfc72a 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/README.md +++ b/perception/autoware_traffic_light_occlusion_predictor/README.md @@ -1,8 +1,10 @@ -# The `autoware_traffic_light_occlusion_predictor` Package +# autoware_traffic_light_occlusion_predictor ## Overview `autoware_traffic_light_occlusion_predictor` receives the detected traffic lights rois and calculates the occlusion ratios of each roi with point cloud. +If that rois is judged as occlusion, color, shape, and confidence of `~/output/traffic_signals` become `UNKNOWN`, `UNKNOWN`, and `0.0`. +This node publishes when each car and pedestrian `traffic_signals` is arrived and processed. For each traffic light roi, hundreds of pixels would be selected and projected into the 3D space. Then from the camera point of view, the number of projected pixels that are occluded by the point cloud is counted and used for calculating the occlusion ratio for the roi. As shown in follow image, the red pixels are occluded and the occlusion ratio is the number of red pixels divided by the total pixel numbers. @@ -12,18 +14,20 @@ If no point cloud is received or all point clouds have very large stamp differen ## Input topics -| Name | Type | Description | -| -------------------- | ---------------------------------------------- | ------------------------ | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~/input/rois` | autoware_perception_msgs::TrafficLightRoiArray | traffic light detections | -| `~input/camera_info` | sensor_msgs::CameraInfo | target camera parameter | -| `~/input/cloud` | sensor_msgs::PointCloud2 | LiDAR point cloud | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------------------ | -------------------------------- | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/car/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | vehicular traffic light signals | +| `~/input/pedestrian/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | pedestrian traffic light signals | +| `~/input/rois` | tier4_perception_msgs::msg::TrafficLightRoiArray | traffic light detections | +| `~/input/camera_info` | sensor_msgs::msg::CameraInfo | target camera parameter | +| `~/input/cloud` | sensor_msgs::msg::PointCloud2 | LiDAR point cloud | ## Output topics -| Name | Type | Description | -| -------------------- | ---------------------------------------------------- | ---------------------------- | -| `~/output/occlusion` | autoware_perception_msgs::TrafficLightOcclusionArray | occlusion ratios of each roi | +| Name | Type | Description | +| -------------------------- | --------------------------------------------- | -------------------------------------------------------------- | +| `~/output/traffic_signals` | tier4_perception_msgs::msg::TrafficLightArray | traffic light signals which occluded image results overwritten | ## Node parameters @@ -34,3 +38,4 @@ If no point cloud is received or all point clouds have very large stamp differen | `max_valid_pt_dist` | double | The points within this distance would be used for calculation | | `max_image_cloud_delay` | double | The maximum delay between LiDAR point cloud and camera image | | `max_wait_t` | double | The maximum time waiting for the LiDAR point cloud | +| `max_occlusion_ratio` | int | The maximum occlusion ratio for setting signal as unknown | diff --git a/perception/autoware_traffic_light_occlusion_predictor/package.xml b/perception/autoware_traffic_light_occlusion_predictor/package.xml index e10dddaf40eaf..a9972c9f73d3c 100644 --- a/perception/autoware_traffic_light_occlusion_predictor/package.xml +++ b/perception/autoware_traffic_light_occlusion_predictor/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_occlusion_predictor - 0.40.0 + 0.41.0 The autoware_traffic_light_occlusion_predictor package Tao Zhong Shunsuke Miura diff --git a/perception/autoware_traffic_light_visualization/CHANGELOG.rst b/perception/autoware_traffic_light_visualization/CHANGELOG.rst index 420b4bf485eec..8429db3fca7f1 100644 --- a/perception/autoware_traffic_light_visualization/CHANGELOG.rst +++ b/perception/autoware_traffic_light_visualization/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_traffic_light_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_traffic_light_visualization): fix bugprone-branch-clone (`#9668 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/autoware_traffic_light_visualization/package.xml b/perception/autoware_traffic_light_visualization/package.xml index afb30d43f2a70..e55b9088a8521 100644 --- a/perception/autoware_traffic_light_visualization/package.xml +++ b/perception/autoware_traffic_light_visualization/package.xml @@ -2,7 +2,7 @@ autoware_traffic_light_visualization - 0.40.0 + 0.41.0 The autoware_traffic_light_visualization package Yukihiro Saito Tao Zhong diff --git a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index ccfaf37fd7c6d..f12eb656033b6 100644 --- a/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/autoware_traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -169,7 +169,7 @@ void TrafficLightMapVisualizerNode::trafficSignalsCallback( visualization_msgs::msg::Marker marker; if ( isAttributeValue(pt, "color", "red") && - elem.color == autoware_perception_msgs::msg::TrafficLightElement::RED) { + elem.color == autoware_perception_msgs::msg::TrafficLightElement::RED) { // NOLINT lightAsMarker( get_node_logging_interface(), pt, &marker, "traffic_light", current_time); } else if ( // NOLINT @@ -177,7 +177,6 @@ void TrafficLightMapVisualizerNode::trafficSignalsCallback( elem.color == autoware_perception_msgs::msg::TrafficLightElement::GREEN) { lightAsMarker( get_node_logging_interface(), pt, &marker, "traffic_light", current_time); - } else if ( // NOLINT isAttributeValue(pt, "color", "yellow") && elem.color == autoware_perception_msgs::msg::TrafficLightElement::AMBER) { diff --git a/perception/perception_utils/CHANGELOG.rst b/perception/perception_utils/CHANGELOG.rst index ddbe6e3f40270..23f2b8565e014 100644 --- a/perception/perception_utils/CHANGELOG.rst +++ b/perception/perception_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package perception_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/perception/perception_utils/package.xml b/perception/perception_utils/package.xml index 058811b048459..07793f979fd51 100644 --- a/perception/perception_utils/package.xml +++ b/perception/perception_utils/package.xml @@ -2,7 +2,7 @@ perception_utils - 0.40.0 + 0.41.0 The perception_utils package Shunsuke Miura Yoshi Ri diff --git a/planning/.pages b/planning/.pages index 424f83cf47611..7444815cdd28a 100644 --- a/planning/.pages +++ b/planning/.pages @@ -39,7 +39,7 @@ nav: - 'About Freespace Planner': planning/autoware_freespace_planner - 'Algorithm': planning/autoware_freespace_planning_algorithms - 'RRT*': planning/autoware_freespace_planning_algorithms/rrtstar - - 'Mission Planner': planning/autoware_mission_planner + - 'Mission Planner': planning/autoware_mission_planner_universe - 'Motion Planning': - 'Path Optimizer': - 'About Path Optimizer': planning/autoware_path_optimizer @@ -61,7 +61,7 @@ nav: - '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/motion_velocity_planner/autoware_motion_velocity_planner_node/ + - 'About Motion Velocity Planner': planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/ - 'Available Modules': - 'Dynamic Obstacle Stop': planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/ - 'Out of Lane': planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/ diff --git a/planning/README.md b/planning/README.md index ccf8288df3911..3fadafe54ed4c 100644 --- a/planning/README.md +++ b/planning/README.md @@ -18,7 +18,7 @@ Enabling and disabling modules involves managing settings in key configuration a ### Key Files for Configuration -The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disable or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: +The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disabled or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: - `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. - `motion_stop_planner_type`: Set `default` to either `obstacle_stop_planner` or `obstacle_cruise_planner`. @@ -35,7 +35,7 @@ The [launch files](https://github.com/autowarefoundation/autoware.universe/tree/ corresponds to launch_avoidance_module from `default_preset.yaml`. -### Parameters configuration +### Parameter Configuration There are multiple parameters available for configuration, and users have the option to modify them in [here](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning). It's important to note that not all parameters are adjustable via `rqt_reconfigure`. To ensure the changes are effective, modify the parameters and then restart Autoware. Additionally, detailed information about each parameter is available in the corresponding documents under the planning tab. @@ -43,7 +43,7 @@ There are multiple parameters available for configuration, and users have the op This guide outlines the steps for integrating your custom module into Autoware: -- Add your modules to the `default_preset.yaml` file. For example +- Add your modules to the `default_preset.yaml` file. For example: ```yaml - arg: @@ -51,7 +51,7 @@ This guide outlines the steps for integrating your custom module into Autoware: default: "true" ``` -- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): +- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example, in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): ```xml @@ -63,14 +63,14 @@ This guide outlines the steps for integrating your custom module into Autoware: /> ``` -- If applicable, place your parameter folder within the appropriate existing parameter folder. For example [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) is in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). -- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example `behavior_velocity_planner_intersection_module_param_path` is used. +- If applicable, place your parameter folder within the appropriate existing parameter folder. For example, [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) are in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). +- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example, `behavior_velocity_planner_intersection_module_param_path` is used. ```xml ``` -- Define your parameter path variable within the corresponding launcher. For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) +- Define your parameter path variable within the corresponding launcher. For example, in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) ```xml @@ -82,7 +82,7 @@ This guide outlines the steps for integrating your custom module into Autoware: ## Join Our Community-Driven Effort -Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome it all with open arms. +Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome all contributions with open arms. ### How to Contribute? @@ -105,7 +105,7 @@ Interested in joining our meetings? We’d love to have you! For more informatio Occasionally, we publish papers specific to the Planning Component in Autoware. We encourage you to explore these publications and find valuable insights for your work. If you find them useful and incorporate any of our methodologies or algorithms in your projects, citing our papers would be immensely helpful. This support allows us to reach a broader audience and continue contributing to the field. -If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. +If you use the Jerk Constrained Velocity Planning algorithm in the [Motion Velocity Smoother](./autoware_velocity_smoother/README.md) module in the Planning Component, we kindly request you cite the relevant paper. diff --git a/planning/autoware_costmap_generator/CHANGELOG.rst b/planning/autoware_costmap_generator/CHANGELOG.rst index 6215cd6361b49..398a155c2f1d7 100644 --- a/planning/autoware_costmap_generator/CHANGELOG.rst +++ b/planning/autoware_costmap_generator/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package autoware_costmap_generator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_costmap_generator): tier4_debug_msgs changed to autoware_internal-debug_msgs in autoware_costmap_generator (`#9901 `_) + feat: tier4_debug_msgs changed to autoware_internal-debug_msgs in files planning/autoware_costmap_generator +* fix(autoware_costmap_generator): fix bugprone-branch-clone (`#9669 `_) + fix: bugprone-error +* chore(autoware_costmap_generator): suppress Could not find a connection between 'map' and 'base_link' (`#9655 `_) +* Contributors: Fumiya Watanabe, Vishal Chauhan, Yukinari Hisaki, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp index 4fed5bcb6cff3..b6749117a6177 100644 --- a/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware/costmap_generator/costmap_generator.hpp @@ -58,10 +58,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -99,7 +99,8 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_costmap_; rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Publisher::SharedPtr pub_processing_time_; - rclcpp::Publisher::SharedPtr pub_processing_time_ms_; + rclcpp::Publisher::SharedPtr + pub_processing_time_ms_; rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; autoware::universe_utils::InterProcessPollingSubscriber diff --git a/planning/autoware_costmap_generator/package.xml b/planning/autoware_costmap_generator/package.xml index 9097109de0a0b..d2d0eba64cdd0 100644 --- a/planning/autoware_costmap_generator/package.xml +++ b/planning/autoware_costmap_generator/package.xml @@ -2,7 +2,7 @@ autoware_costmap_generator - 0.40.0 + 0.41.0 The autoware_costmap_generator package Kosuke Takeuchi Takamasa Horibe diff --git a/planning/autoware_costmap_generator/src/costmap_generator.cpp b/planning/autoware_costmap_generator/src/costmap_generator.cpp index 052bb2728a3b1..70b8051bcdc12 100644 --- a/planning/autoware_costmap_generator/src/costmap_generator.cpp +++ b/planning/autoware_costmap_generator/src/costmap_generator.cpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -68,14 +69,15 @@ namespace // Copied from scenario selector geometry_msgs::msg::PoseStamped::ConstSharedPtr getCurrentPose( - const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger) + const tf2_ros::Buffer & tf_buffer, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) { geometry_msgs::msg::TransformStamped tf_current_pose; try { tf_current_pose = tf_buffer.lookupTransform("map", "base_link", tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger, "%s", ex.what()); + RCLCPP_ERROR_THROTTLE(logger, *clock, 5000, "%s", ex.what()); return nullptr; } @@ -174,7 +176,8 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) create_publisher("processing_time", 1); time_keeper_ = std::make_shared(pub_processing_time_); pub_processing_time_ms_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); // Timer const auto period_ns = rclcpp::Rate(param_->update_rate).period(); @@ -253,7 +256,7 @@ void CostmapGenerator::update_data() void CostmapGenerator::set_current_pose() { - current_pose_ = getCurrentPose(tf_buffer_, this->get_logger()); + current_pose_ = getCurrentPose(tf_buffer_, this->get_logger(), this->get_clock()); } void CostmapGenerator::onTimer() @@ -267,7 +270,7 @@ void CostmapGenerator::onTimer() if (!isActive()) { // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_ms_->publish(processing_time_msg); @@ -483,7 +486,7 @@ void CostmapGenerator::publishCostmap( pub_costmap_->publish(*out_gridmap_msg); // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_ms_->publish(processing_time_msg); diff --git a/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp b/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp index 64f558b4f5604..6b1f4331f55ad 100644 --- a/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/src/utils/objects_to_costmap.cpp @@ -200,10 +200,10 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( grid_map::Polygon polygon; if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { polygon = makePolygonFromObjectConvexHull(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { // NOLINT polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { - // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder + // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder and remove NOLINT polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); } const auto highest_probability_label = *std::max_element( diff --git a/planning/autoware_external_velocity_limit_selector/CHANGELOG.rst b/planning/autoware_external_velocity_limit_selector/CHANGELOG.rst index 0dd0f835c1f53..d13758041970f 100644 --- a/planning/autoware_external_velocity_limit_selector/CHANGELOG.rst +++ b/planning/autoware_external_velocity_limit_selector/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_external_velocity_limit_selector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in fil… (`#9902 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_external_velocity_limit_selector +* test(external_velocity_limit_selector): add node test (`#8944 `_) + add node smoke test +* Contributors: Fumiya Watanabe, Maxime CLEMENT, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_external_velocity_limit_selector/CMakeLists.txt b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt index ca758d1262b48..aa8d07d91135e 100644 --- a/planning/autoware_external_velocity_limit_selector/CMakeLists.txt +++ b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt @@ -21,6 +21,13 @@ rclcpp_components_register_node(external_velocity_limit_selector_node EXECUTABLE external_velocity_limit_selector ) +if(BUILD_TESTING) + add_launch_test( + test/test_external_velocity_limit_selector_node_launch.py + TIMEOUT "30" + ) +endif() + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 6a8fae3bf394c..be4893d5a49c6 100644 --- a/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -29,7 +29,7 @@ namespace autoware::external_velocity_limit_selector { -using tier4_debug_msgs::msg::StringStamped; +using autoware_internal_debug_msgs::msg::StringStamped; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; using tier4_planning_msgs::msg::VelocityLimitConstraints; diff --git a/planning/autoware_external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml index b75a4fab72d7c..f7acfcb4d927a 100644 --- a/planning/autoware_external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -2,7 +2,7 @@ autoware_external_velocity_limit_selector - 0.40.0 + 0.41.0 The autoware_external_velocity_limit_selector ROS 2 package Satoshi Ota Shinnosuke Hirakawa @@ -17,10 +17,10 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs generate_parameter_library rclcpp rclcpp_components - tier4_debug_msgs tier4_planning_msgs ros2cli @@ -28,6 +28,7 @@ ament_lint_auto autoware_lint_common + ros_testing ament_cmake diff --git a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index cf42763fc6b60..cc2b134900a31 100644 --- a/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -14,7 +14,6 @@ #include "autoware/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" -#include #include #include #include diff --git a/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py b/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py new file mode 100644 index 0000000000000..1c7e882fbc81f --- /dev/null +++ b/planning/autoware_external_velocity_limit_selector/test/test_external_velocity_limit_selector_node_launch.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.logging import get_logger +import launch_testing +import pytest +from rcl_interfaces.msg import Parameter +from rcl_interfaces.msg import ParameterType +from rcl_interfaces.msg import ParameterValue +from rcl_interfaces.srv import SetParameters +import rclpy +import rclpy.qos +from tier4_planning_msgs.msg import VelocityLimit +from tier4_planning_msgs.msg import VelocityLimitClearCommand + +logger = get_logger(__name__) + + +@pytest.mark.launch_test +def generate_test_description(): + test_external_velocity_limit_selector_launch_file = os.path.join( + get_package_share_directory("autoware_external_velocity_limit_selector"), + "launch", + "external_velocity_limit_selector.launch.xml", + ) + external_velocity_limit_selector = IncludeLaunchDescription( + AnyLaunchDescriptionSource(test_external_velocity_limit_selector_launch_file), + ) + + return launch.LaunchDescription( + [ + external_velocity_limit_selector, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +class TestExternalVelocityLimitSelector(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def velocity_limit_callback(self, msg): + self.msg_buffer_ = msg + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + qos = rclpy.qos.QoSProfile(depth=1, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL) + self.pub_api_limit_ = self.test_node.create_publisher( + VelocityLimit, "/planning/scenario_planning/max_velocity_default", qos + ) + self.pub_internal_limit_ = self.test_node.create_publisher( + VelocityLimit, "/planning/scenario_planning/max_velocity_candidates", qos + ) + self.pub_clear_limit_ = self.test_node.create_publisher( + VelocityLimitClearCommand, "/planning/scenario_planning/clear_velocity_limit", qos + ) + self.msg_buffer_ = None + self.velocity_limit_output_ = None + self.test_node.create_subscription( + VelocityLimit, + "/planning/scenario_planning/max_velocity", + self.velocity_limit_callback, + 1, + ) + + def wait_for_output(self): + while not self.msg_buffer_: + rclpy.spin_once(self.test_node, timeout_sec=0.1) + self.velocity_limit_output_ = self.msg_buffer_ + self.msg_buffer_ = None + + def tearDown(self): + self.test_node.destroy_node() + + def update_max_vel_param(self, max_vel): + set_params_client = self.test_node.create_client( + SetParameters, "/external_velocity_limit_selector/set_parameters" + ) + while not set_params_client.wait_for_service(timeout_sec=1.0): + continue + set_params_request = SetParameters.Request() + set_params_request.parameters = [ + Parameter( + name="max_vel", + value=ParameterValue(type=ParameterType.PARAMETER_DOUBLE, double_value=max_vel), + ), + ] + future = set_params_client.call_async(set_params_request) + rclpy.spin_until_future_complete(self.test_node, future) + + if future.result() is None: + self.test_node.get_logger().error( + "Exception while calling service: %r" % future.exception() + ) + raise self.failureException("setting of initial parameters failed") + + @staticmethod + def make_velocity_limit_msg(vel): + msg = VelocityLimit() + msg.use_constraints = False + msg.max_velocity = vel + return msg + + def test_external_velocity_limit_selector_node(self): + self.update_max_vel_param(15.0) + # clear velocity limit to trigger first output + clear_cmd = VelocityLimitClearCommand(command=True) + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + # velocity limit is 0 before any limit is set + self.assertEqual(self.velocity_limit_output_.max_velocity, 0.0) + + # Send velocity limits + # new API velocity limit: higher than the node param -> limit is set to the param value + api_limit = self.make_velocity_limit_msg(20.0) + self.pub_api_limit_.publish(api_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 15.0) + # new API velocity limit + api_limit = self.make_velocity_limit_msg(10.0) + self.pub_api_limit_.publish(api_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 10.0) + # new INTERNAL velocity limit + internal_limit = self.make_velocity_limit_msg(5.0) + self.pub_internal_limit_.publish(internal_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 5.0) + # CLEAR: back to API velocity limit + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 10.0) + # lower the max_vel node parameter + self.update_max_vel_param(2.5) + self.pub_clear_limit_.publish(clear_cmd) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 2.5) + # velocity limit set by internal limit is no longer applied since above max_vel parameter + internal_limit = self.make_velocity_limit_msg(5.0) + self.pub_internal_limit_.publish(internal_limit) + self.wait_for_output() + self.assertEqual(self.velocity_limit_output_.max_velocity, 2.5) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/planning/autoware_freespace_planner/CHANGELOG.rst b/planning/autoware_freespace_planner/CHANGELOG.rst index ad8960a64d3d2..75774a107b615 100644 --- a/planning/autoware_freespace_planner/CHANGELOG.rst +++ b/planning/autoware_freespace_planner/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_freespace_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_freespace_planner): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_freespace_planner (`#9903 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in flies planning/autoware_freespace_planner +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp index 1f5a52ca040a0..bd431372b70dc 100644 --- a/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware/freespace_planner/freespace_planner_node.hpp @@ -39,13 +39,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -112,7 +112,8 @@ class FreespacePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_pose_array_pub_; rclcpp::Publisher::SharedPtr debug_partial_pose_array_pub_; rclcpp::Publisher::SharedPtr parking_state_pub_; - rclcpp::Publisher::SharedPtr processing_time_pub_; + rclcpp::Publisher::SharedPtr + processing_time_pub_; rclcpp::Subscription::SharedPtr route_sub_; diff --git a/planning/autoware_freespace_planner/package.xml b/planning/autoware_freespace_planner/package.xml index fd7e39d191647..7586d110d4f51 100644 --- a/planning/autoware_freespace_planner/package.xml +++ b/planning/autoware_freespace_planner/package.xml @@ -2,7 +2,7 @@ autoware_freespace_planner - 0.40.0 + 0.41.0 The autoware_freespace_planner package Kosuke Takeuchi Takamasa Horibe diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index a937114e653c6..2749408bedb7b 100644 --- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -95,8 +95,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti debug_pose_array_pub_ = create_publisher("~/debug/pose_array", qos); debug_partial_pose_array_pub_ = create_publisher("~/debug/partial_pose_array", qos); parking_state_pub_ = create_publisher("is_completed", qos); - processing_time_pub_ = - create_publisher("~/debug/processing_time_ms", 1); + processing_time_pub_ = create_publisher( + "~/debug/processing_time_ms", 1); } // TF @@ -362,7 +362,7 @@ void FreespacePlannerNode::onTimer() is_new_parking_cycle_ = false; // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); processing_time_pub_->publish(processing_time_msg); diff --git a/planning/autoware_freespace_planning_algorithms/CHANGELOG.rst b/planning/autoware_freespace_planning_algorithms/CHANGELOG.rst index 8f6b94a2e0a80..7c97be99b7625 100644 --- a/planning/autoware_freespace_planning_algorithms/CHANGELOG.rst +++ b/planning/autoware_freespace_planning_algorithms/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package autoware_freespace_planning_algorithms ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_freespace_planning_algorithms): fix bugprone-errors (`#9670 `_) + * fix: bugprone-error + * fix: bugprone-error + --------- +* build(autoware_freespace_planning_algorithms): increase test timeout to 2 mins (`#9639 `_) +* Contributors: Fumiya Watanabe, M. Fatih Cırıt, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_freespace_planning_algorithms/CMakeLists.txt b/planning/autoware_freespace_planning_algorithms/CMakeLists.txt index 5b6013528f7fc..7ac50842d21e3 100644 --- a/planning/autoware_freespace_planning_algorithms/CMakeLists.txt +++ b/planning/autoware_freespace_planning_algorithms/CMakeLists.txt @@ -34,6 +34,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gtest(${PROJECT_NAME}-test test/src/test_freespace_planning_algorithms.cpp ) + set_tests_properties(${PROJECT_NAME}-test PROPERTIES TIMEOUT 120) target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME} ) diff --git a/planning/autoware_freespace_planning_algorithms/package.xml b/planning/autoware_freespace_planning_algorithms/package.xml index 4bffa7bd15c1d..873e95ec42b14 100644 --- a/planning/autoware_freespace_planning_algorithms/package.xml +++ b/planning/autoware_freespace_planning_algorithms/package.xml @@ -2,7 +2,7 @@ autoware_freespace_planning_algorithms - 0.40.0 + 0.41.0 The autoware_freespace_planning_algorithms package Kosuke Takeuchi Takamasa Horibe diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index 56d6526e26272..255288603016b 100644 --- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -115,7 +115,7 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost std::vector is_obstacle_table; is_obstacle_table.resize(nb_of_cells); for (uint32_t i = 0; i < nb_of_cells; ++i) { - const int cost = costmap_.data[i]; + const int cost = costmap_.data[i]; // NOLINT if (cost < 0 || planner_common_param_.obstacle_threshold <= cost) { is_obstacle_table[i] = true; } diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 0a17b112eed6f..c9f4b46dab737 100644 --- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -388,7 +388,7 @@ double AstarSearch::getExpansionDistance(const AstarNode & current_node) const double AstarSearch::getSteeringCost(const int steering_index) const { return planner_common_param_.curve_weight * - (abs(steering_index) / planner_common_param_.turning_steps); + (static_cast(abs(steering_index)) / planner_common_param_.turning_steps); } double AstarSearch::getSteeringChangeCost( diff --git a/planning/autoware_mission_planner/plugins/plugin_description.xml b/planning/autoware_mission_planner/plugins/plugin_description.xml deleted file mode 100644 index 488e6afd35df1..0000000000000 --- a/planning/autoware_mission_planner/plugins/plugin_description.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/autoware_mission_planner/CHANGELOG.rst b/planning/autoware_mission_planner_universe/CHANGELOG.rst similarity index 94% rename from planning/autoware_mission_planner/CHANGELOG.rst rename to planning/autoware_mission_planner_universe/CHANGELOG.rst index ed21313c96af2..8965e78f50b32 100644 --- a/planning/autoware_mission_planner/CHANGELOG.rst +++ b/planning/autoware_mission_planner_universe/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_mission_planner -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_mission_planner_universe +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_mission_planner)!: feat(autoware_mission_planner_universe)!: add _universe suffix to package name (`#9941 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome 0.40.0 (2024-12-12) ------------------- diff --git a/planning/autoware_mission_planner/CMakeLists.txt b/planning/autoware_mission_planner_universe/CMakeLists.txt similarity index 81% rename from planning/autoware_mission_planner/CMakeLists.txt rename to planning/autoware_mission_planner_universe/CMakeLists.txt index 939903c2999ff..5c2445c9e2dcf 100644 --- a/planning/autoware_mission_planner/CMakeLists.txt +++ b/planning/autoware_mission_planner_universe/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_mission_planner) +project(autoware_mission_planner_universe) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(goal_pose_visualizer_component SHARED ) rclcpp_components_register_node(goal_pose_visualizer_component - PLUGIN "autoware::mission_planner::GoalPoseVisualizer" + PLUGIN "autoware::mission_planner_universe::GoalPoseVisualizer" EXECUTABLE goal_pose_visualizer ) @@ -21,12 +21,12 @@ ament_auto_add_library(${PROJECT_NAME}_component SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_component - PLUGIN "autoware::mission_planner::MissionPlanner" + PLUGIN "autoware::mission_planner_universe::MissionPlanner" EXECUTABLE mission_planner ) rclcpp_components_register_node(${PROJECT_NAME}_component - PLUGIN "autoware::mission_planner::RouteSelector" + PLUGIN "autoware::mission_planner_universe::RouteSelector" EXECUTABLE route_selector ) @@ -34,7 +34,7 @@ ament_auto_add_library(${PROJECT_NAME}_lanelet2_plugins SHARED src/lanelet2_plugins/default_planner.cpp src/lanelet2_plugins/utility_functions.cpp ) -pluginlib_export_plugin_description_file(autoware_mission_planner plugins/plugin_description.xml) +pluginlib_export_plugin_description_file(autoware_mission_planner_universe plugins/plugin_description.xml) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} diff --git a/planning/autoware_mission_planner/README.md b/planning/autoware_mission_planner_universe/README.md similarity index 99% rename from planning/autoware_mission_planner/README.md rename to planning/autoware_mission_planner_universe/README.md index b5993d0106add..720ecd976f65f 100644 --- a/planning/autoware_mission_planner/README.md +++ b/planning/autoware_mission_planner_universe/README.md @@ -53,6 +53,7 @@ It distributes route requests and planning results according to current MRM oper | `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | | `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | | `input/operation_mode_state` | autoware_adapi_v1_msgs/OperationModeState | operation mode state | +| `input/odometry` | nav_msgs/msg/Odometry | vehicle odometry | ### Publications diff --git a/planning/autoware_mission_planner/config/mission_planner.param.yaml b/planning/autoware_mission_planner_universe/config/mission_planner.param.yaml similarity index 100% rename from planning/autoware_mission_planner/config/mission_planner.param.yaml rename to planning/autoware_mission_planner_universe/config/mission_planner.param.yaml diff --git a/planning/autoware_mission_planner/include/autoware/mission_planner/mission_planner_plugin.hpp b/planning/autoware_mission_planner_universe/include/autoware/mission_planner_universe/mission_planner_plugin.hpp similarity index 83% rename from planning/autoware_mission_planner/include/autoware/mission_planner/mission_planner_plugin.hpp rename to planning/autoware_mission_planner_universe/include/autoware/mission_planner_universe/mission_planner_plugin.hpp index 837ea15a55486..17287db8c8b90 100644 --- a/planning/autoware_mission_planner/include/autoware/mission_planner/mission_planner_plugin.hpp +++ b/planning/autoware_mission_planner_universe/include/autoware/mission_planner_universe/mission_planner_plugin.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ -#define AUTOWARE__MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ +#ifndef AUTOWARE__MISSION_PLANNER_UNIVERSE__MISSION_PLANNER_PLUGIN_HPP_ +#define AUTOWARE__MISSION_PLANNER_UNIVERSE__MISSION_PLANNER_PLUGIN_HPP_ #include @@ -24,7 +24,7 @@ #include -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { class PlannerPlugin @@ -45,6 +45,6 @@ class PlannerPlugin virtual void clearRoute() = 0; }; -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe -#endif // AUTOWARE__MISSION_PLANNER__MISSION_PLANNER_PLUGIN_HPP_ +#endif // AUTOWARE__MISSION_PLANNER_UNIVERSE__MISSION_PLANNER_PLUGIN_HPP_ diff --git a/planning/autoware_mission_planner/launch/goal_pose_visualizer.launch.xml b/planning/autoware_mission_planner_universe/launch/goal_pose_visualizer.launch.xml similarity index 74% rename from planning/autoware_mission_planner/launch/goal_pose_visualizer.launch.xml rename to planning/autoware_mission_planner_universe/launch/goal_pose_visualizer.launch.xml index a376bd1da7e58..c6266e71aabbd 100644 --- a/planning/autoware_mission_planner/launch/goal_pose_visualizer.launch.xml +++ b/planning/autoware_mission_planner_universe/launch/goal_pose_visualizer.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/planning/autoware_mission_planner/launch/mission_planner.launch.xml b/planning/autoware_mission_planner_universe/launch/mission_planner.launch.xml similarity index 81% rename from planning/autoware_mission_planner/launch/mission_planner.launch.xml rename to planning/autoware_mission_planner_universe/launch/mission_planner.launch.xml index c15874118011a..ec20ed0a1318c 100644 --- a/planning/autoware_mission_planner/launch/mission_planner.launch.xml +++ b/planning/autoware_mission_planner_universe/launch/mission_planner.launch.xml @@ -2,10 +2,10 @@ - + - + @@ -16,7 +16,7 @@ - + diff --git a/planning/autoware_mission_planner/media/architecture.drawio.svg b/planning/autoware_mission_planner_universe/media/architecture.drawio.svg similarity index 100% rename from planning/autoware_mission_planner/media/architecture.drawio.svg rename to planning/autoware_mission_planner_universe/media/architecture.drawio.svg diff --git a/planning/autoware_mission_planner/media/goal_footprints.svg b/planning/autoware_mission_planner_universe/media/goal_footprints.svg similarity index 100% rename from planning/autoware_mission_planner/media/goal_footprints.svg rename to planning/autoware_mission_planner_universe/media/goal_footprints.svg diff --git a/planning/autoware_mission_planner/media/rerouting_safety.svg b/planning/autoware_mission_planner_universe/media/rerouting_safety.svg similarity index 100% rename from planning/autoware_mission_planner/media/rerouting_safety.svg rename to planning/autoware_mission_planner_universe/media/rerouting_safety.svg diff --git a/planning/autoware_mission_planner/media/route_sections.svg b/planning/autoware_mission_planner_universe/media/route_sections.svg similarity index 100% rename from planning/autoware_mission_planner/media/route_sections.svg rename to planning/autoware_mission_planner_universe/media/route_sections.svg diff --git a/planning/autoware_mission_planner/package.xml b/planning/autoware_mission_planner_universe/package.xml similarity index 91% rename from planning/autoware_mission_planner/package.xml rename to planning/autoware_mission_planner_universe/package.xml index e7cf974b2ba25..804623d60a63f 100644 --- a/planning/autoware_mission_planner/package.xml +++ b/planning/autoware_mission_planner_universe/package.xml @@ -1,9 +1,9 @@ - autoware_mission_planner - 0.40.0 - The autoware_mission_planner package + autoware_mission_planner_universe + 0.41.0 + The autoware_mission_planner_universe package Takagi, Isamu Kosuke Takeuchi Ryohsuke Mitsudome diff --git a/planning/autoware_mission_planner_universe/plugins/plugin_description.xml b/planning/autoware_mission_planner_universe/plugins/plugin_description.xml new file mode 100644 index 0000000000000..51d560bdfb6c3 --- /dev/null +++ b/planning/autoware_mission_planner_universe/plugins/plugin_description.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/autoware_mission_planner/schema/mission_planner.schema.json b/planning/autoware_mission_planner_universe/schema/mission_planner.schema.json similarity index 100% rename from planning/autoware_mission_planner/schema/mission_planner.schema.json rename to planning/autoware_mission_planner_universe/schema/mission_planner.schema.json diff --git a/planning/autoware_mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp b/planning/autoware_mission_planner_universe/src/goal_pose_visualizer/goal_pose_visualizer.cpp similarity index 88% rename from planning/autoware_mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp rename to planning/autoware_mission_planner_universe/src/goal_pose_visualizer/goal_pose_visualizer.cpp index 62a718279a4da..3299646332917 100644 --- a/planning/autoware_mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.cpp +++ b/planning/autoware_mission_planner_universe/src/goal_pose_visualizer/goal_pose_visualizer.cpp @@ -14,7 +14,7 @@ #include "goal_pose_visualizer.hpp" -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { GoalPoseVisualizer::GoalPoseVisualizer(const rclcpp::NodeOptions & node_options) : Node("goal_pose_visualizer", node_options) @@ -34,7 +34,7 @@ void GoalPoseVisualizer::echo_back_route_callback( goal_pose.pose = msg->goal_pose; pub_goal_pose_->publish(goal_pose); } -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe #include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mission_planner::GoalPoseVisualizer) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mission_planner_universe::GoalPoseVisualizer) diff --git a/planning/autoware_mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp b/planning/autoware_mission_planner_universe/src/goal_pose_visualizer/goal_pose_visualizer.hpp similarity index 93% rename from planning/autoware_mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp rename to planning/autoware_mission_planner_universe/src/goal_pose_visualizer/goal_pose_visualizer.hpp index 3737597556ce4..33fa5795cca26 100644 --- a/planning/autoware_mission_planner/src/goal_pose_visualizer/goal_pose_visualizer.hpp +++ b/planning/autoware_mission_planner_universe/src/goal_pose_visualizer/goal_pose_visualizer.hpp @@ -20,7 +20,7 @@ #include #include -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { class GoalPoseVisualizer : public rclcpp::Node { @@ -35,5 +35,5 @@ class GoalPoseVisualizer : public rclcpp::Node const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr msg); }; -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe #endif // GOAL_POSE_VISUALIZER__GOAL_POSE_VISUALIZER_HPP_ diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.cpp similarity index 98% rename from planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp rename to planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.cpp index 50a6becf2e124..acd2b75fa2e21 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.cpp @@ -41,7 +41,7 @@ #include #include -namespace autoware::mission_planner::lanelet2 +namespace autoware::mission_planner_universe::lanelet2 { void DefaultPlanner::initialize_common(rclcpp::Node * node) @@ -383,8 +383,9 @@ void DefaultPlanner::clearRoute() route_handler_.clearRoute(); } -} // namespace autoware::mission_planner::lanelet2 +} // namespace autoware::mission_planner_universe::lanelet2 #include PLUGINLIB_EXPORT_CLASS( - autoware::mission_planner::lanelet2::DefaultPlanner, autoware::mission_planner::PlannerPlugin) + autoware::mission_planner_universe::lanelet2::DefaultPlanner, + autoware::mission_planner_universe::PlannerPlugin) diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.hpp similarity index 93% rename from planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp rename to planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.hpp index ef5f34d4a3697..5af96e976ef8b 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/default_planner.hpp @@ -15,7 +15,7 @@ #ifndef LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ #define LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ -#include +#include #include #include #include @@ -29,7 +29,7 @@ #include -namespace autoware::mission_planner::lanelet2 +namespace autoware::mission_planner_universe::lanelet2 { struct DefaultPlannerParameters @@ -40,7 +40,7 @@ struct DefaultPlannerParameters bool check_footprint_inside_lanes; }; -class DefaultPlanner : public mission_planner::PlannerPlugin +class DefaultPlanner : public mission_planner_universe::PlannerPlugin { public: DefaultPlanner() : vehicle_info_(), is_graph_ready_(false), param_(), node_(nullptr) {} @@ -101,6 +101,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections); }; -} // namespace autoware::mission_planner::lanelet2 +} // namespace autoware::mission_planner_universe::lanelet2 #endif // LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.cpp similarity index 98% rename from planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp rename to planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.cpp index 40bccd118965f..fd6e40ab0566e 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.cpp @@ -25,7 +25,7 @@ #include #include -namespace autoware::mission_planner::lanelet2 +namespace autoware::mission_planner_universe::lanelet2 { autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( autoware::universe_utils::LinearRing2d footprint) @@ -158,4 +158,4 @@ geometry_msgs::msg::Pose get_closest_centerline_pose( return convertBasicPoint3dToPose(refined_point, lane_yaw); } -} // namespace autoware::mission_planner::lanelet2 +} // namespace autoware::mission_planner_universe::lanelet2 diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.hpp similarity index 95% rename from planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp rename to planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.hpp index 36a2e17fb5ff0..233c8ef08ec23 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner_universe/src/lanelet2_plugins/utility_functions.hpp @@ -29,7 +29,7 @@ #include -namespace autoware::mission_planner::lanelet2 +namespace autoware::mission_planner_universe::lanelet2 { using RouteSections = std::vector; @@ -64,5 +64,5 @@ geometry_msgs::msg::Pose get_closest_centerline_pose( const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, autoware::vehicle_info_utils::VehicleInfo vehicle_info); -} // namespace autoware::mission_planner::lanelet2 +} // namespace autoware::mission_planner_universe::lanelet2 #endif // LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ diff --git a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp b/planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.cpp similarity index 95% rename from planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp rename to planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.cpp index 41502d8c3c2c3..37e31a77ab5d5 100644 --- a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.cpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.cpp @@ -20,7 +20,7 @@ #include -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { ArrivalChecker::ArrivalChecker(rclcpp::Node * node) : vehicle_stop_checker_(node) @@ -72,4 +72,4 @@ bool ArrivalChecker::is_arrived(const PoseStamped & pose) const return vehicle_stop_checker_.isVehicleStopped(duration_); } -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe diff --git a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp b/planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.hpp similarity index 94% rename from planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp rename to planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.hpp index d2e9a7408b791..9c339fae8ed07 100644 --- a/planning/autoware_mission_planner/src/mission_planner/arrival_checker.hpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/arrival_checker.hpp @@ -22,7 +22,7 @@ #include #include -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { class ArrivalChecker @@ -44,6 +44,6 @@ class ArrivalChecker autoware::motion_utils::VehicleStopChecker vehicle_stop_checker_; }; -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe #endif // MISSION_PLANNER__ARRIVAL_CHECKER_HPP_ diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp b/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.cpp similarity index 97% rename from planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp rename to planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.cpp index cbbcfb84d2cbb..58a081a6d6a2b 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.cpp @@ -32,13 +32,14 @@ #include #include -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) : Node("mission_planner", options), arrival_checker_(this), - plugin_loader_("autoware_mission_planner", "autoware::mission_planner::PlannerPlugin"), + plugin_loader_( + "autoware_mission_planner_universe", "autoware::mission_planner_universe::PlannerPlugin"), tf_buffer_(get_clock()), tf_listener_(tf_buffer_), odometry_(nullptr), @@ -52,8 +53,8 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) minimum_reroute_length_ = declare_parameter("minimum_reroute_length"); allow_reroute_in_autonomous_mode_ = declare_parameter("allow_reroute_in_autonomous_mode"); - planner_ = - plugin_loader_.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner"); + planner_ = plugin_loader_.createSharedInstance( + "autoware::mission_planner_universe::lanelet2::DefaultPlanner"); planner_->initialize(this); const auto durable_qos = rclcpp::QoS(1).transient_local(); @@ -87,14 +88,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) is_mission_planner_ready_ = false; logger_configure_ = std::make_unique(this); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); } void MissionPlanner::publish_processing_time( autoware::universe_utils::StopWatch stop_watch) { - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); @@ -671,7 +672,7 @@ bool MissionPlanner::check_reroute_safety( accumulated_length, safety_length); return false; } -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe #include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mission_planner::MissionPlanner) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mission_planner_universe::MissionPlanner) diff --git a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp b/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.hpp similarity index 94% rename from planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp rename to planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.hpp index 987ca6482ec11..10288e18c8b2a 100644 --- a/planning/autoware_mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/mission_planner.hpp @@ -18,7 +18,7 @@ #include "arrival_checker.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include +#include #include #include #include @@ -28,9 +28,9 @@ #include #include #include +#include #include #include -#include #include #include #include @@ -45,7 +45,7 @@ #include #include -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { using autoware_adapi_v1_msgs::msg::OperationModeState; @@ -146,9 +146,10 @@ class MissionPlanner : public rclcpp::Node bool check_reroute_safety(const LaneletRoute & original_route, const LaneletRoute & target_route); std::unique_ptr logger_configure_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; }; -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe #endif // MISSION_PLANNER__MISSION_PLANNER_HPP_ diff --git a/planning/autoware_mission_planner/src/mission_planner/route_selector.cpp b/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.cpp similarity index 95% rename from planning/autoware_mission_planner/src/mission_planner/route_selector.cpp rename to planning/autoware_mission_planner_universe/src/mission_planner/route_selector.cpp index 5ee565af00423..b628f90794a44 100644 --- a/planning/autoware_mission_planner/src/mission_planner/route_selector.cpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.cpp @@ -20,7 +20,7 @@ #include #include -namespace autoware::mission_planner::uuid +namespace autoware::mission_planner_universe::uuid { std::array generate_random_id() @@ -40,9 +40,9 @@ UUID generate_if_empty(const UUID & uuid) return result; } -} // namespace autoware::mission_planner::uuid +} // namespace autoware::mission_planner_universe::uuid -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { RouteInterface::RouteInterface(rclcpp::Clock::SharedPtr clock) @@ -138,14 +138,14 @@ RouteSelector::RouteSelector(const rclcpp::NodeOptions & options) main_request_ = std::monostate{}; // Processing time - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); } void RouteSelector::publish_processing_time( autoware::universe_utils::StopWatch stop_watch) { - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); @@ -309,7 +309,7 @@ ResponseStatus RouteSelector::resume_main_route(ClearRoute::Request::SharedPtr r throw std::logic_error("route_selector: unknown main route request"); } -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe #include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mission_planner::RouteSelector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mission_planner_universe::RouteSelector) diff --git a/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp b/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.hpp similarity index 93% rename from planning/autoware_mission_planner/src/mission_planner/route_selector.hpp rename to planning/autoware_mission_planner_universe/src/mission_planner/route_selector.hpp index 5c8f81aaf216e..4b631c05c1f88 100644 --- a/planning/autoware_mission_planner/src/mission_planner/route_selector.hpp +++ b/planning/autoware_mission_planner_universe/src/mission_planner/route_selector.hpp @@ -18,8 +18,8 @@ #include #include +#include #include -#include #include #include #include @@ -28,7 +28,7 @@ #include #include -namespace autoware::mission_planner +namespace autoware::mission_planner_universe { using autoware_common_msgs::msg::ResponseStatus; @@ -82,7 +82,8 @@ class RouteSelector : public rclcpp::Node rclcpp::Client::SharedPtr cli_set_lanelet_route_; rclcpp::Subscription::SharedPtr sub_state_; rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; bool initialized_; bool mrm_operating_; @@ -106,6 +107,6 @@ class RouteSelector : public rclcpp::Node ResponseStatus resume_main_route(ClearRoute::Request::SharedPtr req); }; -} // namespace autoware::mission_planner +} // namespace autoware::mission_planner_universe #endif // MISSION_PLANNER__ROUTE_SELECTOR_HPP_ diff --git a/planning/autoware_mission_planner/src/mission_planner/service_utils.cpp b/planning/autoware_mission_planner_universe/src/mission_planner/service_utils.cpp similarity index 100% rename from planning/autoware_mission_planner/src/mission_planner/service_utils.cpp rename to planning/autoware_mission_planner_universe/src/mission_planner/service_utils.cpp diff --git a/planning/autoware_mission_planner/src/mission_planner/service_utils.hpp b/planning/autoware_mission_planner_universe/src/mission_planner/service_utils.hpp similarity index 100% rename from planning/autoware_mission_planner/src/mission_planner/service_utils.hpp rename to planning/autoware_mission_planner_universe/src/mission_planner/service_utils.hpp diff --git a/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp b/planning/autoware_mission_planner_universe/test/test_lanelet2_plugins_default_planner.cpp similarity index 99% rename from planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp rename to planning/autoware_mission_planner_universe/test/test_lanelet2_plugins_default_planner.cpp index 8da26ab851bb4..bbe3b6c40fe47 100644 --- a/planning/autoware_mission_planner/test/test_lanelet2_plugins_default_planner.cpp +++ b/planning/autoware_mission_planner_universe/test/test_lanelet2_plugins_default_planner.cpp @@ -40,7 +40,7 @@ using geometry_msgs::msg::Pose; using RoutePoints = std::vector; // inherit DefaultPlanner to access protected methods and make wrapper to private methods -struct DefaultPlanner : public autoware::mission_planner::lanelet2::DefaultPlanner +struct DefaultPlanner : public autoware::mission_planner_universe::lanelet2::DefaultPlanner { // todo(someone): create tests with various kinds of maps void set_default_test_map() { route_handler_.setMap(autoware::test_utils::makeMapBinMsg()); } @@ -83,7 +83,7 @@ class DefaultPlannerTest : public ::testing::Test const auto autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); const auto mission_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_mission_planner"); + ament_index_cpp::get_package_share_directory("autoware_mission_planner_universe"); options.arguments( {"--ros-args", "--params-file", autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", diff --git a/planning/autoware_mission_planner/test/test_utility_functions.cpp b/planning/autoware_mission_planner_universe/test/test_utility_functions.cpp similarity index 95% rename from planning/autoware_mission_planner/test/test_utility_functions.cpp rename to planning/autoware_mission_planner_universe/test/test_utility_functions.cpp index da44a2ad17cf0..a531900d69dc1 100644 --- a/planning/autoware_mission_planner/test/test_utility_functions.cpp +++ b/planning/autoware_mission_planner_universe/test/test_utility_functions.cpp @@ -25,16 +25,16 @@ #include -using autoware::mission_planner::lanelet2::convert_linear_ring_to_polygon; -using autoware::mission_planner::lanelet2::convertBasicPoint3dToPose; -using autoware::mission_planner::lanelet2::convertCenterlineToPoints; -using autoware::mission_planner::lanelet2::exists; -using autoware::mission_planner::lanelet2::get_closest_centerline_pose; -using autoware::mission_planner::lanelet2::insert_marker_array; -using autoware::mission_planner::lanelet2::is_in_lane; -using autoware::mission_planner::lanelet2::is_in_parking_lot; -using autoware::mission_planner::lanelet2::is_in_parking_space; -using autoware::mission_planner::lanelet2::project_goal_to_map; +using autoware::mission_planner_universe::lanelet2::convert_linear_ring_to_polygon; +using autoware::mission_planner_universe::lanelet2::convertBasicPoint3dToPose; +using autoware::mission_planner_universe::lanelet2::convertCenterlineToPoints; +using autoware::mission_planner_universe::lanelet2::exists; +using autoware::mission_planner_universe::lanelet2::get_closest_centerline_pose; +using autoware::mission_planner_universe::lanelet2::insert_marker_array; +using autoware::mission_planner_universe::lanelet2::is_in_lane; +using autoware::mission_planner_universe::lanelet2::is_in_parking_lot; +using autoware::mission_planner_universe::lanelet2::is_in_parking_space; +using autoware::mission_planner_universe::lanelet2::project_goal_to_map; using autoware::vehicle_info_utils::VehicleInfo; using geometry_msgs::msg::Pose; diff --git a/planning/autoware_objects_of_interest_marker_interface/CHANGELOG.rst b/planning/autoware_objects_of_interest_marker_interface/CHANGELOG.rst index 16b583990ec3e..6491c3d036148 100644 --- a/planning/autoware_objects_of_interest_marker_interface/CHANGELOG.rst +++ b/planning/autoware_objects_of_interest_marker_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_objects_of_interest_marker_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_objects_of_interest_marker_interface): fix bugprone-branch-clone (`#9671 `_) +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_objects_of_interest_marker_interface/package.xml b/planning/autoware_objects_of_interest_marker_interface/package.xml index 6d179f7872dc1..82e6637685ea7 100644 --- a/planning/autoware_objects_of_interest_marker_interface/package.xml +++ b/planning/autoware_objects_of_interest_marker_interface/package.xml @@ -1,7 +1,7 @@ autoware_objects_of_interest_marker_interface - 0.40.0 + 0.41.0 The autoware_objects_of_interest_marker_interface package Fumiya Watanabe diff --git a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index cbdb2542b97e7..e18190a8bf9e7 100644 --- a/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/autoware_objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -76,14 +76,14 @@ ColorRGBA ObjectsOfInterestMarkerInterface::getColor( const ColorName & color_name, const float alpha) { switch (color_name) { + case ColorName::GRAY: + return coloring::getGray(alpha); case ColorName::GREEN: return coloring::getGreen(alpha); case ColorName::AMBER: return coloring::getAmber(alpha); case ColorName::RED: return coloring::getRed(alpha); - case ColorName::GRAY: - return coloring::getGray(alpha); default: return coloring::getGray(alpha); } diff --git a/planning/autoware_obstacle_cruise_planner/CHANGELOG.rst b/planning/autoware_obstacle_cruise_planner/CHANGELOG.rst index 827fee6a58039..b1922cde64477 100644 --- a/planning/autoware_obstacle_cruise_planner/CHANGELOG.rst +++ b/planning/autoware_obstacle_cruise_planner/CHANGELOG.rst @@ -2,6 +2,47 @@ Changelog for package autoware_obstacle_cruise_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_obstacle_cruise_planner)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_obstacle_cruise_planner (`#9905 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_obstacle_cruise_planner +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* docs(obstacle_cruise_planner): add supplemental figures (`#9154 `_) + * add behavior determination flowchart + * add cruise planning block diagram + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Mamoru Sobue, Mitsuhiro Sakamoto, Satoshi OTA, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 42a569c07ddb2..273363489d56f 100644 --- a/planning/autoware_obstacle_cruise_planner/README.md +++ b/planning/autoware_obstacle_cruise_planner/README.md @@ -75,6 +75,10 @@ The obstacles not in front of the ego will be ignored. ![determine_cruise_stop_slow_down](./media/determine_cruise_stop_slow_down.drawio.svg) +The behavior determination flowchart is shown below. + +![behavior_determination_flowchart](./media/behavior_determination_flowchart.drawio.svg) + #### Determine cruise vehicles The obstacles meeting the following condition are determined as obstacles for cruising. @@ -241,6 +245,10 @@ $$ | `lpf(val)` | apply low-pass filter to `val` | | `pid(val)` | apply pid to `val` | +#### Block diagram + +![cruise_planning_block_diagram](./media/cruise_planning_block_diagram.drawio.svg) + ### Slow down planning | Parameter | Type | Description | diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp index f2269aeaa2967..6feeb3d38b769 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp @@ -17,11 +17,11 @@ #include -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include -using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; class CruisePlanningDebugInfo { diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index ef6ae1662dcee..f5e183afd12bd 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -20,6 +20,7 @@ #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/planning_factor_interface/planning_factor_interface.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" @@ -47,15 +48,16 @@ class PlannerInterface rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) - : longitudinal_info_(longitudinal_info), + : planning_factor_interface_{std::make_unique< + autoware::planning_factor_interface::PlanningFactorInterface>( + &node, "obstacle_cruise_planner")}, + longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param), debug_data_ptr_(debug_data_ptr), slow_down_param_(SlowDownParam(node)), stop_param_(StopParam(node, longitudinal_info)) { - velocity_factors_pub_ = - node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = node.create_publisher("~/output/stop_speed_exceeded", 1); metrics_pub_ = node.create_publisher("~/metrics", 10); @@ -101,6 +103,7 @@ class PlannerInterface const std::optional & stop_pose = std::nullopt, const std::optional & stop_obstacle = std::nullopt); void publishMetrics(const rclcpp::Time & current_time); + void publishPlanningFactors() { planning_factor_interface_->publish(); } void clearMetrics(); void onParam(const std::vector & parameters) @@ -128,6 +131,9 @@ class PlannerInterface double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; } protected: + std::unique_ptr + planning_factor_interface_; + // Parameters bool enable_debug_info_{false}; bool enable_calculation_time_info_{false}; @@ -145,7 +151,6 @@ class PlannerInterface stop_watch_; // Publishers - rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp index ef4f5d0eba808..1e1a4b75d6178 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp @@ -17,11 +17,11 @@ #include -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" #include -using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; class StopPlanningDebugInfo { diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp index 04badd2a956ef..64156be9b7987 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -17,8 +17,8 @@ #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp" -#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include "autoware_perception_msgs/msg/predicted_object.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -28,8 +28,6 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" -#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" @@ -39,9 +37,8 @@ #include using autoware::vehicle_info_utils::VehicleInfo; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; @@ -54,8 +51,6 @@ using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; -using tier4_debug_msgs::msg::Float32Stamped; -using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index d929647d9bcd0..ebabd96a54608 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -94,9 +94,6 @@ size_t getIndexWithLongitudinalOffset( return 0; } -VelocityFactorArray makeVelocityFactorArray( - const rclcpp::Time & time, const std::string & behavior = PlanningBehavior::ROUTE_OBSTACLE, - const std::optional pose = std::nullopt); } // namespace obstacle_cruise_utils #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg new file mode 100644 index 0000000000000..e37e2250ed3f8 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/media/behavior_determination_flowchart.drawio.svg @@ -0,0 +1,1961 @@ + + + + + + + + + + + + + + + + + +
+
+
+ + Stop obstacle + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Slow down + +
+ + obstacle + +
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ + Object + +
+ (PredictedObject / PointCloud) +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Cruise obstacle + +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ cruising? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ stop? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Should +
maintain stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Previously +
stop obstacle?
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Previously +
slow down obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
distance > C1?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego + driving +
+ forward? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Moving in +
+ same + direction? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
+ distance + < A1? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Not + crossing +
ego's trajectory?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Inside ego's +
trajectory?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Ego moving? +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
+ distance + < B1? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
outside stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Time to approach +
+ ego's + trajectory < B2? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
outside cruising?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
inside cruising?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Type for +
inside stop?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A3?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A4?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Previously +
cruise obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Longitudinal +
velocity > A5?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ Not + transient +
+
obstacle?
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Type + for +
+ slow down? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Lateral +
distance < C2?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ decrement +
count
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ increment +
count
+
+
+
+ + + + + + + + + + + + + + +
+
+
count > -C3?
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
count > C4?
+
+
+ + + + + + + + + + + + + + +
+
+
+
Collision
+
predicted?
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
reset
+
count
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + A1: + + + behavior_determination.cruise.max_lat_margin +
+ +
+ A2: + + behavior_determination.cruise.outside_obstacle.max_lateral_time_margin +
+ +
+
+ A3: + + behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop +
+ +
+
+ A4: + + behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise +
+ +
+
+ A5: + + behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold +
+ +
+
+ +
+ +
+
+ B1: + + behavior_determination.stop.max_lat_margin +
+ +
+
+ B2: + + behavior_determination.stop.outside_obstacle.max_lateral_time_margin +
+ +
+
+ B3: + + behavior_determination.stop.crossing_obstacle.collision_time_margin +
+ +
+
+ +
+ +
+
+ C1: + behavior_determination.slow_down.max_lat_margin +
+
+ + + behavior_determination.slow_down.lat_hysteresis_margin + +
+ +
+
+ C2: + behavior_determination.slow_down.max_lat_margin +
+
+ - + behavior_determination.slow_down.lat_hysteresis_margin + +
+ +
+
+ C3: + + behavior_determination.slow_down.successive_num_to_exit_slow_down_condition +
+ +
+
+ C4: + + behavior_determination.slow_down.successive_num_to_entry_slow_down_condition +
+ +
+
+ +
+ +
+
+ D1: + + behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise +
+ +
+
+ D2: + + behavior_determination.stop_obstacle_hold_time_threshold +
+ +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Time to approach +
+ ego's + trajectory < A2? +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Should maintain stop? +
+
+
+ Transient obstacle? +
+
+
+
Collision predicted?
+
+
+
+ + + + + + + + + + + + + + +
+
+
Obstacle velocity < D1 AND elapsed time since last stop decision < D2
+
+
Time to reach collision point > B3 OR collision not predicted at the time of reaching collision point
+
+
+
+
At least 1 vehicle footprint polygon at each point on trajectory intersects object footprint polygon
+
+
+
+ + + + + + + + + + + + + + + + + diff --git a/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg new file mode 100644 index 0000000000000..c6b29be2b1096 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/media/cruise_planning_block_diagram.drawio.svg @@ -0,0 +1,989 @@ + + + + + + + + + + + + + + + + + + + +
+
+
+ Obstacle velocity +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Obstacle acceleration +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Ego velocity / acceleration +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Actual distance to obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+
Normalize
+
&
+ LPF +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ PID +
&
+
suppress output
+
when accelerating
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Ideal distance to obstacle +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Distance error +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Normalized distance error +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Target acceleration +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
RSS
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Actual distance to obstacle +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ - +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ LPF +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Square +
(preserve sign)
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ Squared distance error +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ Trajectory +
+
+
+ + + + + + + + + + diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 8583d0639e5ce..c70c8ce6f710f 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -1,7 +1,7 @@ autoware_obstacle_cruise_planner - 0.40.0 + 0.41.0 The autoware_obstacle_cruise_planner package Takayuki Murooka @@ -18,7 +18,7 @@ ament_cmake_auto autoware_cmake - autoware_adapi_v1_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -26,6 +26,7 @@ autoware_objects_of_interest_marker_interface autoware_osqp_interface autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing @@ -38,7 +39,6 @@ std_msgs tf2 tf2_ros - tier4_debug_msgs tier4_metric_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 6931ff58e8457..4242f0d559946 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -726,6 +726,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 8. Publish debug data published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); planner_ptr_->publishMetrics(now()); + planner_ptr_->publishPlanningFactors(); publishDebugMarker(); publishDebugInfo(); objects_of_interest_marker_interface_.publishMarkerArray(); diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 544597f05ff75..20b564addf149 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -335,9 +335,10 @@ std::vector PIDBasedPlanner::planCruise( debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data); - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE, - stop_traj_points.at(wall_idx).pose)); + planning_factor_interface_->add( + stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::NONE, + tier4_planning_msgs::msg::SafetyFactorArray{}); } // do cruise planning diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index b08bb10ef69cf..b32d7f3a1bcbb 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -182,8 +182,6 @@ std::vector PlannerInterface::generateStopTrajectory( : std::abs(vehicle_info_.min_longitudinal_offset_m); if (stop_obstacles.empty()) { - velocity_factors_pub_->publish( - obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time)); // delete marker const auto markers = autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); @@ -336,8 +334,10 @@ std::vector PlannerInterface::generateStopTrajectory( // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose)); + planning_factor_interface_->add( + output_traj_points, planner_data.ego_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); // Store stop reason debug data debug_data_ptr_->stop_metrics = makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); @@ -590,10 +590,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); - velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( - planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, - slow_down_traj_points.at(slow_down_wall_idx).pose)); autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + planning_factor_interface_->add( + slow_down_traj_points, planner_data.ego_pose, + slow_down_traj_points.at(*slow_down_start_idx).pose, + slow_down_traj_points.at(*slow_down_end_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward, + stable_slow_down_vel); } // add debug virtual wall diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index 82f4e6978181f..b27def0bfcbe5 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -118,25 +118,4 @@ std::vector getClosestStopObstacles(const std::vector pose) -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = time; - - if (pose) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = behavior; - velocity_factor.pose = pose.value(); - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - } // namespace obstacle_cruise_utils diff --git a/planning/autoware_obstacle_stop_planner/CHANGELOG.rst b/planning/autoware_obstacle_stop_planner/CHANGELOG.rst index 867553c9ffd4e..46abbe1a1729f 100644 --- a/planning/autoware_obstacle_stop_planner/CHANGELOG.rst +++ b/planning/autoware_obstacle_stop_planner/CHANGELOG.rst @@ -2,6 +2,45 @@ Changelog for package autoware_obstacle_stop_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_obstacle_stop_planner)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_obstacle_stop_planner (`#9906 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_obstacle_stop_planner +* feat(autoware_planning_test_manager): remove dependency of VirtualTrafficLightState and ExpandStopRange (`#9953 `_) + * feat(autoware_planning_test_manager): remove dependency of virtual traffic light + * modify obstacle_stop test code + --------- +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* fix(obstacle_stop_planner): migrate planning factor (`#9939 `_) + * fix(obstacle_stop_planner): migrate planning factor + * fix(autoware_default_adapi): add coversion map + --------- +* Contributors: Fumiya Watanabe, Satoshi OTA, Takayuki Murooka, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_obstacle_stop_planner/package.xml b/planning/autoware_obstacle_stop_planner/package.xml index c9c78b3e87c3b..d0e3a0f7e2f82 100644 --- a/planning/autoware_obstacle_stop_planner/package.xml +++ b/planning/autoware_obstacle_stop_planner/package.xml @@ -2,7 +2,7 @@ autoware_obstacle_stop_planner - 0.40.0 + 0.41.0 The autoware_obstacle_stop_planner package Satoshi Ota @@ -21,8 +21,10 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_internal_debug_msgs autoware_motion_utils autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing @@ -40,7 +42,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp index 82a4d5a7ec550..3154ce45972a3 100644 --- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -181,7 +181,7 @@ AdaptiveCruiseController::AdaptiveCruiseController( param_.rough_velocity_rate = node_->declare_parameter(acc_ns + "rough_velocity_rate"); /* publisher */ - pub_debug_ = node_->create_publisher( + pub_debug_ = node_->create_publisher( "~/adaptive_cruise_control/debug_values", 1); } diff --git a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp index ce57f585197ad..f4a11bc4314bd 100644 --- a/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp +++ b/planning/autoware_obstacle_stop_planner/src/adaptive_cruise_control.hpp @@ -17,10 +17,10 @@ #include +#include #include #include #include -#include #include #include @@ -58,7 +58,8 @@ class AdaptiveCruiseController const PredictedObject & target_object); private: - rclcpp::Publisher::SharedPtr pub_debug_; + rclcpp::Publisher::SharedPtr + pub_debug_; rclcpp::Node * node_; /* @@ -217,7 +218,7 @@ class AdaptiveCruiseController void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time); /* Debug */ - mutable tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values_; + mutable autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_values_; enum DBGVAL { ESTIMATED_VEL_PCL = 0, ESTIMATED_VEL_OBJ = 1, diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp index 93526518306af..a3337c5dd2524 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.cpp @@ -45,12 +45,14 @@ namespace autoware::motion_planning { ObstacleStopPlannerDebugNode::ObstacleStopPlannerDebugNode( rclcpp::Node * node, const double base_link2front) -: node_(node), base_link2front_(base_link2front) +: node_(node), + base_link2front_(base_link2front), + planning_factor_interface_{ + std::make_unique( + node, "obstacle_stop_planner")} { virtual_wall_pub_ = node_->create_publisher("~/virtual_wall", 1); debug_viz_pub_ = node_->create_publisher("~/debug/marker", 1); - velocity_factor_pub_ = - node_->create_publisher("/planning/velocity_factors/obstacle_stop", 1); pub_debug_values_ = node_->create_publisher("~/obstacle_stop/debug_values", 1); } @@ -189,8 +191,13 @@ void ObstacleStopPlannerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto velocity_factor_msg = makeVelocityFactorArray(); - velocity_factor_pub_->publish(velocity_factor_msg); + if (stop_pose_ptr_) { + planning_factor_interface_->add( + std::numeric_limits::quiet_NaN(), *stop_pose_ptr_, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); + } + planning_factor_interface_->publish(); // publish debug values Float32MultiArrayStamped debug_msg{}; @@ -492,23 +499,4 @@ MarkerArray ObstacleStopPlannerDebugNode::makeVisualizationMarker() return msg; } -VelocityFactorArray ObstacleStopPlannerDebugNode::makeVelocityFactorArray() -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = node_->now(); - - if (stop_pose_ptr_) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = PlanningBehavior::ROUTE_OBSTACLE; - velocity_factor.pose = *stop_pose_ptr_; - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - } // namespace autoware::motion_planning diff --git a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp index d13e775e3d683..3b8b57e7de4c6 100644 --- a/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp +++ b/planning/autoware_obstacle_stop_planner/src/debug_marker.hpp @@ -14,13 +14,12 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ +#include #include -#include -#include +#include #include #include -#include #include #include @@ -42,10 +41,7 @@ using std_msgs::msg::Header; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; enum class PolygonType : int8_t { Vehicle = 0, Collision, SlowDownRange, SlowDown, Obstacle }; @@ -109,7 +105,6 @@ class ObstacleStopPlannerDebugNode bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); MarkerArray makeVirtualWallMarker(); MarkerArray makeVisualizationMarker(); - VelocityFactorArray makeVelocityFactorArray(); void setDebugValues(const DebugValues::TYPE type, const double val) { @@ -120,11 +115,13 @@ class ObstacleStopPlannerDebugNode private: rclcpp::Publisher::SharedPtr virtual_wall_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Node * node_; double base_link2front_; + std::unique_ptr + planning_factor_interface_; + std::shared_ptr stop_pose_ptr_; std::shared_ptr target_stop_pose_ptr_; std::shared_ptr slow_down_start_pose_ptr_; diff --git a/planning/autoware_obstacle_stop_planner/src/node.hpp b/planning/autoware_obstacle_stop_planner/src/node.hpp index fba24061896b6..ea8aed19bdcfd 100644 --- a/planning/autoware_obstacle_stop_planner/src/node.hpp +++ b/planning/autoware_obstacle_stop_planner/src/node.hpp @@ -29,6 +29,10 @@ #include #include +#include +#include +#include +#include #include #include #include @@ -36,10 +40,6 @@ #include #include #include -#include -#include -#include -#include #include #include #include @@ -81,13 +81,13 @@ using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware::universe_utils::StopWatch; using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::BoolStamped; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_debug_msgs::msg::BoolStamped; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; -using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; diff --git a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp index 9f840586a1959..9da8409c6c2ae 100644 --- a/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp +++ b/planning/autoware_obstacle_stop_planner/test/test_autoware_obstacle_stop_planner_node_interface.cpp @@ -18,6 +18,8 @@ #include #include +#include + #include #include @@ -25,6 +27,7 @@ using autoware::motion_planning::ObstacleStopPlannerNode; using autoware::planning_test_manager::PlanningInterfaceTestManager; +using tier4_planning_msgs::msg::ExpandStopRange; std::shared_ptr generateTestManager() { @@ -66,8 +69,17 @@ void publishMandatoryTopics( test_manager->publishPointCloud(test_target_node, "obstacle_stop_planner/input/pointcloud"); test_manager->publishAcceleration(test_target_node, "obstacle_stop_planner/input/acceleration"); test_manager->publishPredictedObjects(test_target_node, "obstacle_stop_planner/input/objects"); - test_manager->publishExpandStopRange( - test_target_node, "obstacle_stop_planner/input/expand_stop_range"); +} + +void publishExpandStopRange( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto expand_stop_range = test_manager->getTestNode()->create_publisher( + "obstacle_stop_planner/input/expand_stop_range", 1); + expand_stop_range->publish(ExpandStopRange{}); + autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3); } TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) @@ -78,6 +90,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + publishExpandStopRange(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); @@ -97,6 +110,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) auto test_target_node = generateNode(); publishMandatoryTopics(test_manager, test_target_node); + publishExpandStopRange(test_manager, test_target_node); // test for normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalTrajectory(test_target_node)); diff --git a/planning/autoware_path_optimizer/CHANGELOG.rst b/planning/autoware_path_optimizer/CHANGELOG.rst index 19798e6697ec6..e7b8e6b96114a 100644 --- a/planning/autoware_path_optimizer/CHANGELOG.rst +++ b/planning/autoware_path_optimizer/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package autoware_path_optimizer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_path_optimizer)!: tier4_debug_msgs changed to autoware-internal_debug_msgs in autoware_path_optimizer (`#9907 `_) + * feat: tier4_debug_msgs changed to autoware-internal_debug_msgs in files planning/autoware_path_optimizer + * Update planning/autoware_path_optimizer/package.xml + --------- + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* feat(motion_planning): use StringStamped in autoware_internal_debug_msgs (`#9742 `_) + feat(motion planning): use StringStamped in autoware_internal_debug_msgs +* Contributors: Fumiya Watanabe, Takayuki Murooka, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp index 5b16d4d5ba5c6..7f343c5a2a60f 100644 --- a/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware/path_optimizer/type_alias.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ #define AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/string_stamped.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -24,8 +26,6 @@ #include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" -#include "tier4_debug_msgs/msg/float64_stamped.hpp" -#include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace autoware::path_optimizer @@ -43,8 +43,8 @@ using nav_msgs::msg::Odometry; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug -using tier4_debug_msgs::msg::Float64Stamped; -using tier4_debug_msgs::msg::StringStamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_internal_debug_msgs::msg::StringStamped; } // namespace autoware::path_optimizer #endif // AUTOWARE__PATH_OPTIMIZER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index eb02db8ea9586..144810b545dbe 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -2,7 +2,7 @@ autoware_path_optimizer - 0.40.0 + 0.41.0 The autoware_path_optimizer package Takayuki Murooka Kosuke Takeuchi @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -27,7 +28,6 @@ rclcpp_components std_msgs tf2_ros - tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py b/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py index 29dfccb533fbe..604aa1afeed18 100755 --- a/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py +++ b/planning/autoware_path_optimizer/scripts/calculation_time_plotter.py @@ -17,10 +17,10 @@ import argparse from collections import deque +from autoware_internal_debug_msgs.msg import StringStamped import matplotlib.pyplot as plt import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import StringStamped class CalculationCostAnalyzer(Node): diff --git a/planning/autoware_path_smoother/CHANGELOG.rst b/planning/autoware_path_smoother/CHANGELOG.rst index 4b7f86361c63b..9d647e7994aa0 100644 --- a/planning/autoware_path_smoother/CHANGELOG.rst +++ b/planning/autoware_path_smoother/CHANGELOG.rst @@ -2,6 +2,22 @@ Changelog for package autoware_path_smoother ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_path_smoother)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_path_smoother (`#9910 `_) + * feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_path_smoother + * Update planning/autoware_path_smoother/package.xml + --------- + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* feat(motion_planning): use StringStamped in autoware_internal_debug_msgs (`#9742 `_) + feat(motion planning): use StringStamped in autoware_internal_debug_msgs +* fix(autoware_path_smoother): fix bugprone-branch-clone (`#9697 `_) + * fix: bugprone-error + * fix: bugprone-error + --------- +* Contributors: Fumiya Watanabe, Takayuki Murooka, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp b/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp index 5ec4b21955892..5f14ebec8975e 100644 --- a/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp +++ b/planning/autoware_path_smoother/include/autoware/path_smoother/type_alias.hpp @@ -15,6 +15,8 @@ #ifndef AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ #define AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/string_stamped.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -22,8 +24,6 @@ #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" -#include "tier4_debug_msgs/msg/float64_stamped.hpp" -#include "tier4_debug_msgs/msg/string_stamped.hpp" namespace autoware::path_smoother { @@ -37,8 +37,8 @@ using autoware_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // debug -using tier4_debug_msgs::msg::Float64Stamped; -using tier4_debug_msgs::msg::StringStamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; +using autoware_internal_debug_msgs::msg::StringStamped; } // namespace autoware::path_smoother #endif // AUTOWARE__PATH_SMOOTHER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_smoother/package.xml b/planning/autoware_path_smoother/package.xml index 3fa7b9fa482ed..6d2ea76a15c12 100644 --- a/planning/autoware_path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -2,7 +2,7 @@ autoware_path_smoother - 0.40.0 + 0.41.0 The autoware_path_smoother package Takayuki Murooka Maxime CLEMENT @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -26,7 +27,6 @@ rclcpp_components std_msgs tf2_ros - tier4_debug_msgs ament_cmake_ros ament_index_python diff --git a/planning/autoware_path_smoother/scripts/calculation_time_plotter.py b/planning/autoware_path_smoother/scripts/calculation_time_plotter.py index 3a18fc56f39bc..5450f919e5f9c 100755 --- a/planning/autoware_path_smoother/scripts/calculation_time_plotter.py +++ b/planning/autoware_path_smoother/scripts/calculation_time_plotter.py @@ -17,10 +17,10 @@ import argparse from collections import deque +from autoware_internal_debug_msgs.msg import StringStamped import matplotlib.pyplot as plt import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import StringStamped class CalculationCostAnalyzer(Node): diff --git a/planning/autoware_path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp index 20786b7b3601e..8c751e698c8fb 100644 --- a/planning/autoware_path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -53,9 +53,7 @@ Eigen::SparseMatrix makePMatrix(const int num_points) assign_value_to_triplet_vec(r, c, 6.0); } } else if (std::abs(c - r) == 1) { - if (r == 0 || r == num_points - 1) { - assign_value_to_triplet_vec(r, c, -2.0); - } else if (c == 0 || c == num_points - 1) { + if (r == 0 || r == num_points - 1 || c == 0 || c == num_points - 1) { assign_value_to_triplet_vec(r, c, -2.0); } else { assign_value_to_triplet_vec(r, c, -4.0); diff --git a/planning/autoware_planning_factor_interface/CHANGELOG.rst b/planning/autoware_planning_factor_interface/CHANGELOG.rst new file mode 100644 index 0000000000000..6a71f2cee42f1 --- /dev/null +++ b/planning/autoware_planning_factor_interface/CHANGELOG.rst @@ -0,0 +1,68 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_planning_factor_interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* Contributors: Fumiya Watanabe, Satoshi OTA + +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* Contributors: Fumiya Watanabe, Satoshi OTA + +0.40.0 (2025-01-06) +------------------- + +0.39.0 (2024-11-25) +------------------- + +0.38.0 (2024-11-11) +------------------- diff --git a/planning/autoware_planning_factor_interface/CMakeLists.txt b/planning/autoware_planning_factor_interface/CMakeLists.txt new file mode 100644 index 0000000000000..0c25a2c21fa19 --- /dev/null +++ b/planning/autoware_planning_factor_interface/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_planning_factor_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_planning_factor_interface SHARED + DIRECTORY src +) + +ament_auto_package() diff --git a/planning/autoware_planning_factor_interface/README.md b/planning/autoware_planning_factor_interface/README.md new file mode 100644 index 0000000000000..7f715790995ba --- /dev/null +++ b/planning/autoware_planning_factor_interface/README.md @@ -0,0 +1 @@ +# planning factor interface diff --git a/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp new file mode 100644 index 0000000000000..a2325b197853d --- /dev/null +++ b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp @@ -0,0 +1,240 @@ +// 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__PLANNING_FACTOR_INTERFACE__PLANNING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__PLANNING_FACTOR_INTERFACE__PLANNING_FACTOR_INTERFACE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::planning_factor_interface +{ + +using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::ControlPoint; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; +using tier4_planning_msgs::msg::SafetyFactorArray; + +class PlanningFactorInterface +{ +public: + PlanningFactorInterface(rclcpp::Node * node, const std::string & name) + : name_{name}, + pub_factors_{ + node->create_publisher("/planning/planning_factors/" + name, 1)}, + clock_{node->get_clock()} + { + } + + /** + * @brief factor setter for single control point. + * + * @param path points. + * @param ego current pose. + * @param control point pose. (e.g. stop or slow down point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + template + void add( + const std::vector & points, const Pose & ego_pose, const Pose & control_point_pose, + const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto distance = static_cast(autoware::motion_utils::calcSignedArcLength( + points, ego_pose.position, control_point_pose.position)); + add( + distance, control_point_pose, behavior, safety_factors, is_driving_forward, velocity, + shift_length, detail); + } + + /** + * @brief factor setter for two control points (section). + * + * @param path points. + * @param ego current pose. + * @param control section start pose. (e.g. lane change start point pose) + * @param control section end pose. (e.g. lane change end point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + template + void add( + const std::vector & points, const Pose & ego_pose, const Pose & start_pose, + const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto start_distance = static_cast( + autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, start_pose.position)); + const auto end_distance = static_cast( + autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, end_pose.position)); + add( + start_distance, end_distance, start_pose, end_pose, behavior, safety_factors, + is_driving_forward, velocity, shift_length, detail); + } + + /** + * @brief factor setter for single control point. + * + * @param distance to control point. + * @param control point pose. (e.g. stop point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + void add( + const double distance, const Pose & control_point_pose, const uint16_t behavior, + const SafetyFactorArray & safety_factors, const bool is_driving_forward = true, + const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") + { + const auto control_point = tier4_planning_msgs::build() + .pose(control_point_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(distance); + + const auto factor = tier4_planning_msgs::build() + .module(name_) + .is_driving_forward(is_driving_forward) + .control_points({control_point}) + .behavior(behavior) + .detail(detail) + .safety_factors(safety_factors); + + factors_.push_back(factor); + } + + /** + * @brief factor setter for two control points (section). + * + * @param distance to control section start point. + * @param distance to control section end point. + * @param control section start pose. (e.g. lane change start point pose) + * @param control section end pose. (e.g. lane change end point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + void add( + const double start_distance, const double end_distance, const Pose & start_pose, + const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto control_start_point = tier4_planning_msgs::build() + .pose(start_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(start_distance); + + const auto control_end_point = tier4_planning_msgs::build() + .pose(end_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(end_distance); + + const auto factor = tier4_planning_msgs::build() + .module(name_) + .is_driving_forward(is_driving_forward) + .control_points({control_start_point, control_end_point}) + .behavior(behavior) + .detail(detail) + .safety_factors(safety_factors); + + factors_.push_back(factor); + } + + /** + * @brief publish planning factors. + */ + void publish() + { + PlanningFactorArray msg; + msg.header.frame_id = "map"; + msg.header.stamp = clock_->now(); + msg.factors = factors_; + + pub_factors_->publish(msg); + + factors_.clear(); + } + +private: + std::string name_; + + rclcpp::Publisher::SharedPtr pub_factors_; + + rclcpp::Clock::SharedPtr clock_; + + std::vector factors_; +}; + +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); + +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); + +} // namespace autoware::planning_factor_interface + +#endif // AUTOWARE__PLANNING_FACTOR_INTERFACE__PLANNING_FACTOR_INTERFACE_HPP_ diff --git a/common/autoware_qp_interface/package.xml b/planning/autoware_planning_factor_interface/package.xml similarity index 52% rename from common/autoware_qp_interface/package.xml rename to planning/autoware_planning_factor_interface/package.xml index b61d03a36db72..1a5a4d3f9dbd7 100644 --- a/common/autoware_qp_interface/package.xml +++ b/planning/autoware_planning_factor_interface/package.xml @@ -1,28 +1,27 @@ - autoware_qp_interface - 0.40.0 - Interface for the QP solvers - Takayuki Murooka - Fumiya Watanabe - Maxime CLEMENT + autoware_planning_factor_interface + 0.41.0 + The autoware_planning_factor_interface package Satoshi Ota - Apache 2.0 + Mamoru Sobue + Apache License 2.0 + + Satoshi Ota ament_cmake_auto autoware_cmake - eigen - osqp_vendor - proxsuite + autoware_motion_utils + autoware_planning_msgs + autoware_universe_utils rclcpp - rclcpp_components + tier4_planning_msgs ament_cmake_ros ament_lint_auto autoware_lint_common - eigen ament_cmake diff --git a/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp new file mode 100644 index 0000000000000..6578776dddc4f --- /dev/null +++ b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp @@ -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. + +#include + +#include +#include + +namespace autoware::planning_factor_interface +{ +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); + +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +} // namespace autoware::planning_factor_interface diff --git a/planning/autoware_planning_test_manager/CHANGELOG.rst b/planning/autoware_planning_test_manager/CHANGELOG.rst index f1ac0f088c3cc..2ac0d456364e7 100644 --- a/planning/autoware_planning_test_manager/CHANGELOG.rst +++ b/planning/autoware_planning_test_manager/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package autoware_planning_test_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset (`#9967 `_) + * feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset + * fix + --------- +* feat(autoware_planning_test_manager): remove dependency of VirtualTrafficLightState and ExpandStopRange (`#9953 `_) + * feat(autoware_planning_test_manager): remove dependency of virtual traffic light + * modify obstacle_stop test code + --------- +* test(autoware_behavior_path_start_planner_module): add unit tests for shift shift pull out planner (`#9776 `_) + feat(behavior_path_planner): add unit tests for ShiftPullOut path planning +* refactor(autoware_test_utils): enhance makeMapBinMsg to accept package name and map filename parameters (`#9617 `_) + * feat: enhance makeMapBinMsg to accept package name and map filename parameters + * feat: set default package name to 'autoware_test_utils' in makeMapBinMsg and related functions + --------- +* Contributors: Fumiya Watanabe, Kyoichi Sugahara, Ryohsuke Mitsudome, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index 2c9a935c2077b..cf45154893af7 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -30,7 +30,7 @@ << std::endl; \ } -#include +#include #include #include @@ -52,14 +52,9 @@ #include #include #include -#include -#include -#include -#include #include #include #include -#include #include #include @@ -87,14 +82,9 @@ using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; -using tier4_api_msgs::msg::CrosswalkStatus; -using tier4_api_msgs::msg::IntersectionStatus; -using tier4_planning_msgs::msg::ExpandStopRange; -using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; -using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; enum class ModuleName { UNKNOWN = 0, @@ -117,7 +107,6 @@ class PlanningInterfaceTestManager void publishPointCloud(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishAcceleration(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishPredictedObjects(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishExpandStopRange(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOccupancyGrid(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishCostMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishMap(rclcpp::Node::SharedPtr target_node, std::string topic_name); @@ -128,10 +117,8 @@ class PlanningInterfaceTestManager void publishRoute(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTF(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishInitialPoseTF(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishLateralOffset(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishOperationModeState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void publishTrafficSignals(rclcpp::Node::SharedPtr target_node, std::string topic_name); - void publishVirtualTrafficLightState(rclcpp::Node::SharedPtr target_node, std::string topic_name); void setTrajectoryInputTopicName(std::string topic_name); void setParkingTrajectoryInputTopicName(std::string topic_name); @@ -178,6 +165,7 @@ class PlanningInterfaceTestManager void testOffTrackFromTrajectory(rclcpp::Node::SharedPtr target_node); int getReceivedTopicNum(); + rclcpp::Node::SharedPtr getTestNode() const; private: // Publisher (necessary for node running) @@ -187,7 +175,6 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr point_cloud_pub_; rclcpp::Publisher::SharedPtr acceleration_pub_; rclcpp::Publisher::SharedPtr predicted_objects_pub_; - rclcpp::Publisher::SharedPtr expand_stop_range_pub_; rclcpp::Publisher::SharedPtr occupancy_grid_pub_; rclcpp::Publisher::SharedPtr cost_map_pub_; rclcpp::Publisher::SharedPtr map_pub_; @@ -199,10 +186,8 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr route_pub_; rclcpp::Publisher::SharedPtr TF_pub_; rclcpp::Publisher::SharedPtr initial_pose_tf_pub_; - rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; rclcpp::Publisher::SharedPtr traffic_signals_pub_; - rclcpp::Publisher::SharedPtr virtual_traffic_light_states_pub_; // Subscriber rclcpp::Subscription::SharedPtr traj_sub_; diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp index b61c579df83b9..8a075594ebd3a 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -24,6 +24,7 @@ #include #include +#include #include namespace autoware_planning_test_manager::utils @@ -34,9 +35,11 @@ using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using RouteSections = std::vector; -Pose createPoseFromLaneID(const lanelet::Id & lane_id) +inline Pose createPoseFromLaneID( + const lanelet::Id & lane_id, const std::string & package_name = "autoware_test_utils", + const std::string & map_filename = "lanelet2_map.osm") { - auto map_bin_msg = autoware::test_utils::makeMapBinMsg(); + auto map_bin_msg = autoware::test_utils::makeMapBinMsg(package_name, map_filename); // create route_handler auto route_handler = std::make_shared(); route_handler->setMap(map_bin_msg); @@ -67,16 +70,19 @@ Pose createPoseFromLaneID(const lanelet::Id & lane_id) // Function to create a route from given start and goal lanelet ids // start pose and goal pose are set to the middle of the lanelet -LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id) +inline LaneletRoute makeBehaviorRouteFromLaneId( + const int & start_lane_id, const int & goal_lane_id, + const std::string & package_name = "autoware_test_utils", + const std::string & map_filename = "lanelet2_map.osm") { LaneletRoute route; route.header.frame_id = "map"; - auto start_pose = createPoseFromLaneID(start_lane_id); - auto goal_pose = createPoseFromLaneID(goal_lane_id); + auto start_pose = createPoseFromLaneID(start_lane_id, package_name, map_filename); + auto goal_pose = createPoseFromLaneID(goal_lane_id, package_name, map_filename); route.start_pose = start_pose; route.goal_pose = goal_pose; - auto map_bin_msg = autoware::test_utils::makeMapBinMsg(); + auto map_bin_msg = autoware::test_utils::makeMapBinMsg(package_name, map_filename); // create route_handler auto route_handler = std::make_shared(); route_handler->setMap(map_bin_msg); @@ -113,7 +119,7 @@ LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & return route; } -Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id) +inline Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id) { Odometry current_odometry; current_odometry.pose.pose = createPoseFromLaneID(lane_id); diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index 80801a8102453..af42894edf6d5 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -2,7 +2,7 @@ autoware_planning_test_manager - 0.40.0 + 0.41.0 ROS 2 node for testing interface of the nodes in planning module Kyoichi Sugahara Takamasa Horibe @@ -14,7 +14,7 @@ ament_cmake_auto autoware_cmake - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_control_msgs autoware_lanelet2_extension @@ -31,9 +31,7 @@ rclcpp tf2_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs - tier4_v2x_msgs unique_identifier_msgs yaml_cpp_vendor diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 62f89cab44e7b..f07efd0cda116 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -71,13 +71,6 @@ void PlanningInterfaceTestManager::publishPredictedObjects( test_node_, target_node, topic_name, predicted_objects_pub_, PredictedObjects{}); } -void PlanningInterfaceTestManager::publishExpandStopRange( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, expand_stop_range_pub_, ExpandStopRange{}); -} - void PlanningInterfaceTestManager::publishOccupancyGrid( rclcpp::Node::SharedPtr target_node, std::string topic_name) { @@ -160,13 +153,6 @@ void PlanningInterfaceTestManager::publishTF( autoware::test_utils::makeTFMsg(target_node, "base_link", "map")); } -void PlanningInterfaceTestManager::publishLateralOffset( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, lateral_offset_pub_, LateralOffset{}); -} - void PlanningInterfaceTestManager::publishOperationModeState( rclcpp::Node::SharedPtr target_node, std::string topic_name) { @@ -181,14 +167,6 @@ void PlanningInterfaceTestManager::publishTrafficSignals( test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{}); } -void PlanningInterfaceTestManager::publishVirtualTrafficLightState( - rclcpp::Node::SharedPtr target_node, std::string topic_name) -{ - autoware::test_utils::publishToTargetNode( - test_node_, target_node, topic_name, virtual_traffic_light_states_pub_, - VirtualTrafficLightStateArray{}); -} - void PlanningInterfaceTestManager::publishInitialPoseTF( rclcpp::Node::SharedPtr target_node, std::string topic_name) { @@ -471,4 +449,9 @@ int PlanningInterfaceTestManager::getReceivedTopicNum() return count_; } +rclcpp::Node::SharedPtr PlanningInterfaceTestManager::getTestNode() const +{ + return test_node_; +} + } // namespace autoware::planning_test_manager diff --git a/planning/autoware_planning_topic_converter/CHANGELOG.rst b/planning/autoware_planning_topic_converter/CHANGELOG.rst index e937e664654a2..3b70087c34e1d 100644 --- a/planning/autoware_planning_topic_converter/CHANGELOG.rst +++ b/planning/autoware_planning_topic_converter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_planning_topic_converter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_planning_topic_converter/package.xml b/planning/autoware_planning_topic_converter/package.xml index e2585924a61d2..cc81643ca5a37 100644 --- a/planning/autoware_planning_topic_converter/package.xml +++ b/planning/autoware_planning_topic_converter/package.xml @@ -1,7 +1,7 @@ autoware_planning_topic_converter - 0.40.0 + 0.41.0 The autoware_planning_topic_converter package Satoshi OTA diff --git a/planning/autoware_planning_validator/CHANGELOG.rst b/planning/autoware_planning_validator/CHANGELOG.rst index 76018e5bf5f11..cb05c9be90c21 100644 --- a/planning/autoware_planning_validator/CHANGELOG.rst +++ b/planning/autoware_planning_validator/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_planning_validator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_planning_validator)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_planning_validator (`#9911 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_planning_validator +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp index e996855b9b4da..298e7f74c289b 100644 --- a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp +++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp @@ -26,10 +26,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -37,13 +37,13 @@ namespace autoware::planning_validator { using autoware::universe_utils::StopWatch; +using autoware_internal_debug_msgs::msg::Float64Stamped; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_planning_validator::msg::PlanningValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float64Stamped; struct ValidationParams { diff --git a/planning/autoware_planning_validator/package.xml b/planning/autoware_planning_validator/package.xml index 1652c2ad4f544..fd369a259bd66 100644 --- a/planning/autoware_planning_validator/package.xml +++ b/planning/autoware_planning_validator/package.xml @@ -2,7 +2,7 @@ autoware_planning_validator - 0.40.0 + 0.41.0 ros node for autoware_planning_validator Takamasa Horibe Kosuke Takeuchi diff --git a/planning/autoware_remaining_distance_time_calculator/CHANGELOG.rst b/planning/autoware_remaining_distance_time_calculator/CHANGELOG.rst index 570e7b4783ca4..f715c8102a7ed 100644 --- a/planning/autoware_remaining_distance_time_calculator/CHANGELOG.rst +++ b/planning/autoware_remaining_distance_time_calculator/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package autoware_remaining_distance_time_calculator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(remaining_distance_time_calculator): integrate generate_parameter_library (`#8826 `_) + * add parameter description + * use parameter listener + * supress deprecated error + * change scope of compile option to private + --------- +* feat(remaining_distance_time_calculator): skip calculation during parking (`#9013 `_) +* Contributors: Fumiya Watanabe, Ismet Atabay, Mitsuhiro Sakamoto + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt index d29a153a0ce5d..31fc4b1572f33 100644 --- a/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt +++ b/planning/autoware_remaining_distance_time_calculator/CMakeLists.txt @@ -4,10 +4,22 @@ project(autoware_remaining_distance_time_calculator) find_package(autoware_cmake REQUIRED) autoware_package() +generate_parameter_library(remaining_distance_time_calculator_parameters + param/remaining_distance_time_calculator_parameters.yaml +) + ament_auto_add_library(${PROJECT_NAME} SHARED src/remaining_distance_time_calculator_node.cpp ) +target_link_libraries(${PROJECT_NAME} + remaining_distance_time_calculator_parameters +) + +target_compile_options(${PROJECT_NAME} PRIVATE + -Wno-error=deprecated-declarations +) + rclcpp_components_register_node(${PROJECT_NAME} PLUGIN "autoware::remaining_distance_time_calculator::RemainingDistanceTimeCalculatorNode" EXECUTABLE ${PROJECT_NAME}_node diff --git a/planning/autoware_remaining_distance_time_calculator/README.md b/planning/autoware_remaining_distance_time_calculator/README.md index 694c6764de91c..c227b73b87448 100644 --- a/planning/autoware_remaining_distance_time_calculator/README.md +++ b/planning/autoware_remaining_distance_time_calculator/README.md @@ -8,6 +8,7 @@ This package aims to provide mission remaining distance and remaining time calcu - The calculations are activated once we have a route planned for a mission in Autoware. - The calculations are triggered timely based on the `update_rate` parameter. +- The calculations are skipped if the scenario is PARKING, and the remaining time and distance values are set to 0.0. ### Module Parameters diff --git a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml index cfb456c57ca41..be7898bdd8913 100644 --- a/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml +++ b/planning/autoware_remaining_distance_time_calculator/launch/remaining_distance_time_calculator.launch.xml @@ -4,6 +4,7 @@ + @@ -13,6 +14,7 @@ + diff --git a/planning/autoware_remaining_distance_time_calculator/package.xml b/planning/autoware_remaining_distance_time_calculator/package.xml index 924e6d62d6aba..25a223fbd7a3c 100644 --- a/planning/autoware_remaining_distance_time_calculator/package.xml +++ b/planning/autoware_remaining_distance_time_calculator/package.xml @@ -1,7 +1,7 @@ autoware_remaining_distance_time_calculator - 0.40.0 + 0.41.0 Calculates and publishes remaining distance and time of the mission. Ahmed Ebrahim @@ -18,6 +18,7 @@ autoware_route_handler autoware_test_utils autoware_universe_utils + generate_parameter_library geometry_msgs nav_msgs rclcpp diff --git a/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml b/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml new file mode 100644 index 0000000000000..0f1b1b9efbb6d --- /dev/null +++ b/planning/autoware_remaining_distance_time_calculator/param/remaining_distance_time_calculator_parameters.yaml @@ -0,0 +1,4 @@ +remaining_distance_time_calculator: + update_rate: + type: double + description: Timer callback period. [Hz] diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp index 383e85731604e..74f3f2f43077d 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.cpp @@ -37,6 +37,7 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( : Node("remaining_distance_time_calculator", options), is_graph_ready_{false}, has_received_route_{false}, + has_received_scenario_{false}, velocity_limit_{99.99}, remaining_distance_{0.0}, remaining_time_{0.0} @@ -57,14 +58,18 @@ RemainingDistanceTimeCalculatorNode::RemainingDistanceTimeCalculatorNode( sub_planning_velocity_ = create_subscription( "/planning/scenario_planning/current_max_velocity", qos_transient_local, std::bind(&RemainingDistanceTimeCalculatorNode::on_velocity_limit, this, _1)); + sub_scenario_ = this->create_subscription( + "~/input/scenario", 1, std::bind(&RemainingDistanceTimeCalculatorNode::on_scenario, this, _1)); pub_mission_remaining_distance_time_ = create_publisher( "~/output/mission_remaining_distance_time", rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable()); - node_param_.update_rate = declare_parameter("update_rate"); + param_listener_ = std::make_shared<::remaining_distance_time_calculator::ParamListener>( + this->get_node_parameters_interface()); + const auto param = param_listener_->get_params(); - const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); + const auto period_ns = rclcpp::Rate(param.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), period_ns, std::bind(&RemainingDistanceTimeCalculatorNode::on_timer, this)); } @@ -100,9 +105,25 @@ void RemainingDistanceTimeCalculatorNode::on_velocity_limit( } } +void RemainingDistanceTimeCalculatorNode::on_scenario( + const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg) +{ + scenario_ = msg; + has_received_scenario_ = true; +} + void RemainingDistanceTimeCalculatorNode::on_timer() { - if (is_graph_ready_ && has_received_route_) { + if (!has_received_scenario_) { + return; + } + + // check if the scenario is parking or not + if (scenario_->current_scenario == tier4_planning_msgs::msg::Scenario::PARKING) { + remaining_distance_ = 0.0; + remaining_time_ = 0.0; + publish_mission_remaining_distance_time(); + } else if (is_graph_ready_ && has_received_route_) { calculate_remaining_distance(); calculate_remaining_time(); publish_mission_remaining_distance_time(); diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index d6c900af4dfed..8a38941d383ee 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -24,6 +25,7 @@ #include #include #include +#include #include #include @@ -38,11 +40,6 @@ namespace autoware::remaining_distance_time_calculator { -struct NodeParam -{ - double update_rate{0.0}; -}; - class RemainingDistanceTimeCalculatorNode : public rclcpp::Node { public: @@ -59,6 +56,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_planning_velocity_; + rclcpp::Subscription::SharedPtr sub_scenario_; rclcpp::Publisher::SharedPtr pub_mission_remaining_distance_time_; @@ -75,14 +73,16 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node geometry_msgs::msg::Pose current_vehicle_pose_; geometry_msgs::msg::Vector3 current_vehicle_velocity_; geometry_msgs::msg::Pose goal_pose_; + tier4_planning_msgs::msg::Scenario::ConstSharedPtr scenario_; bool has_received_route_; + bool has_received_scenario_; double velocity_limit_; double remaining_distance_; double remaining_time_; // Parameter - NodeParam node_param_; + std::shared_ptr<::remaining_distance_time_calculator::ParamListener> param_listener_; // Callbacks void on_timer(); @@ -90,6 +90,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node void on_route(const LaneletRoute::ConstSharedPtr & msg); void on_map(const HADMapBin::ConstSharedPtr & msg); void on_velocity_limit(const VelocityLimit::ConstSharedPtr & msg); + void on_scenario(const tier4_planning_msgs::msg::Scenario::ConstSharedPtr & msg); /** * @brief calculate mission remaining distance diff --git a/planning/autoware_route_handler/CHANGELOG.rst b/planning/autoware_route_handler/CHANGELOG.rst index d70e5897c96b5..eb61d677e96ce 100644 --- a/planning/autoware_route_handler/CHANGELOG.rst +++ b/planning/autoware_route_handler/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_route_handler ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_route_handler): fix bugprone-exception-escape (`#9738 `_) + * fix: bugprone-error + * fix: add include + --------- +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_route_handler/package.xml b/planning/autoware_route_handler/package.xml index ededbcc9eef3d..f6f770ea59daf 100644 --- a/planning/autoware_route_handler/package.xml +++ b/planning/autoware_route_handler/package.xml @@ -2,7 +2,7 @@ autoware_route_handler - 0.40.0 + 0.41.0 The route_handling package Fumiya Watanabe Zulfaqar Azmi diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index 01e52d85f01c3..89930fd4a23a9 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include #include @@ -1163,32 +1164,40 @@ lanelet::ConstLanelets RouteHandler::getAllLeftSharedLinestringLanelets( const bool & invert_opposite) const noexcept { lanelet::ConstLanelets linestring_shared; - auto lanelet_at_left = getLeftLanelet(lane); - auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); - while (lanelet_at_left) { - linestring_shared.push_back(lanelet_at_left.value()); - lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); - if (!lanelet_at_left) { - break; + try { + auto lanelet_at_left = getLeftLanelet(lane); + auto lanelet_at_left_opposite = getLeftOppositeLanelets(lane); + while (lanelet_at_left) { + linestring_shared.push_back(lanelet_at_left.value()); + lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); + if (!lanelet_at_left) { + break; + } + lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value()); } - lanelet_at_left_opposite = getLeftOppositeLanelets(lanelet_at_left.value()); - } - if (!lanelet_at_left_opposite.empty() && include_opposite) { - if (invert_opposite) { - linestring_shared.push_back(lanelet_at_left_opposite.front().invert()); - } else { - linestring_shared.push_back(lanelet_at_left_opposite.front()); - } - auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); - while (lanelet_at_right) { + if (!lanelet_at_left_opposite.empty() && include_opposite) { if (invert_opposite) { - linestring_shared.push_back(lanelet_at_right.value().invert()); + linestring_shared.push_back(lanelet_at_left_opposite.front().invert()); } else { - linestring_shared.push_back(lanelet_at_right.value()); + linestring_shared.push_back(lanelet_at_left_opposite.front()); + } + auto lanelet_at_right = getRightLanelet(lanelet_at_left_opposite.front()); + while (lanelet_at_right) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_right.value().invert()); + } else { + linestring_shared.push_back(lanelet_at_right.value()); + } + lanelet_at_right = getRightLanelet(lanelet_at_right.value()); } - lanelet_at_right = getRightLanelet(lanelet_at_right.value()); } + } catch (const std::exception & e) { + std::cerr << "Exception in getAllLeftSharedLinestringLanelets: " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in getAllLeftSharedLinestringLanelets" << std::endl; + return {}; } return linestring_shared; } @@ -1198,32 +1207,40 @@ lanelet::ConstLanelets RouteHandler::getAllRightSharedLinestringLanelets( const bool & invert_opposite) const noexcept { lanelet::ConstLanelets linestring_shared; - auto lanelet_at_right = getRightLanelet(lane); - auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); - while (lanelet_at_right) { - linestring_shared.push_back(lanelet_at_right.value()); - lanelet_at_right = getRightLanelet(lanelet_at_right.value()); - if (!lanelet_at_right) { - break; + try { + auto lanelet_at_right = getRightLanelet(lane); + auto lanelet_at_right_opposite = getRightOppositeLanelets(lane); + while (lanelet_at_right) { + linestring_shared.push_back(lanelet_at_right.value()); + lanelet_at_right = getRightLanelet(lanelet_at_right.value()); + if (!lanelet_at_right) { + break; + } + lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value()); } - lanelet_at_right_opposite = getRightOppositeLanelets(lanelet_at_right.value()); - } - if (!lanelet_at_right_opposite.empty() && include_opposite) { - if (invert_opposite) { - linestring_shared.push_back(lanelet_at_right_opposite.front().invert()); - } else { - linestring_shared.push_back(lanelet_at_right_opposite.front()); - } - auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); - while (lanelet_at_left) { + if (!lanelet_at_right_opposite.empty() && include_opposite) { if (invert_opposite) { - linestring_shared.push_back(lanelet_at_left.value().invert()); + linestring_shared.push_back(lanelet_at_right_opposite.front().invert()); } else { - linestring_shared.push_back(lanelet_at_left.value()); + linestring_shared.push_back(lanelet_at_right_opposite.front()); + } + auto lanelet_at_left = getLeftLanelet(lanelet_at_right_opposite.front()); + while (lanelet_at_left) { + if (invert_opposite) { + linestring_shared.push_back(lanelet_at_left.value().invert()); + } else { + linestring_shared.push_back(lanelet_at_left.value()); + } + lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); } - lanelet_at_left = getLeftLanelet(lanelet_at_left.value()); } + } catch (const std::exception & e) { + std::cerr << "Exception in getAllRightSharedLinestringLanelets: " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in getAllRightSharedLinestringLanelets" << std::endl; + return {}; } return linestring_shared; } diff --git a/planning/autoware_rtc_interface/CHANGELOG.rst b/planning/autoware_rtc_interface/CHANGELOG.rst index 11672d2c47e3e..b2bb8027a5efb 100644 --- a/planning/autoware_rtc_interface/CHANGELOG.rst +++ b/planning/autoware_rtc_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_rtc_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_rtc_interface): fix bugprone-branch-clone (`#9698 `_) +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_rtc_interface/package.xml b/planning/autoware_rtc_interface/package.xml index 861554e81408e..c6d35ea0e5e93 100644 --- a/planning/autoware_rtc_interface/package.xml +++ b/planning/autoware_rtc_interface/package.xml @@ -1,7 +1,7 @@ autoware_rtc_interface - 0.40.0 + 0.41.0 The autoware_rtc_interface package Fumiya Watanabe diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp index 47bc278c30564..7fcd0596b6b32 100644 --- a/planning/autoware_rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -80,9 +80,7 @@ Module getModuleType(const std::string & module_name) module.type = Module::OCCLUSION_SPOT; } else if (module_name == "stop_line") { module.type = Module::NONE; - } else if (module_name == "traffic_light") { - module.type = Module::TRAFFIC_LIGHT; - } else if (module_name == "virtual_traffic_light") { + } else if (module_name == "traffic_light" || module_name == "virtual_traffic_light") { module.type = Module::TRAFFIC_LIGHT; } else if (module_name == "lane_change_left") { module.type = Module::LANE_CHANGE_LEFT; diff --git a/planning/autoware_scenario_selector/CHANGELOG.rst b/planning/autoware_scenario_selector/CHANGELOG.rst index 877192f0e68f9..868adbed5c554 100644 --- a/planning/autoware_scenario_selector/CHANGELOG.rst +++ b/planning/autoware_scenario_selector/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_scenario_selector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_scenario_selector): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_scenario_selector (`#9914 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_scenario_selector +* fix(autoware_scenario_selector): fix bugprone-branch-clone (`#9699 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp index 244876ee2cb22..7e215fcfef8dd 100644 --- a/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp +++ b/planning/autoware_scenario_selector/include/autoware/scenario_selector/node.hpp @@ -19,13 +19,13 @@ #include #include +#include #include #include #include #include #include #include -#include #include #include @@ -97,7 +97,8 @@ class ScenarioSelectorNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // polling subscribers universe_utils::InterProcessPollingSubscriber< diff --git a/planning/autoware_scenario_selector/package.xml b/planning/autoware_scenario_selector/package.xml index e726e40acecce..eabd381891771 100644 --- a/planning/autoware_scenario_selector/package.xml +++ b/planning/autoware_scenario_selector/package.xml @@ -2,7 +2,7 @@ autoware_scenario_selector - 0.40.0 + 0.41.0 The autoware_scenario_selector ROS 2 package Taiki Tanaka Tomoya Kimura diff --git a/planning/autoware_scenario_selector/src/node.cpp b/planning/autoware_scenario_selector/src/node.cpp index 4bb64e27368d8..91e86bfea6dba 100644 --- a/planning/autoware_scenario_selector/src/node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -156,9 +156,8 @@ std::string ScenarioSelectorNode::selectScenarioByPosition() return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } else if (is_in_parking_lot) { return tier4_planning_msgs::msg::Scenario::PARKING; - } else { - return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } + return tier4_planning_msgs::msg::Scenario::LANEDRIVING; } if (current_scenario_ == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { @@ -391,7 +390,7 @@ void ScenarioSelectorNode::onTimer() pub_scenario_->publish(scenario); // Publish ProcessingTime - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); @@ -491,8 +490,8 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); published_time_publisher_ = std::make_unique(this); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); } } // namespace autoware::scenario_selector diff --git a/planning/autoware_static_centerline_generator/CHANGELOG.rst b/planning/autoware_static_centerline_generator/CHANGELOG.rst deleted file mode 100644 index 0338cdc8b66bb..0000000000000 --- a/planning/autoware_static_centerline_generator/CHANGELOG.rst +++ /dev/null @@ -1,287 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_static_centerline_generator -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.40.0 (2024-12-12) -------------------- -* Merge branch 'main' into release-0.40.0 -* Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" - This reverts commit c9f0f2688c57b0f657f5c1f28f036a970682e7f5. -* fix(static_centerline_generator): several bug fixes (`#9426 `_) - * fix: dependent packages - * feat: use steer angle, use warn for steer angle failure, calc curvature dicontinuously - * fix cppcheck - --------- -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* chore(package.xml): bump version to 0.39.0 (`#9587 `_) - * chore(package.xml): bump version to 0.39.0 - * fix: fix ticket links in CHANGELOG.rst - * fix: remove unnecessary diff - --------- - Co-authored-by: Yutaka Kondo -* fix: fix ticket links in CHANGELOG.rst (`#9588 `_) -* fix(cpplint): include what you use - planning (`#9570 `_) -* refactor(global_parameter_loader): prefix package and namespace with autoware (`#9303 `_) -* feat!: replace tier4_map_msgs with autoware_map_msgs for MapProjectorInfo (`#9392 `_) -* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) - * make lanelet2_map_visualization independent - * remove unused files - * remove unused package - * fix package name - * add autoware\_ prefix - * add autoware to exec name - * add autoware prefix - * removed unnecessary dependency - --------- -* 0.39.0 -* update changelog -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix(static_centerline_generator): map_tf_generator package name needs update (`#9383 `_) - fix map_tf_generator name in autoware_static_centerline_generator.launch -* refactor(map_loader)!: prefix package and namespace with autoware (`#8927 `_) - * make lanelet2_map_visualization independent - * remove unused files - * remove unused package - * fix package name - * add autoware\_ prefix - * add autoware to exec name - * add autoware prefix - * removed unnecessary dependency - --------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Fumiya Watanabe, M. Fatih Cırıt, Masaki Baba, Ryohsuke Mitsudome, Takayuki Murooka, Yutaka Kondo, Zhanhong Yan - -0.39.0 (2024-11-25) -------------------- -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* fix: fix ticket links to point to https://github.com/autowarefoundation/autoware.universe (`#9304 `_) -* chore(package.xml): bump version to 0.38.0 (`#9266 `_) (`#9284 `_) - * unify package.xml version to 0.37.0 - * remove system_monitor/CHANGELOG.rst - * add changelog - * 0.38.0 - --------- -* Contributors: Esteve Fernandez, Yutaka Kondo - -0.38.0 (2024-11-08) -------------------- -* unify package.xml version to 0.37.0 -* refactor(osqp_interface): added autoware prefix to osqp_interface (`#8958 `_) -* refactor(autoware_interpolation): prefix package and namespace with autoware (`#8088 `_) - Co-authored-by: kosuke55 -* fix(other_planning_packages): align the parameters with launcher (`#8793 `_) - * parameters in planning/others aligned - * update json - --------- -* refactor(map_projection_loader)!: prefix package and namespace with autoware (`#8420 `_) - * add autoware\_ prefix - * add autoware\_ prefix - --------- - Co-authored-by: SakodaShintaro -* fix(autoware_static_centerline_generator): fix unusedFunction (`#8647 `_) - * fix:unusedFunction - * fix:unusedFunction - * fix:compile error - --------- -* refactor(geography_utils): prefix package and namespace with autoware (`#7790 `_) - * refactor(geography_utils): prefix package and namespace with autoware - * move headers to include/autoware/ - --------- -* fix(autoware_static_centerline_generator): fix funcArgNamesDifferent (`#8019 `_) - fix:funcArgNamesDifferent -* fix(static_centerline_generator): save_map only once (`#7770 `_) -* refactor(static_centerline_optimizer): clean up the code (`#7756 `_) -* feat: add `autoware\_` prefix to `lanelet2_extension` (`#7640 `_) -* feat(static_centerline_generator): organize AUTO/GUI/VMB modes (`#7432 `_) -* refactor(universe_utils/motion_utils)!: add autoware namespace (`#7594 `_) -* refactor(motion_utils)!: add autoware prefix and include dir (`#7539 `_) - refactor(motion_utils): add autoware prefix and include dir -* feat(autoware_universe_utils)!: rename from tier4_autoware_utils (`#7538 `_) - Co-authored-by: kosuke55 -* refactor(route_handler)!: rename to include/autoware/{package_name} (`#7530 `_) - refactor(route_handler)!: rename to include/autoware/{package_name} -* feat(map_loader): add waypoints flag (`#7480 `_) - * feat(map_loader): handle centelrine and waypoints - * update README - * fix doc - * update schema - * fix - * fix - --------- -* feat(path_optimizer): rename to include/autoware/{package_name} (`#7529 `_) -* feat(path_smoother): rename to include/autoware/{package_name} (`#7527 `_) - * feat(path_smoother): rename to include/autoware/{package_name} - * fix - --------- -* refactor(behaivor_path_planner)!: rename to include/autoware/{package_name} (`#7522 `_) - * refactor(behavior_path_planner)!: make autoware dir in include - * refactor(start_planner): make autoware include dir - * refactor(goal_planner): make autoware include dir - * sampling planner module - * fix sampling planner build - * dynamic_avoidance - * lc - * side shift - * autoware_behavior_path_static_obstacle_avoidance_module - * autoware_behavior_path_planner_common - * make behavior_path dir - * pre-commit - * fix pre-commit - * fix build - --------- -* feat(mission_planner): rename to include/autoware/{package_name} (`#7513 `_) - * feat(mission_planner): rename to include/autoware/{package_name} - * feat(mission_planner): rename to include/autoware/{package_name} - * feat(mission_planner): rename to include/autoware/{package_name} - --------- -* fix(static_centerline_generator): fix dependency (`#7442 `_) - * fix: deps - * fix: package name - * fix: package name - --------- -* refactor(route_handler): route handler add autoware prefix (`#7341 `_) - * rename route handler package - * update packages dependencies - * update include guards - * update includes - * put in autoware namespace - * fix formats - * keep header and source file name as before - --------- -* refactor(mission_planner)!: add autoware prefix and namespace (`#7414 `_) - * refactor(mission_planner)!: add autoware prefix and namespace - * fix svg - --------- -* refactor(vehicle_info_utils)!: prefix package and namespace with autoware (`#7353 `_) - * chore(autoware_vehicle_info_utils): rename header - * chore(bpp-common): vehicle info - * chore(path_optimizer): vehicle info - * chore(velocity_smoother): vehicle info - * chore(bvp-common): vehicle info - * chore(static_centerline_generator): vehicle info - * chore(obstacle_cruise_planner): vehicle info - * chore(obstacle_velocity_limiter): vehicle info - * chore(mission_planner): vehicle info - * chore(obstacle_stop_planner): vehicle info - * chore(planning_validator): vehicle info - * chore(surround_obstacle_checker): vehicle info - * chore(goal_planner): vehicle info - * chore(start_planner): vehicle info - * chore(control_performance_analysis): vehicle info - * chore(lane_departure_checker): vehicle info - * chore(predicted_path_checker): vehicle info - * chore(vehicle_cmd_gate): vehicle info - * chore(obstacle_collision_checker): vehicle info - * chore(operation_mode_transition_manager): vehicle info - * chore(mpc): vehicle info - * chore(control): vehicle info - * chore(common): vehicle info - * chore(perception): vehicle info - * chore(evaluator): vehicle info - * chore(freespace): vehicle info - * chore(planning): vehicle info - * chore(vehicle): vehicle info - * chore(simulator): vehicle info - * chore(launch): vehicle info - * chore(system): vehicle info - * chore(sensing): vehicle info - * fix(autoware_joy_controller): remove unused deps - --------- -* refactor(path_smoother)!: prefix package and namespace with autoware (`#7381 `_) - * git mv - * fix - * fix launch - * rever a part of prefix - * fix test - * fix - * fix static_centerline_optimizer - * fix - --------- -* refactor(path_optimizer, velocity_smoother)!: prefix package and namespace with autoware (`#7354 `_) - * chore(autoware_velocity_smoother): update namespace - * chore(autoware_path_optimizer): update namespace - --------- -* chore(bpp): add prefix `autoware\_` (`#7288 `_) - * chore(common): rename package - * fix(static_obstacle_avoidance): fix header - * fix(dynamic_obstacle_avoidance): fix header - * fix(side_shift): fix header - * fix(sampling_planner): fix header - * fix(start_planner): fix header - * fix(goal_planner): fix header - * fix(lane_change): fix header - * fix(external_lane_change): fix header - * fix(AbLC): fix header - * fix(bpp-node): fix header - * fix(static_centerline_generator): fix header - * fix(.pages): update link - --------- -* feat!: replace autoware_auto_msgs with autoware_msgs for planning modules (`#7246 `_) - Co-authored-by: Cynthia Liu - Co-authored-by: NorahXiong - Co-authored-by: beginningfan -* chore(autoware_velocity_smoother, autoware_path_optimizer): rename packages (`#7202 `_) - * chore(autoware_path_optimizer): rename package and namespace - * chore(autoware_static_centerline_generator): rename package and namespace - * chore: update module name - * chore(autoware_velocity_smoother): rename package and namespace - * chore(tier4_planning_launch): update module name - * chore: update module name - * fix: test - * fix: test - * fix: test - --------- -* refactor(behavior_velocity_planner)!: prefix package and namespace with autoware\_ (`#6693 `_) -* fix(autoware_static_centerline_generator): update the centerline correctly with map projector (`#6825 `_) - * fix(static_centerline_generator): fixed the bug of offset lat/lon values - * fix typo - --------- -* fix(autoware_static_centerline_generator): remove prefix from topics and node names (`#7028 `_) -* build(static_centerline_generator): prefix package and namespace with autoware\_ (`#6817 `_) - * build(static_centerline_generator): prefix package and namespace with autoware\_ - * style(pre-commit): autofix - * build: fix CMake target - * build(autoware_static_centerline_generator): more renames - * style(pre-commit): autofix - * build(autoware_static_centerline_generator): fix namespace - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * style(pre-commit): autofix - * style(pre-commit): autofix - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * fix(autoware_static_centerline_generator): fix build issues - * fix(autoware_static_centerline_generator): fix build issues - * style(pre-commit): autofix - * fix(autoware_static_centerline_optimizer): fix clang-tidy issues - * style(pre-commit): autofix - * build: fix build errors - * fix: remove else statements after return - * fix(autoware_static_centerline_generator): fix clang-tidy issues - * style(pre-commit): autofix - * revert changes for static_centerline_generator - * fix(autoware_static_centerline_generator): add autoware\_ prefix - * style(pre-commit): autofix - * fix(autoware_static_centerline_generator): fix filenames - * fix(autoware_static_centerline_generator): fix namespaces - * style(pre-commit): autofix - * fix: added prefix to missing strings - * refactor(autoware_static_centerline_generator): move header files to src - * refactor(autoware_static_centerline_generator): fix include paths - * style(pre-commit): autofix - * refactor(autoware_static_centerline_generator): rename base folder - * Update planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml - Co-authored-by: M. Fatih Cırıt - * build(autoware_static_centerline_generator): fix include in CMake - * build(autoware_static_centerline_generator): fix missing includes - --------- - Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> - Co-authored-by: M. Fatih Cırıt -* Contributors: Esteve Fernandez, Kosuke Takeuchi, Masaki Baba, Ryohsuke Mitsudome, Satoshi OTA, Takayuki Murooka, Yutaka Kondo, Zhe Shen, kobayu858, mkquda - -0.26.0 (2024-04-03) -------------------- diff --git a/planning/autoware_static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt deleted file mode 100644 index 261e8beb0022f..0000000000000 --- a/planning/autoware_static_centerline_generator/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_static_centerline_generator) - -find_package(autoware_cmake REQUIRED) - -find_package(builtin_interfaces REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) - -rosidl_generate_interfaces( - autoware_static_centerline_generator - "srv/LoadMap.srv" - "srv/PlanRoute.srv" - "srv/PlanPath.srv" - "msg/PointsWithLaneId.msg" - DEPENDENCIES builtin_interfaces geometry_msgs -) - -autoware_package() - -ament_auto_add_executable(main - src/main.cpp - src/static_centerline_generator_node.cpp - src/centerline_source/optimization_trajectory_based_centerline.cpp - src/centerline_source/bag_ego_trajectory_based_centerline.cpp - src/utils.cpp -) - -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(main - autoware_static_centerline_generator "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp") - target_link_libraries(main "${cpp_typesupport_target}") -endif() - -ament_auto_package( - INSTALL_TO_SHARE - launch - config - rviz -) - -target_include_directories(main PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src -) - -if(BUILD_TESTING) - add_launch_test( - test/test_static_centerline_generator.test.py - TIMEOUT "30" - ) - install(DIRECTORY - test/data/ - DESTINATION share/${PROJECT_NAME}/test/data/ - ) -endif() - -install(PROGRAMS - scripts/app.py - scripts/centerline_updater_helper.py - scripts/show_lanelet2_map_diff.py - DESTINATION lib/${PROJECT_NAME} -) diff --git a/planning/autoware_static_centerline_generator/README.md b/planning/autoware_static_centerline_generator/README.md deleted file mode 100644 index 426d5487cf0cb..0000000000000 --- a/planning/autoware_static_centerline_generator/README.md +++ /dev/null @@ -1,83 +0,0 @@ -# Static Centerline Generator - -## Purpose - -This package statically calculates the centerline satisfying path footprints inside the drivable area. - -On narrow-road driving, the default centerline, which is the middle line between lanelets' right and left boundaries, often causes path footprints outside the drivable area. -To make path footprints inside the drivable area, we use online path shape optimization by [the autoware_path_optimizer package](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/autoware_path_optimizer/). - -Instead of online path shape optimization, we introduce static centerline optimization. -With this static centerline optimization, we have following advantages. - -- We can see the optimized centerline shape in advance. - - With the default autoware, path shape is not determined until the vehicle drives there. - - This enables offline path shape evaluation. -- We do not have to calculate a heavy and sometimes unstable path optimization since the path footprints are already inside the drivable area. - -## Use cases - -There are two interfaces to communicate with the centerline optimizer. - -### Vector Map Builder Interface - -Note: This function of Vector Map Builder has not been released. Please wait for a while. -Currently there is no documentation about Vector Map Builder's operation for this function. - -The optimized centerline can be generated from Vector Map Builder's operation. - -We can run - -- path planning server -- http server to connect path planning server and Vector Map Builder - -with the following command by designating `` - -```sh -ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml vehicle_model:= -``` - -FYI, port ID of the http server is 4010 by default. - -### Command Line Interface - -The optimized centerline can be generated from the command line interface by designating - -- `` -- `` (not mandatory) -- `` -- `` -- `` - -```sh -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= -``` - -The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. -If you want to change the output map path, you can remap the path by designating ``. - -## Visualization - -When launching the path planning server, rviz is launched as well as follows. -![rviz](./media/rviz.png) - -- The yellow footprints are the original ones from the osm map file. - - FYI: Footprints are generated based on the centerline and vehicle size. -- The red footprints are the optimized ones. -- The gray area is the drivable area. -- You can see that the red footprints are inside the drivable area although the yellow ones are outside. - -### Unsafe footprints - -Sometimes the optimized centerline footprints are close to the lanes' boundaries. -We can check how close they are with `unsafe footprints` marker as follows. - -Footprints' color depends on its distance to the boundaries, and text expresses its distance. - -![rviz](./media/unsafe_footprints.png) - -By default, footprints' color is - -- when the distance is less than 0.1 [m] : red -- when the distance is less than 0.2 [m] : green -- when the distance is less than 0.3 [m] : blue diff --git a/planning/autoware_static_centerline_generator/config/common.param.yaml b/planning/autoware_static_centerline_generator/config/common.param.yaml deleted file mode 100644 index 6c547c8cd83dc..0000000000000 --- a/planning/autoware_static_centerline_generator/config/common.param.yaml +++ /dev/null @@ -1,17 +0,0 @@ -/**: - ros__parameters: - # constraints param for normal driving - max_vel: 11.1 # max velocity limit [m/s] - - normal: - min_acc: -1.0 # min deceleration [m/ss] - max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -1.0 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] - - # constraints to be observed - limit: - min_acc: -2.5 # min deceleration limit [m/ss] - max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml b/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml deleted file mode 100644 index eb6709764ce3e..0000000000000 --- a/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - # ego - ego_nearest_dist_threshold: 3.0 # [m] - ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml deleted file mode 100644 index 060590803428a..0000000000000 --- a/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml +++ /dev/null @@ -1,9 +0,0 @@ -/**: - ros__parameters: - marker_color: ["FF0000", "FF5A00", "FFFF00"] - marker_color_dist_thresh : [0.1, 0.2, 0.3] - output_trajectory_interval: 1.0 - - validation: - dist_threshold_to_road_border: 0.0 - max_steer_angle_margin: 0.0 # [rad] NOTE: Positive value makes max steer angle threshold to decrease. diff --git a/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml b/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml deleted file mode 100644 index 8941b92b4e78e..0000000000000 --- a/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml +++ /dev/null @@ -1,12 +0,0 @@ -/**: - ros__parameters: - wheel_radius: 0.39 - wheel_width: 0.42 - wheel_base: 2.74 # between front wheel center and rear wheel center - wheel_tread: 1.63 # between left wheel center and right wheel center - front_overhang: 1.0 # between front wheel center and vehicle front - rear_overhang: 1.03 # between rear wheel center and vehicle rear - left_overhang: 0.1 # between left wheel center and vehicle left - right_overhang: 0.1 # between right wheel center and vehicle right - vehicle_height: 2.5 - max_steer_angle: 0.70 # [rad] diff --git a/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml b/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml deleted file mode 100644 index cb368ca336316..0000000000000 --- a/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml deleted file mode 100644 index 0590009ab378c..0000000000000 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ /dev/null @@ -1,91 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/media/rviz.png b/planning/autoware_static_centerline_generator/media/rviz.png deleted file mode 100644 index e76ede78294f0..0000000000000 Binary files a/planning/autoware_static_centerline_generator/media/rviz.png and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/media/unsafe_footprints.png b/planning/autoware_static_centerline_generator/media/unsafe_footprints.png deleted file mode 100644 index b4404f4cfa89b..0000000000000 Binary files a/planning/autoware_static_centerline_generator/media/unsafe_footprints.png and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg b/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg deleted file mode 100644 index 97cd0355df831..0000000000000 --- a/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg +++ /dev/null @@ -1,2 +0,0 @@ -int64 lane_id -geometry_msgs/Point[] points diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml deleted file mode 100644 index cea18e5605921..0000000000000 --- a/planning/autoware_static_centerline_generator/package.xml +++ /dev/null @@ -1,57 +0,0 @@ - - - - autoware_static_centerline_generator - 0.40.0 - The autoware_static_centerline_generator package - Takayuki Murooka - Kosuke Takeuchi - Apache License 2.0 - - Takayuki Murooka - - ament_cmake_auto - autoware_cmake - - rosidl_default_generators - - autoware_behavior_path_planner_common - autoware_geography_utils - autoware_global_parameter_loader - autoware_interpolation - autoware_lanelet2_extension - autoware_lanelet2_map_visualizer - autoware_map_loader - autoware_map_msgs - autoware_map_projection_loader - autoware_map_tf_generator - autoware_mission_planner - autoware_motion_utils - autoware_osqp_interface - autoware_path_optimizer - autoware_path_smoother - autoware_perception_msgs - autoware_planning_msgs - autoware_route_handler - autoware_universe_utils - autoware_vehicle_info_utils - geometry_msgs - rclcpp - rclcpp_components - - python3-flask-cors - rosidl_default_runtime - - ament_cmake_gmock - ament_lint_auto - autoware_behavior_path_planner - autoware_behavior_velocity_planner - autoware_lint_common - ros_testing - - rosidl_interface_packages - - - ament_cmake - - diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz deleted file mode 100644 index f1bd110783009..0000000000000 --- a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz +++ /dev/null @@ -1,459 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.550000011920929 - Tree Height: 386 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Dummy Car1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" - - Class: rviz_plugins::AutowareStatePanel - Name: AutowareStatePanel -Visualization Manager: - Class: "" - Displays: - - Class: rviz_common/Group - Displays: - - Class: rviz_default_plugins/TF - Enabled: false - Frame Timeout: 15 - Frames: - All Enabled: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - {} - Update Interval: 0 - Value: false - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: false - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: false - Enabled: false - Name: System - - Class: rviz_common/Group - Displays: - - Alpha: 0.20000000298023224 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 28.71826171875 - Min Value: -7.4224700927734375 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: FlatColor - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 237 - Min Color: 211; 215; 207 - Min Intensity: 0 - Name: PointCloudMap - Position Transformer: XYZ - Selectable: false - Size (Pixels): 1 - Size (m): 0.019999999552965164 - Style: Points - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/pointcloud_map - Use Fixed Frame: true - Use rainbow: false - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Lanelet2VectorMap - Namespaces: - center_lane_line: true - center_line_arrows: true - lane_start_bound: false - lanelet_id: false - left_lane_bound: true - right_lane_bound: true - road_lanelets: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /map/vector_map_marker - Value: true - Enabled: true - Name: Map - - Class: rviz_plugins/PathWithLaneId - Color Border Vel Max: 3 - Enabled: false - Name: Raw Centerline - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/input_centerline - Value: true - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View LaneId: - Scale: 0.10000000149011612 - Value: false - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: false - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_plugins/Trajectory - Color Border Vel Max: 3 - Enabled: true - Name: Optimized Centerline - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/output/centerline - Value: true - View Footprint: - Alpha: 0.5 - Color: 0; 255; 0 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: false - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: false - - Class: rviz_common/Group - Displays: - - Class: rviz_plugins/Path - Color Border Vel Max: 3 - Enabled: false - Name: Raw Centerline (Path type) - Topic: - Depth: 5 - Durability Policy: Transient Local - Filter size: 10 - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/raw_centerline - Value: false - View Drivable Area: - Alpha: 0.9990000128746033 - Color: 0; 148; 205 - Value: true - Width: 0.30000001192092896 - View Footprint: - Alpha: 1 - Color: 230; 230; 50 - Offset from BaseLink: 0 - Rear Overhang: 1.0299999713897705 - Value: true - Vehicle Length: 4.769999980926514 - Vehicle Width: 1.8300000429153442 - View Path: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Value: true - Width: 2 - View Point: - Alpha: 1 - Color: 0; 60; 255 - Offset: 0 - Radius: 0.10000000149011612 - Value: false - View Text Velocity: - Scale: 0.30000001192092896 - Value: false - View Velocity: - Alpha: 1 - Color: 0; 0; 0 - Constant Color: false - Scale: 0.30000001192092896 - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/output/validation_results - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: true - Name: Debug Markers - Namespaces: - curvature: false - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/markers - Value: true - - Class: rviz_default_plugins/MarkerArray - Enabled: false - Name: MarkerArray - Namespaces: - {} - Topic: - Depth: 5 - Durability Policy: Transient Local - History Policy: Keep Last - Reliability Policy: Reliable - Value: /static_centerline_generator/debug/ego_footprint_bounds - Value: true - Enabled: true - Name: debug - Enabled: true - Global Options: - Background Color: 10; 10; 10 - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /planning/mission_planning/goal - - Acceleration: 0 - Class: rviz_plugins/PedestrianInitialPoseTool - Interactive: false - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 1 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/CarInitialPoseTool - H vehicle height: 2 - Interactive: false - L vehicle length: 4 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 8 - W vehicle width: 1.7999999523162842 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Acceleration: 0 - Class: rviz_plugins/BusInitialPoseTool - H vehicle height: 3.5 - Interactive: false - L vehicle length: 10.5 - Max velocity: 33.29999923706055 - Min velocity: -33.29999923706055 - Pose Topic: /simulation/dummy_perception_publisher/object_info - Target Frame: - Theta std deviation: 0.0872664600610733 - Velocity: 0 - W vehicle width: 2.5 - X std deviation: 0.029999999329447746 - Y std deviation: 0.029999999329447746 - Z position: 0 - Z std deviation: 0.029999999329447746 - - Class: rviz_plugins/MissionCheckpointTool - Pose Topic: /planning/mission_planning/checkpoint - Theta std deviation: 0.2617993950843811 - X std deviation: 0.5 - Y std deviation: 0.5 - Z position: 0 - - Class: rviz_plugins/DeleteAllObjectsTool - Pose Topic: /simulation/dummy_perception_publisher/object_info - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz_default_plugins) - X: 0 - Y: 0 - Saved: - - Class: rviz_default_plugins/ThirdPersonFollower - Distance: 18 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: ThirdPersonFollower - Near Clip Distance: 0.009999999776482582 - Pitch: 0.20000000298023224 - Target Frame: base_link - Value: ThirdPersonFollower (rviz) - Yaw: 3.141592025756836 - - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Invert Z Axis: false - Name: TopDownOrtho - Near Clip Distance: 0.009999999776482582 - Scale: 10 - Target Frame: viewer - Value: TopDownOrtho (rviz) - X: 0 - Y: 0 -Window Geometry: - AutowareStatePanel: - collapsed: false - Displays: - collapsed: false - Height: 1043 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1920 - X: 0 - Y: 0 diff --git a/planning/autoware_static_centerline_generator/scripts/app.py b/planning/autoware_static_centerline_generator/scripts/app.py deleted file mode 100755 index 08bff8b80dcb7..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/app.py +++ /dev/null @@ -1,178 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 Tier IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import json -import uuid - -from autoware_static_centerline_generator.srv import LoadMap -from autoware_static_centerline_generator.srv import PlanPath -from autoware_static_centerline_generator.srv import PlanRoute -from flask import Flask -from flask import jsonify -from flask import request -from flask_cors import CORS -import rclpy -from rclpy.node import Node - -rclpy.init() -node = Node("static_centerline_generator_http_server") - -app = Flask(__name__) -CORS(app) - - -def create_client(service_type, server_name): - # create client - cli = node.create_client(service_type, server_name) - while not cli.wait_for_service(timeout_sec=1.0): - print("{} service not available, waiting again...".format(server_name)) - return cli - - -@app.route("/map", methods=["POST"]) -def get_map(): - data = request.get_json() - - map_uuid = str(uuid.uuid4()) - global map_id - map_id = map_uuid - - # create client - cli = create_client(LoadMap, "/planning/static_centerline_generator/load_map") - - # request map loading - req = LoadMap.Request(map=data["map"]) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "InvalidMapFormat": - return jsonify(code=res.message, message="Map format is invalid."), 400 - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - return map_uuid - - -@app.route("/planned_route", methods=["GET"]) -def post_planned_route(): - args = request.args.to_dict() - global map_id - if map_id != args.get("map_id"): - # TODO(murooka) error handling for map_id mismatch - print("map_id is not correct.") - - # create client - cli = create_client(PlanRoute, "/planning/static_centerline_generator/plan_route") - - # request route planning - req = PlanRoute.Request( - start_lane_id=int(args.get("start_lane_id")), end_lane_id=int(args.get("end_lane_id")) - ) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "MapNotFound": - return jsonify(code=res.message, message="Map is missing."), 404 - elif res.message == "RouteNotFound": - return jsonify(code=res.message, message="Planning route failed."), 404 - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - return json.dumps(tuple(res.lane_ids)) - - -@app.route("/planned_path", methods=["GET"]) -def post_planned_path(): - args = request.args.to_dict() - global map_id - if map_id != args.get("map_id"): - # TODO(murooka) error handling for map_id mismatch - print("map_id is not correct.") - - # create client - cli = create_client(PlanPath, "/planning/static_centerline_generator/plan_path") - - # request path planning - route_lane_ids = [eval(i) for i in request.args.getlist("route[]")] - req = PlanPath.Request(route=route_lane_ids) - future = cli.call_async(req) - - # get result - rclpy.spin_until_future_complete(node, future) - res = future.result() - - # error handling - if res.message == "MapNotFound": - return jsonify(code=res.message, message="Map is missing."), 404 - elif res.message == "LaneletsNotConnected": - return ( - jsonify( - code=res.message, - message="Lanelets are not connected.", - object_ids=tuple(res.unconnected_lane_ids), - ), - 400, - ) - elif res.message != "": - return ( - jsonify( - code="InternalServerError", - message="Error occurred on the server. Please check the terminal.", - ), - 500, - ) - - # create output json - result_json = [] - for points_with_lane_id in res.points_with_lane_ids: - current_lane_points = [] - for geom_point in points_with_lane_id.points: - point = {"x": geom_point.x, "y": geom_point.y, "z": geom_point.z} - current_lane_points.append(point) - - current_result_json = {} - current_result_json["lane_id"] = int(points_with_lane_id.lane_id) - current_result_json["points"] = current_lane_points - - result_json.append(current_result_json) - - return json.dumps(tuple(result_json)) - - -if __name__ == "__main__": - app.debug = True - app.secret_key = "tmp_secret_key" - app.run(host="localhost", port=4010) diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py deleted file mode 100755 index f3d908713361d..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ /dev/null @@ -1,207 +0,0 @@ -#!/bin/env python3 - -# 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. - - -import sys -import time - -from PyQt5 import QtCore -from PyQt5.QtWidgets import QApplication -from PyQt5.QtWidgets import QGroupBox -from PyQt5.QtWidgets import QMainWindow -from PyQt5.QtWidgets import QPushButton -from PyQt5.QtWidgets import QSizePolicy -from PyQt5.QtWidgets import QSlider -from PyQt5.QtWidgets import QVBoxLayout -from PyQt5.QtWidgets import QWidget -from autoware_planning_msgs.msg import Trajectory -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from std_msgs.msg import Empty -from std_msgs.msg import Float32 -from std_msgs.msg import Int32 - - -class CenterlineUpdaterWidget(QMainWindow): - def __init__(self): - super(self.__class__, self).__init__() - self.setupUI() - - def setupUI(self): - self.setObjectName("MainWindow") - self.resize(480, 120) - self.setWindowFlags(QtCore.Qt.WindowStaysOnTopHint) - - central_widget = QWidget(self) - self.grid_layout = QVBoxLayout(central_widget) - self.grid_layout.setContentsMargins(10, 10, 10, 10) - - # slide of the trajectory's start and end index - self.traj_start_index_slider = QSlider(QtCore.Qt.Horizontal) - self.traj_end_index_slider = QSlider(QtCore.Qt.Horizontal) - self.min_traj_start_index = 0 - self.max_traj_end_index = None - - # Layout: Range of Centerline - centerline_vertical_box = QVBoxLayout(self) - centerline_vertical_box.addWidget(self.traj_start_index_slider) - centerline_vertical_box.addWidget(self.traj_end_index_slider) - centerline_group = QGroupBox("Centerline") - centerline_group.setLayout(centerline_vertical_box) - self.grid_layout.addWidget(centerline_group) - - """ - # 2. Road Boundary - road_boundary_group = QGroupBox("Road Boundary") - road_boundary_vertical_box = QVBoxLayout(self) - road_boundary_group.setLayout(road_boundary_vertical_box) - self.grid_layout.addWidget(road_boundary_group) - - # 2.1. Slider - self.road_boundary_lateral_margin_slider = QSlider(QtCore.Qt.Horizontal) - road_boundary_vertical_box.addWidget(self.road_boundary_lateral_margin_slider) - self.road_boundary_lateral_margin_ratio = 10 - self.road_boundary_lateral_margin_slider.setMinimum(0) - self.road_boundary_lateral_margin_slider.setMaximum( - 5 * self.road_boundary_lateral_margin_ratio - ) - road_boundary_vertical_box.addWidget(QPushButton("reset")) - """ - - # 3. General - general_group = QGroupBox("General") - general_vertical_box = QVBoxLayout(self) - general_group.setLayout(general_vertical_box) - self.grid_layout.addWidget(general_group) - - # 3.1. Validate Centerline - self.validate_button = QPushButton("validate") - self.validate_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - general_vertical_box.addWidget(self.validate_button) - - # 3.2. Save Map - self.save_map_button = QPushButton("save map") - self.save_map_button.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - general_vertical_box.addWidget(self.save_map_button) - - self.setCentralWidget(central_widget) - - def initWithEndIndex(self, max_traj_end_index): - self.max_traj_end_index = max_traj_end_index - - # initialize sliders - self.traj_start_index_slider.setMinimum(self.min_traj_start_index) - self.traj_start_index_slider.setMaximum(self.max_traj_end_index) - self.traj_start_index_slider.setValue(self.min_traj_start_index) - - self.traj_end_index_slider.setMinimum(self.min_traj_start_index) - self.traj_end_index_slider.setMaximum(self.max_traj_end_index) - self.traj_end_index_slider.setValue(self.max_traj_end_index) - - -class CenterlineUpdaterHelper(Node): - def __init__(self): - super().__init__("centerline_updater_helper") - # Qt - self.widget = CenterlineUpdaterWidget() - self.widget.show() - self.widget.save_map_button.clicked.connect(self.onSaveMapButtonPushed) - self.widget.validate_button.clicked.connect(self.onValidateButtonPushed) - self.widget.traj_start_index_slider.valueChanged.connect(self.onStartIndexChanged) - self.widget.traj_end_index_slider.valueChanged.connect(self.onEndIndexChanged) - """ - self.widget.road_boundary_lateral_margin_slider.valueChanged.connect( - self.onRoadBoundaryLateralMargin - ) - """ - - # ROS - self.pub_save_map = self.create_publisher(Empty, "/static_centerline_generator/save_map", 1) - self.pub_validate = self.create_publisher(Empty, "/static_centerline_generator/validate", 1) - self.pub_traj_start_index = self.create_publisher( - Int32, "/static_centerline_generator/traj_start_index", 1 - ) - self.pub_traj_end_index = self.create_publisher( - Int32, "/static_centerline_generator/traj_end_index", 1 - ) - self.pub_road_boundary_lateral_margin = self.create_publisher( - Float32, "/static_centerline_generator/road_boundary_lateral_margin", 1 - ) - - transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - self.sub_whole_centerline = self.create_subscription( - Trajectory, - "/static_centerline_generator/output/whole_centerline", - self.onWholeCenterline, - transient_local_qos, - ) - - while self.widget.max_traj_end_index is None: - rclpy.spin_once(self, timeout_sec=0.01) - time.sleep(0.1) - - def onWholeCenterline(self, whole_centerline): - self.widget.initWithEndIndex(len(whole_centerline.points) - 1) - - def onSaveMapButtonPushed(self, event): - msg = Empty() - self.pub_save_map.publish(msg) - - # NOTE: After saving the map, the generated centerline is written - # in original_map_ptr_ in static_centerline_generator_node. - # When saving the map again, another centerline is written without - # removing the previous centerline. - # Therefore, saving the map can be called only once. - self.widget.save_map_button.setEnabled(False) - - def onValidateButtonPushed(self, event): - msg = Empty() - self.pub_validate.publish(msg) - - def onStartIndexChanged(self, event): - msg = Int32() - msg.data = self.widget.traj_start_index_slider.value() - self.pub_traj_start_index.publish(msg) - - def onEndIndexChanged(self, event): - msg = Int32() - msg.data = self.widget.traj_end_index_slider.value() - self.pub_traj_end_index.publish(msg) - - def onRoadBoundaryLateralMargin(self, event): - msg = Float32() - msg.data = ( - self.widget.road_boundary_lateral_margin_slider.value() - / self.widget.road_boundary_lateral_margin_ratio - ) - self.pub_road_boundary_lateral_margin.publish(msg) - - -def main(args=None): - app = QApplication(sys.argv) - - rclpy.init(args=args) - node = CenterlineUpdaterHelper() - - while True: - app.processEvents() - rclpy.spin_once(node, timeout_sec=0.01) - - -if __name__ == "__main__": - main() diff --git a/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py b/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py deleted file mode 100755 index 00d06ca2213d1..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py +++ /dev/null @@ -1,139 +0,0 @@ -#!/bin/env python3 - -# 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. - -import argparse -from decimal import Decimal -import os -import subprocess -import xml.etree.ElementTree as ET - - -def sort_attributes(root): - for shallow_element in root: - # sort nodes - attrib = shallow_element.attrib - if len(attrib) > 1: - attributes = sorted(attrib.items()) - attrib.clear() - attrib.update(attributes) - if shallow_element.tag == "relation": - pass - - # sort the element in the tag - for deep_element in shallow_element: - attrib = deep_element.attrib - if len(attrib) > 1: - # adjust attribute order, e.g. by sorting - attributes = sorted(attrib.items()) - attrib.clear() - attrib.update(attributes) - - # sort tags - sorted_shallow_element = sorted(shallow_element, key=lambda x: x.items()) - if len(shallow_element) != 0: - # NOTE: This "tail" is related to the indent of the next tag - first_tail = shallow_element[0].tail - last_tail = shallow_element[-1].tail - for idx, val_shallow_element in enumerate(sorted_shallow_element): - shallow_element[idx] = val_shallow_element - if idx == len(shallow_element) - 1: - shallow_element[idx].tail = last_tail - else: - shallow_element[idx].tail = first_tail - - -def remove_diff_to_ignore(osm_root): - decimal_precision = 11 # for lat/lon values - - # remove attributes of the osm tag - osm_root.attrib.clear() - - # remove the MetaInfo tag generated by Vector Map Builder - if osm_root[0].tag == "MetaInfo": - osm_root.remove(osm_root[0]) - - # remove unnecessary attributes for diff - for osm_child in osm_root: - if osm_child.tag == "osm": - osm_child.attrib.pop("osm") - if "visible" in osm_child.attrib and osm_child.attrib["visible"]: - osm_child.attrib.pop("visible") - if "version" in osm_child.attrib and osm_child.attrib["version"]: - osm_child.attrib.pop("version") - if "action" in osm_child.attrib and osm_child.attrib["action"] == "modify": - osm_child.attrib.pop("action") - if "lat" in osm_child.attrib: - osm_child.attrib["lat"] = str( - Decimal(float(osm_child.attrib["lat"])).quantize(Decimal(f"1e-{decimal_precision}")) - ) - if "lon" in osm_child.attrib: - osm_child.attrib["lon"] = str( - Decimal(float(osm_child.attrib["lon"])).quantize(Decimal(f"1e-{decimal_precision}")) - ) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument( - "-s", "--sort-attributes", action="store_true", help="Sort attributes of LL2 maps" - ) - parser.add_argument( - "-i", - "--ignore-minor-attributes", - action="store_true", - help="Ignore minor attributes of LL2 maps which does not change the map's behavior", - ) - args = parser.parse_args() - - original_osm_file_name = "/tmp/autoware_static_centerline_generator/input/lanelet2_map.osm" - modified_osm_file_name = "/tmp/autoware_static_centerline_generator/output/lanelet2_map.osm" - - # load LL2 maps - original_osm_tree = ET.parse(original_osm_file_name) - original_osm_root = original_osm_tree.getroot() - modified_osm_tree = ET.parse(modified_osm_file_name) - modified_osm_root = modified_osm_tree.getroot() - - # sort attributes - if args.sort_attributes: - sort_attributes(modified_osm_root) - sort_attributes(original_osm_root) - - # ignore minor attributes - if args.ignore_minor_attributes: - remove_diff_to_ignore(original_osm_root) - remove_diff_to_ignore(modified_osm_root) - - # write LL2 maps - output_dir_path = "/tmp/autoware_static_centerline_generator/show_lanelet2_map_diff/" - os.makedirs(output_dir_path + "original/", exist_ok=True) - os.makedirs(output_dir_path + "modified/", exist_ok=True) - - original_osm_tree.write(output_dir_path + "original/lanelet2_map.osm") - modified_osm_tree.write(output_dir_path + "modified/lanelet2_map.osm") - - # show diff - print("[INFO] Show diff of following LL2 maps") - print(f" {output_dir_path + 'original/lanelet2_map.osm'}") - print(f" {output_dir_path + 'modified/lanelet2_map.osm'}") - subprocess.run( - [ - "diff", - output_dir_path + "original/lanelet2_map.osm", - output_dir_path + "modified/lanelet2_map.osm", - "--color", - ] - ) diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh deleted file mode 100755 index c170e24e475d9..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh +++ /dev/null @@ -1,5 +0,0 @@ -#!/bin/bash - -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-right.db3" vehicle_model:=lexus - -# ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" mode:="GUI" lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory_turn-left-right.db3" vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh deleted file mode 100755 index 488091989d1fa..0000000000000 --- a/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp deleted file mode 100644 index dc950f190652b..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// 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 "centerline_source/bag_ego_trajectory_based_centerline.hpp" - -#include "rclcpp/serialization.hpp" -#include "rosbag2_cpp/reader.hpp" -#include "static_centerline_generator_node.hpp" - -#include - -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -std::vector generate_centerline_with_bag(rclcpp::Node & node) -{ - const auto bag_filename = node.declare_parameter("bag_filename"); - - // open rosbag - rosbag2_cpp::Reader bag_reader; - bag_reader.open(bag_filename); - - // extract 2D position of ego's trajectory from rosbag - rclcpp::Serialization bag_serialization; - std::vector centerline_traj_points; - while (bag_reader.has_next()) { - const rosbag2_storage::SerializedBagMessageSharedPtr msg = bag_reader.read_next(); - - if (msg->topic_name != "/localization/kinematic_state") { - continue; - } - - rclcpp::SerializedMessage serialized_msg(*msg->serialized_data); - const auto ros_msg = std::make_shared(); - - bag_serialization.deserialize_message(&serialized_msg, ros_msg.get()); - - if (!centerline_traj_points.empty()) { - constexpr double epsilon = 1e-1; - if ( - std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) < - epsilon && - std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) < - epsilon) { - continue; - } - } - TrajectoryPoint centerline_traj_point; - centerline_traj_point.pose.position = ros_msg->pose.pose.position; - centerline_traj_points.push_back(centerline_traj_point); - } - - RCLCPP_INFO(node.get_logger(), "Extracted centerline from the bag."); - - // calculate rough orientation of centerline trajectory points - for (size_t i = 0; i < centerline_traj_points.size(); ++i) { - if (i == centerline_traj_points.size() - 1) { - if (i != 0) { - centerline_traj_points.at(i).pose.orientation = - centerline_traj_points.at(i - 1).pose.orientation; - } - } else { - const double yaw_angle = autoware::universe_utils::calcAzimuthAngle( - centerline_traj_points.at(i).pose.position, centerline_traj_points.at(i + 1).pose.position); - centerline_traj_points.at(i).pose.orientation = - autoware::universe_utils::createQuaternionFromYaw(yaw_angle); - } - } - - return centerline_traj_points; -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp deleted file mode 100644 index 2275f88184c6f..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// 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 CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ -#define CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include - -namespace autoware::static_centerline_generator -{ -std::vector generate_centerline_with_bag(rclcpp::Node & node); -} // namespace autoware::static_centerline_generator -#endif // CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp deleted file mode 100644 index 51725fb8a3799..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ /dev/null @@ -1,187 +0,0 @@ -// 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 "centerline_source/optimization_trajectory_based_centerline.hpp" - -#include "autoware/motion_utils/resample/resample.hpp" -#include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/path_optimizer/node.hpp" -#include "autoware/path_smoother/elastic_band_smoother.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "static_centerline_generator_node.hpp" -#include "utils.hpp" - -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace -{ -rclcpp::NodeOptions create_node_options() -{ - return rclcpp::NodeOptions{}; -} - -Path convert_to_path(const PathWithLaneId & path_with_lane_id) -{ - Path path; - path.header = path_with_lane_id.header; - path.left_bound = path_with_lane_id.left_bound; - path.right_bound = path_with_lane_id.right_bound; - for (const auto & point : path_with_lane_id.points) { - path.points.push_back(point.point); - } - - return path; -} - -Trajectory convert_to_trajectory(const Path & path) -{ - Trajectory traj; - for (const auto & point : path.points) { - TrajectoryPoint traj_point; - traj_point.pose = point.pose; - traj_point.longitudinal_velocity_mps = point.longitudinal_velocity_mps; - traj_point.lateral_velocity_mps = point.lateral_velocity_mps; - traj_point.heading_rate_rps = point.heading_rate_rps; - - traj.points.push_back(traj_point); - } - return traj; -} -} // namespace - -OptimizationTrajectoryBasedCenterline::OptimizationTrajectoryBasedCenterline(rclcpp::Node & node) -{ - pub_raw_path_with_lane_id_ = - node.create_publisher("input_centerline", utils::create_transient_local_qos()); - pub_raw_path_ = - node.create_publisher("debug/raw_centerline", utils::create_transient_local_qos()); -} - -std::vector -OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( - rclcpp::Node & node, const RouteHandler & route_handler, - const std::vector & route_lane_ids) -{ - const auto route_lanelets = utils::get_lanelets_from_ids(route_handler, route_lane_ids); - - // optimize centerline inside the lane - const auto start_center_pose = utils::get_center_pose(route_handler, route_lane_ids.front()); - - // get ego nearest search parameters and resample interval in behavior_path_planner - const double ego_nearest_dist_threshold = - autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = - autoware::universe_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); - const double behavior_path_interval = - autoware::universe_utils::getOrDeclareParameter(node, "output_path_interval"); - const double behavior_vel_interval = - autoware::universe_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); - - // extract path with lane id from lanelets - const auto raw_path_with_lane_id = [&]() { - const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( - route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - return autoware::motion_utils::resamplePath( - non_resampled_path_with_lane_id, behavior_path_interval); - }(); - pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); - RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); - - // convert path with lane id to path - const auto raw_path = [&]() { - const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return autoware::motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); - }(); - pub_raw_path_->publish(raw_path); - RCLCPP_INFO(node.get_logger(), "Converted to path and published."); - - // smooth trajectory and road collision avoidance - const auto optimized_traj_points = optimize_trajectory(raw_path); - RCLCPP_INFO( - node.get_logger(), - "Smoothed trajectory and made it collision free with the road and published."); - - return optimized_traj_points; -} - -std::vector OptimizationTrajectoryBasedCenterline::optimize_trajectory( - const Path & raw_path) const -{ - // convert to trajectory points - const auto raw_traj_points = [&]() { - const auto raw_traj = convert_to_trajectory(raw_path); - return autoware::motion_utils::convertToTrajectoryPointArray(raw_traj); - }(); - - // create an instance of elastic band and model predictive trajectory. - const auto eb_path_smoother_ptr = - autoware::path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); - const auto mpt_optimizer_ptr = - autoware::path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); - - // NOTE: The optimization is executed every valid_optimized_traj_points_num points. - constexpr int valid_optimized_traj_points_num = 10; - const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; - - // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use - // warm start. - constexpr int num_initial_optimization = 2; - - std::vector whole_optimized_traj_points; - for (int virtual_ego_pose_idx = -num_initial_optimization; - virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { - // calculate virtual ego pose for the optimization - constexpr int virtual_ego_pose_offset_idx = 1; - const auto virtual_ego_pose = - raw_traj_points - .at( - valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + - virtual_ego_pose_offset_idx) - .pose; - - // smooth trajectory by elastic band in the autoware_path_smoother package - const auto smoothed_traj_points = - eb_path_smoother_ptr->smoothTrajectory(raw_traj_points, virtual_ego_pose); - - // road collision avoidance by model predictive trajectory in the autoware_path_optimizer - // package - const autoware::path_optimizer::PlannerData planner_data{ - raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, - virtual_ego_pose}; - const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); - - // connect the previously and currently optimized trajectory points - for (size_t j = 0; j < whole_optimized_traj_points.size(); ++j) { - const double dist = autoware::universe_utils::calcDistance2d( - whole_optimized_traj_points.at(j), optimized_traj_points.front()); - if (dist < 0.5) { - const std::vector extracted_whole_optimized_traj_points{ - whole_optimized_traj_points.begin(), - whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; - whole_optimized_traj_points = extracted_whole_optimized_traj_points; - break; - } - } - for (size_t j = 0; j < optimized_traj_points.size(); ++j) { - whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); - } - } - - return whole_optimized_traj_points; -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp deleted file mode 100644 index 88def6fa7bd4c..0000000000000 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp +++ /dev/null @@ -1,43 +0,0 @@ -// 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 CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -#define CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT - -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include - -namespace autoware::static_centerline_generator -{ -class OptimizationTrajectoryBasedCenterline -{ -public: - OptimizationTrajectoryBasedCenterline() = default; - explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); - std::vector generate_centerline_with_optimization( - rclcpp::Node & node, const RouteHandler & route_handler, - const std::vector & route_lane_ids); - -private: - std::vector optimize_trajectory(const Path & raw_path) const; - - rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; -}; -} // namespace autoware::static_centerline_generator -// clang-format off -#endif // CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -// clang-format on diff --git a/planning/autoware_static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp deleted file mode 100644 index 811d57c6036ef..0000000000000 --- a/planning/autoware_static_centerline_generator/src/main.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "static_centerline_generator_node.hpp" - -#include -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - // initialize node - rclcpp::NodeOptions node_options; - auto node = - std::make_shared( - node_options); - - // get ros parameter - const auto mode = node->declare_parameter("mode"); - - // process - if (mode == "AUTO") { - node->generate_centerline(); - node->validate(); - node->save_map(); - } else if (mode == "GUI") { - node->generate_centerline(); - } else if (mode == "VMB") { - // Do nothing - } else { - throw std::invalid_argument("The `mode` is invalid."); - } - - // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp deleted file mode 100644 index f172e3e0cb1cd..0000000000000 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ /dev/null @@ -1,774 +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 "static_centerline_generator_node.hpp" - -#include "autoware/map_loader/lanelet2_map_loader_node.hpp" -#include "autoware/map_projection_loader/load_info_from_lanelet2_map.hpp" -#include "autoware/map_projection_loader/map_projection_loader.hpp" -#include "autoware/motion_utils/resample/resample.hpp" -#include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/motion_utils/trajectory/trajectory.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/math/unit_conversion.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware_lanelet2_extension/utility/message_conversion.hpp" -#include "autoware_lanelet2_extension/utility/query.hpp" -#include "autoware_lanelet2_extension/utility/utilities.hpp" -#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" -#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "type_alias.hpp" -#include "utils.hpp" - -#include -#include -#include -#include - -#include "std_msgs/msg/empty.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" - -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define RESET_TEXT "\x1B[0m" -#define RED_TEXT "\x1B[31m" -#define YELLOW_TEXT "\x1b[33m" -#define BOLD_TEXT "\x1B[1m" - -namespace autoware::static_centerline_generator -{ -namespace -{ -std::vector get_lane_ids_from_route(const LaneletRoute & route) -{ - std::vector lane_ids; - for (const auto & segment : route.segments) { - const auto & target_lanelet_id = segment.preferred_primitive.id; - lane_ids.push_back(target_lanelet_id); - } - - return lane_ids; -} - -lanelet::BasicPoint2d convert_to_lanelet_point(const geometry_msgs::msg::Point & geom_point) -{ - lanelet::BasicPoint2d point(geom_point.x, geom_point.y); - return point; -} - -LinearRing2d create_vehicle_footprint( - const geometry_msgs::msg::Pose & pose, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) -{ - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m + margin; - const double x_rear = -(i.rear_overhang_m + margin); - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + margin; - const double y_right = -(i.wheel_tread_m / 2.0 + i.right_overhang_m + margin); - - std::vector geom_points; - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_front, y_right, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_rear, y_right, 0.0).position); - geom_points.push_back( - autoware::universe_utils::calcOffsetPose(pose, x_rear, y_left, 0.0).position); - - LinearRing2d footprint; - for (const auto & geom_point : geom_points) { - footprint.push_back(Point2d{geom_point.x, geom_point.y}); - } - footprint.push_back(footprint.back()); - - boost::geometry::correct(footprint); - - return footprint; -} - -geometry_msgs::msg::Pose get_text_pose( - const geometry_msgs::msg::Pose & pose, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) -{ - const auto & i = vehicle_info; - - const double x_front = i.front_overhang_m + i.wheel_base_m; - const double y_left = i.wheel_tread_m / 2.0 + i.left_overhang_m + 0.5; - - return autoware::universe_utils::calcOffsetPose(pose, x_front, y_left, 0.0); -} - -std::array convert_hex_string_to_decimal(const std::string & hex_str_color) -{ - unsigned int hex_int_color; - std::istringstream iss(hex_str_color); - iss >> std::hex >> hex_int_color; - - unsigned int unit = 16 * 16; - unsigned int b = hex_int_color % unit; - unsigned int g = (hex_int_color - b) / unit % unit; - unsigned int r = (hex_int_color - g * unit - b) / unit / unit; - - return std::array{r / 255.0, g / 255.0, b / 255.0}; -} - -std::vector check_lanelet_connection( - const RouteHandler & route_handler, const lanelet::ConstLanelets & route_lanelets) -{ - std::vector unconnected_lane_ids; - - for (size_t i = 0; i < route_lanelets.size() - 1; ++i) { - const auto next_lanelets = route_handler.getNextLanelets(route_lanelets.at(i)); - - const bool is_connected = - std::find_if(next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { - return next_lanelet.id() == route_lanelets.at(i + 1).id(); - }) != next_lanelets.end(); - if (!is_connected) { - unconnected_lane_ids.push_back(route_lanelets.at(i).id()); - } - } - - return unconnected_lane_ids; -} - -std_msgs::msg::Header create_header(const rclcpp::Time & now) -{ - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = now; - return header; -} - -std::vector resample_trajectory_points( - const std::vector & input_traj_points, const double resample_interval) -{ - // resample and calculate trajectory points' orientation - const auto input_traj = autoware::motion_utils::convertToTrajectory(input_traj_points); - auto resampled_input_traj = - autoware::motion_utils::resampleTrajectory(input_traj, resample_interval); - return autoware::motion_utils::convertToTrajectoryPointArray(resampled_input_traj); -} - -std::vector convertToGeometryPoints(const LineString2d & lanelet_points) -{ - std::vector points; - for (const auto & lanelet_point : lanelet_points) { - geometry_msgs::msg::Point point; - point.x = lanelet_point.x(); - point.y = lanelet_point.y(); - points.push_back(point); - } - return points; -} -} // namespace - -StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( - const rclcpp::NodeOptions & node_options) -: Node("static_centerline_generator", node_options) -{ - // publishers - pub_map_bin_ = - create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); - pub_whole_centerline_ = - create_publisher("~/output/whole_centerline", utils::create_transient_local_qos()); - pub_centerline_ = - create_publisher("~/output/centerline", utils::create_transient_local_qos()); - - // debug publishers - pub_validation_results_ = - create_publisher("~/validation_results", utils::create_transient_local_qos()); - pub_debug_markers_ = - create_publisher("~/debug/markers", utils::create_transient_local_qos()); - - pub_debug_ego_footprint_bounds_ = create_publisher( - "~/debug/ego_footprint_bounds", utils::create_transient_local_qos()); - - // subscribers - sub_footprint_margin_for_road_bound_ = create_subscription( - "/static_centerline_generator/road_boundary_lateral_margin", rclcpp::QoS{1}, - [this](const std_msgs::msg::Float32 & msg) { footprint_margin_for_road_bound_ = msg.data; }); - sub_traj_start_index_ = create_subscription( - "/static_centerline_generator/traj_start_index", rclcpp::QoS{1}, - [this](const std_msgs::msg::Int32 & msg) { - if (centerline_handler_.update_start_index(msg.data)) { - visualize_selected_centerline(); - } - }); - sub_traj_end_index_ = create_subscription( - "/static_centerline_generator/traj_end_index", rclcpp::QoS{1}, - [this](const std_msgs::msg::Int32 & msg) { - if (centerline_handler_.update_end_index(msg.data)) { - visualize_selected_centerline(); - } - }); - sub_save_map_ = create_subscription( - "/static_centerline_generator/save_map", rclcpp::QoS{1}, - [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { - if (!centerline_handler_.is_valid()) { - return; - } - save_map(); - }); - sub_validate_ = create_subscription( - "/static_centerline_generator/validate", rclcpp::QoS{1}, - [this]([[maybe_unused]] const std_msgs::msg::Empty & msg) { validate(); }); - - // services - callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - srv_load_map_ = create_service( - "/planning/static_centerline_generator/load_map", - std::bind( - &StaticCenterlineGeneratorNode::on_load_map, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - srv_plan_route_ = create_service( - "/planning/static_centerline_generator/plan_route", - std::bind( - &StaticCenterlineGeneratorNode::on_plan_route, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - srv_plan_path_ = create_service( - "/planning/static_centerline_generator/plan_path", - std::bind( - &StaticCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, - std::placeholders::_2), - rmw_qos_profile_services_default, callback_group_); - - // vehicle info - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); - - centerline_source_ = [&]() { - const auto centerline_source_param = declare_parameter("centerline_source"); - if (centerline_source_param == "optimization_trajectory_base") { - optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); - return CenterlineSource::OptimizationTrajectoryBase; - } else if (centerline_source_param == "bag_ego_trajectory_base") { - return CenterlineSource::BagEgoTrajectoryBase; - } - throw std::logic_error( - "The centerline source is not supported in autoware_static_centerline_generator."); - }(); -} - -void StaticCenterlineGeneratorNode::visualize_selected_centerline() -{ - // publish selected centerline - const auto selected_centerline = centerline_handler_.get_selected_centerline(); - pub_centerline_->publish( - autoware::motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); - - // delete markers for validation - pub_validation_results_->publish(utils::create_delete_all_marker_array({}, now())); - pub_debug_markers_->publish(utils::create_delete_all_marker_array( - {"unsafe_footprints", "unsafe_footprints_distance"}, now())); - pub_debug_ego_footprint_bounds_->publish( - utils::create_delete_all_marker_array({"road_bounds"}, now())); -} - -void StaticCenterlineGeneratorNode::generate_centerline() -{ - // declare planning setting parameters - const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); - - // process - load_map(lanelet2_input_file_path); - const auto whole_centerline_with_route = generate_whole_centerline_with_route(); - centerline_handler_ = CenterlineHandler(whole_centerline_with_route); - - visualize_selected_centerline(); -} - -CenterlineWithRoute StaticCenterlineGeneratorNode::generate_whole_centerline_with_route() -{ - if (!route_handler_ptr_) { - RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); - return CenterlineWithRoute{}; - } - - // generate centerline with route - auto centerline_with_route = [&]() { - if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { - const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); - const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); - const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); - const auto optimized_centerline = - optimization_trajectory_based_centerline_.generate_centerline_with_optimization( - *this, *route_handler_ptr_, route_lane_ids); - return CenterlineWithRoute{optimized_centerline, route_lane_ids}; - } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { - const auto bag_centerline = generate_centerline_with_bag(*this); - const auto route_lane_ids = - plan_route(bag_centerline.front().pose, bag_centerline.back().pose); - return CenterlineWithRoute{bag_centerline, route_lane_ids}; - } - throw std::logic_error( - "The centerline source is not supported in autoware_static_centerline_generator."); - }(); - - // resample - const double output_trajectory_interval = declare_parameter("output_trajectory_interval"); - centerline_with_route.centerline = - resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); - - pub_whole_centerline_->publish(autoware::motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); - - return centerline_with_route; -} - -void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) -{ - // copy the input LL2 map to the temporary file for debugging - const std::string debug_input_file_dir{"/tmp/autoware_static_centerline_generator/input/"}; - std::filesystem::create_directories(debug_input_file_dir); - std::filesystem::copy( - lanelet2_input_file_path, debug_input_file_dir + "lanelet2_map.osm", - std::filesystem::copy_options::overwrite_existing); - - // load map by the map_loader package - map_bin_ptr_ = [&]() -> LaneletMapBin::ConstSharedPtr { - // load map - map_projector_info_ = std::make_unique( - autoware::map_projection_loader::load_info_from_lanelet2_map(lanelet2_input_file_path)); - const auto map_ptr = autoware::map_loader::Lanelet2MapLoaderNode::load_map( - lanelet2_input_file_path, *map_projector_info_); - if (!map_ptr) { - return nullptr; - } - - // NOTE: The original map is stored here since the centerline will be added to all the - // lanelet when lanelet::utils::overwriteLaneletCenterline is called. - original_map_ptr_ = autoware::map_loader::Lanelet2MapLoaderNode::load_map( - lanelet2_input_file_path, *map_projector_info_); - - // overwrite more dense centerline - // NOTE: overwriteLaneletsCenterlineWithWaypoints is used only in real time calculation. - lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); - - // create map bin msg - const auto map_bin_msg = autoware::map_loader::Lanelet2MapLoaderNode::create_map_bin_msg( - map_ptr, lanelet2_input_file_path, now()); - - return std::make_shared(map_bin_msg); - }(); - - // check if map_bin_ptr_ is not null pointer - if (!map_bin_ptr_) { - RCLCPP_ERROR(get_logger(), "Loading map failed"); - return; - } - RCLCPP_INFO(get_logger(), "Loaded map."); - - // publish map bin msg - pub_map_bin_->publish(*map_bin_ptr_); - RCLCPP_INFO(get_logger(), "Published map."); - - // create route_handler - route_handler_ptr_ = std::make_shared(); - route_handler_ptr_->setMap(*map_bin_ptr_); -} - -void StaticCenterlineGeneratorNode::on_load_map( - const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response) -{ - const std::string tmp_lanelet2_input_file_path = "/tmp/input_lanelet2_map.osm"; - - // save map file temporarily since load map's input must be a file - std::ofstream map_writer; - map_writer.open(tmp_lanelet2_input_file_path, std::ios::out); - map_writer << request->map; - map_writer.close(); - - // load map from the saved map file - load_map(tmp_lanelet2_input_file_path); - - if (map_bin_ptr_) { - return; - } - - response->message = "InvalidMapFormat"; -} - -std::vector StaticCenterlineGeneratorNode::plan_route_by_lane_ids( - const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) -{ - if (!route_handler_ptr_) { - RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); - return std::vector{}; - } - - const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); - const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); - return plan_route(start_center_pose, end_center_pose); -} - -std::vector StaticCenterlineGeneratorNode::plan_route( - const geometry_msgs::msg::Pose & start_center_pose, - const geometry_msgs::msg::Pose & end_center_pose) -{ - if (!map_bin_ptr_) { - RCLCPP_ERROR(get_logger(), "Map or route handler is not ready. Return empty lane ids."); - return std::vector{}; - } - - // plan route by the mission_planner package - const auto route = [&]() { - // calculate check points - RCLCPP_INFO(get_logger(), "Calculated check points."); - const auto check_points = - std::vector{start_center_pose, end_center_pose}; - - // create mission_planner plugin - auto plugin_loader = pluginlib::ClassLoader( - "autoware_mission_planner", "autoware::mission_planner::PlannerPlugin"); - auto mission_planner = - plugin_loader.createSharedInstance("autoware::mission_planner::lanelet2::DefaultPlanner"); - - // initialize mission_planner - auto node = rclcpp::Node("mission_planner"); - mission_planner->initialize(&node, map_bin_ptr_); - - // plan route - const auto route = mission_planner->plan(check_points); - - return route; - }(); - - // get lanelets - const auto route_lane_ids = get_lane_ids_from_route(route); - - std::string route_lane_ids_str = ""; - for (const lanelet::Id route_lane_id : route_lane_ids) { - route_lane_ids_str += std::to_string(route_lane_id) + ","; - } - RCLCPP_INFO_STREAM(get_logger(), "Planned route. (" << route_lane_ids_str << ")"); - - return route_lane_ids; -} - -void StaticCenterlineGeneratorNode::on_plan_route( - const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response) -{ - if (!map_bin_ptr_ || !route_handler_ptr_) { - response->message = "MapNotFound"; - RCLCPP_ERROR(get_logger(), "Map is not ready."); - return; - } - - const lanelet::Id start_lanelet_id = request->start_lane_id; - const lanelet::Id end_lanelet_id = request->end_lane_id; - - // plan route - const auto route_lane_ids = plan_route_by_lane_ids(start_lanelet_id, end_lanelet_id); - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // extract lane ids - std::vector lane_ids; - for (const auto & lanelet : route_lanelets) { - lane_ids.push_back(lanelet.id()); - } - - // check calculation result - if (lane_ids.empty()) { - response->message = "RouteNotFound"; - RCLCPP_ERROR(get_logger(), "Route planning failed."); - return; - } - - // set response - response->lane_ids = lane_ids; -} - -void StaticCenterlineGeneratorNode::on_plan_path( - const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) -{ - if (!route_handler_ptr_) { - response->message = "MapNotFound"; - RCLCPP_ERROR(get_logger(), "Route handler is not ready."); - return; - } - - // get lanelets from route lane ids - const auto route_lane_ids = request->route; - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // check if input route lanelets are connected to each other. - const auto unconnected_lane_ids = check_lanelet_connection(*route_handler_ptr_, route_lanelets); - if (!unconnected_lane_ids.empty()) { - response->message = "LaneletsNotConnected"; - response->unconnected_lane_ids = unconnected_lane_ids; - RCLCPP_ERROR(get_logger(), "Lanelets are not connected."); - return; - } - - // plan path - const auto optimized_traj_points = - optimization_trajectory_based_centerline_.generate_centerline_with_optimization( - *this, *route_handler_ptr_, route_lane_ids); - - // check calculation result - if (optimized_traj_points.empty()) { - response->message = "PathNotFound"; - RCLCPP_ERROR(get_logger(), "Path planning failed."); - return; - } - - centerline_handler_ = - CenterlineHandler(CenterlineWithRoute{optimized_traj_points, route_lane_ids}); - - // publish unsafe_footprints - validate(); - - // create output data - auto target_traj_point = optimized_traj_points.cbegin(); - bool is_end_lanelet = false; - for (const auto & lanelet : route_lanelets) { - std::vector current_lanelet_points; - - // check if target point is inside the lanelet - while (lanelet::geometry::inside( - lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) { - // memorize points inside the lanelet - current_lanelet_points.push_back(target_traj_point->pose.position); - target_traj_point++; - - if (target_traj_point == optimized_traj_points.cend()) { - is_end_lanelet = true; - break; - } - } - - if (!current_lanelet_points.empty()) { - // register points with lane_id - autoware_static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; - points_with_lane_id.lane_id = lanelet.id(); - points_with_lane_id.points = current_lanelet_points; - response->points_with_lane_ids.push_back(points_with_lane_id); - } - - if (is_end_lanelet) { - break; - } - } - - // empty string if error did not occur - response->message = ""; -} - -void StaticCenterlineGeneratorNode::validate() -{ - std::cerr << std::endl - << "############################################## Validation Results " - "##############################################" - << std::endl; - - const auto & centerline = centerline_handler_.get_selected_centerline(); - const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - const double dist_thresh_to_road_border = - getRosParameter("validation.dist_threshold_to_road_border"); - const double max_steer_angle_margin = - getRosParameter("validation.max_steer_angle_margin"); - - // calculate color for distance to road border - const auto dist_thresh_vec = getRosParameter>("marker_color_dist_thresh"); - const auto marker_color_vec = getRosParameter>("marker_color"); - const auto get_marker_color = [&](const double dist) -> boost::optional> { - for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { - const double dist_thresh = dist_thresh_vec.at(i); - if (dist < dist_thresh) { - return convert_hex_string_to_decimal(marker_color_vec.at(i)); - } - } - return boost::none; - }; - - // create right/left bound - LineString2d lanelet_right_bound; - LineString2d lanelet_left_bound; - for (const auto & lanelet : route_lanelets) { - for (const auto & point : lanelet.rightBound()) { - boost::geometry::append(lanelet_right_bound, Point2d(point.x(), point.y())); - } - for (const auto & point : lanelet.leftBound()) { - boost::geometry::append(lanelet_left_bound, Point2d(point.x(), point.y())); - } - } - - // calculate curvature - const auto curvature_vec = autoware::motion_utils::calcCurvature(centerline); - const double steer_angle_threshold = vehicle_info_.max_steer_angle_rad - max_steer_angle_margin; - - // calculate the distance between footprint and right/left bounds - MarkerArray marker_array; - double min_dist = std::numeric_limits::max(); - double max_curvature = std::numeric_limits::min(); - for (size_t i = 0; i < centerline.size(); ++i) { - const auto & traj_point = centerline.at(i); - - const auto footprint_poly = create_vehicle_footprint(traj_point.pose, vehicle_info_); - - const double dist_to_right = boost::geometry::distance(footprint_poly, lanelet_right_bound); - const double dist_to_left = boost::geometry::distance(footprint_poly, lanelet_left_bound); - const double min_dist_to_bound = std::min(dist_to_right, dist_to_left); - - if (min_dist_to_bound < min_dist) { - min_dist = min_dist_to_bound; - } - - // create marker - const auto marker_color_opt = get_marker_color(min_dist_to_bound); - const auto text_pose = get_text_pose(traj_point.pose, vehicle_info_); - if (marker_color_opt) { - const auto & marker_color = marker_color_opt.get(); - - // add footprint marker - const auto footprint_marker = utils::create_footprint_marker( - footprint_poly, 0.05, marker_color.at(0), marker_color.at(1), marker_color.at(2), 0.7, - now(), i); - marker_array.markers.push_back(footprint_marker); - - // add text of distance to bounds marker - const auto text_marker = utils::create_text_marker( - "unsafe_footprints_distance", text_pose, min_dist_to_bound, marker_color.at(0), - marker_color.at(1), marker_color.at(2), 0.999, now(), i); - marker_array.markers.push_back(text_marker); - } - - const double curvature = curvature_vec.at(i); - const auto text_marker = - utils::create_text_marker("curvature", text_pose, curvature, 0.05, 0.05, 0.0, 0.9, now(), i); - marker_array.markers.push_back(text_marker); - - if (max_curvature < std::abs(curvature)) { - max_curvature = std::abs(curvature); - } - } - const double max_steer_angle = vehicle_info_.calcSteerAngleFromCurvature(max_curvature); - - // publish road boundaries - const auto left_bound = convertToGeometryPoints(lanelet_left_bound); - const auto right_bound = convertToGeometryPoints(lanelet_right_bound); - - marker_array.markers.push_back( - utils::create_points_marker("left_bound", left_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); - marker_array.markers.push_back( - utils::create_points_marker("right_bound", right_bound, 0.05, 0.0, 0.6, 0.8, 0.8, now())); - pub_debug_markers_->publish(marker_array); - - // show the validation results - // 1. distance from footprints to road boundaries - const bool are_footprints_inside_lanelets = [&]() { - std::cerr << "1. Footprints inside Lanelets:" << std::endl; - if (dist_thresh_to_road_border < min_dist) { - std::cerr << " The generated centerline is inside the lanelet. (threshold:" - << dist_thresh_to_road_border << "[m] < actual:" << min_dist << "[m])" << std::endl - << " Passed." << std::endl; - return true; - } - std::cerr << RED_TEXT - << " The generated centerline is outside the lanelet. (actual:" << min_dist - << "[m] <= threshold:" << dist_thresh_to_road_border << "[m])" << std::endl - << " Failed." << RESET_TEXT << std::endl; - return false; - }(); - // 2. centerline's curvature - std::cerr << "2. Curvature:" << std::endl; - const bool is_curvature_low = - true; // always tre for now since the curvature is just estimated and not enough precise. - if (max_steer_angle < steer_angle_threshold) { - std::cerr << " The generated centerline has no high steer angle. (estimated:" - << autoware::universe_utils::rad2deg(max_steer_angle) - << "[deg] < threshold:" << autoware::universe_utils::rad2deg(steer_angle_threshold) - << "[deg])" << std::endl - << " Passed." << std::endl; - } else { - std::cerr << YELLOW_TEXT << " The generated centerline has a too high steer angle. (threshold:" - << autoware::universe_utils::rad2deg(steer_angle_threshold) - << "[deg] <= estimated:" << autoware::universe_utils::rad2deg(max_steer_angle) - << "[deg])" << std::endl - << " However, the estimated steer angle is not enough precise, so the result is " - "conditional pass." - << std::endl - << " Conditionally Passed." << RESET_TEXT << std::endl; - } - // 3. result - std::cerr << std::endl << BOLD_TEXT << "Result:" << RESET_TEXT << std::endl; - if (are_footprints_inside_lanelets && is_curvature_low) { - std::cerr << BOLD_TEXT << " Passed!" << RESET_TEXT << std::endl; - } else { - std::cerr << BOLD_TEXT << RED_TEXT << " Failed!" << RESET_TEXT << std::endl; - } - - std::cerr << "###################################################################################" - "#############################" - << std::endl - << std::endl; - RCLCPP_INFO(get_logger(), "Validated the generated centerline."); -} - -void StaticCenterlineGeneratorNode::save_map() -{ - if (!route_handler_ptr_) { - return; - } - - const auto & centerline = centerline_handler_.get_selected_centerline(); - const auto & route_lane_ids = centerline_handler_.get_route_lane_ids(); - - const auto lanelet2_output_file_path = getRosParameter("lanelet2_output_file_path"); - - const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, route_lane_ids); - - // update centerline in map - utils::update_centerline(original_map_ptr_, route_lanelets, centerline); - RCLCPP_INFO(get_logger(), "Updated centerline in map."); - - // save map with modified center line - std::filesystem::create_directory("/tmp/autoware_static_centerline_generator"); - const auto map_projector = - autoware::geography_utils::get_lanelet2_projector(*map_projector_info_); - lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); - RCLCPP_INFO( - get_logger(), "Saved map in %s", "/tmp/autoware_static_centerline_generator/lanelet2_map.osm"); - - // copy the output LL2 map to the temporary file for debugging - const std::string debug_output_file_dir{"/tmp/autoware_static_centerline_generator/output/"}; - std::filesystem::create_directories(debug_output_file_dir); - std::filesystem::copy( - lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", - std::filesystem::copy_options::overwrite_existing); -} -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp deleted file mode 100644 index c591dcfc73bd8..0000000000000 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ /dev/null @@ -1,187 +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 STATIC_CENTERLINE_GENERATOR_NODE_HPP_ -#define STATIC_CENTERLINE_GENERATOR_NODE_HPP_ - -#include "autoware/universe_utils/ros/parameter.hpp" -#include "autoware_static_centerline_generator/srv/load_map.hpp" -#include "autoware_static_centerline_generator/srv/plan_path.hpp" -#include "autoware_static_centerline_generator/srv/plan_route.hpp" -#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" -#include "centerline_source/optimization_trajectory_based_centerline.hpp" -#include "rclcpp/rclcpp.hpp" -#include "type_alias.hpp" - -#include "autoware_map_msgs/msg/map_projector_info.hpp" -#include "std_msgs/msg/empty.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" - -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -using autoware_map_msgs::msg::MapProjectorInfo; -using autoware_static_centerline_generator::srv::LoadMap; -using autoware_static_centerline_generator::srv::PlanPath; -using autoware_static_centerline_generator::srv::PlanRoute; - -struct CenterlineWithRoute -{ - std::vector centerline{}; - std::vector route_lane_ids{}; -}; -struct CenterlineHandler -{ - CenterlineHandler() = default; - explicit CenterlineHandler(const CenterlineWithRoute & centerline_with_route) - : whole_centerline_with_route(centerline_with_route), - start_index(0), - end_index(centerline_with_route.centerline.size() - 1) - { - } - std::vector get_selected_centerline() const - { - if (!whole_centerline_with_route) { - return std::vector{}; - } - const auto & centerline_begin = whole_centerline_with_route->centerline.begin(); - return std::vector( - centerline_begin + start_index, centerline_begin + end_index + 1); - } - std::vector get_route_lane_ids() const - { - if (!whole_centerline_with_route) { - return std::vector{}; - } - return whole_centerline_with_route->route_lane_ids; - } - bool is_valid() const { return whole_centerline_with_route && start_index < end_index; } - bool update_start_index(const int arg_start_index) - { - if (whole_centerline_with_route && arg_start_index < end_index) { - start_index = arg_start_index; - return true; - } - return false; - } - bool update_end_index(const int arg_end_index) - { - if (whole_centerline_with_route && start_index < arg_end_index) { - end_index = arg_end_index; - return true; - } - return false; - } - - std::optional whole_centerline_with_route{std::nullopt}; - int start_index{}; - int end_index{}; -}; - -struct RoadBounds -{ - std::vector left_bound; - std::vector right_bound; -}; - -class StaticCenterlineGeneratorNode : public rclcpp::Node -{ -public: - explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); - void generate_centerline(); - void validate(); - void save_map(); - -private: - // load map - void load_map(const std::string & lanelet2_input_file_path); - void on_load_map( - const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response); - - // plan route - std::vector plan_route_by_lane_ids( - const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id); - std::vector plan_route( - const geometry_msgs::msg::Pose & start_center_pose, - const geometry_msgs::msg::Pose & end_center_pose); - - void on_plan_route( - const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); - - // plan centerline - CenterlineWithRoute generate_whole_centerline_with_route(); - std::vector get_route_lane_ids_from_points( - const std::vector & points); - void on_plan_path( - const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); - - void visualize_selected_centerline(); - - // parameter - template - T getRosParameter(const std::string & param_name) - { - return autoware::universe_utils::getOrDeclareParameter(*this, param_name); - } - - lanelet::LaneletMapPtr original_map_ptr_{nullptr}; - LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; - std::shared_ptr route_handler_ptr_{nullptr}; - std::unique_ptr map_projector_info_{nullptr}; - - CenterlineHandler centerline_handler_; - - float footprint_margin_for_road_bound_{0.0}; - - enum class CenterlineSource { - OptimizationTrajectoryBase = 0, - BagEgoTrajectoryBase, - }; - CenterlineSource centerline_source_; - OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; - - // publisher - rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; - rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; - rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; - rclcpp::Publisher::SharedPtr pub_validation_results_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_ego_footprint_bounds_{nullptr}; - rclcpp::Publisher::SharedPtr pub_debug_markers_{nullptr}; - - // subscriber - rclcpp::Subscription::SharedPtr sub_traj_start_index_; - rclcpp::Subscription::SharedPtr sub_traj_end_index_; - rclcpp::Subscription::SharedPtr sub_save_map_; - rclcpp::Subscription::SharedPtr sub_validate_; - rclcpp::Subscription::SharedPtr sub_traj_resample_interval_; - rclcpp::Subscription::SharedPtr sub_footprint_margin_for_road_bound_; - - // service - rclcpp::Service::SharedPtr srv_load_map_; - rclcpp::Service::SharedPtr srv_plan_route_; - rclcpp::Service::SharedPtr srv_plan_path_; - - // callback group for service - rclcpp::CallbackGroup::SharedPtr callback_group_; - - // vehicle info - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; -}; -} // namespace autoware::static_centerline_generator -#endif // STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp deleted file mode 100644 index 2b7b99bfe81be..0000000000000 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ /dev/null @@ -1,47 +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 TYPE_ALIAS_HPP_ -#define TYPE_ALIAS_HPP_ - -#include "autoware/route_handler/route_handler.hpp" -#include "autoware/universe_utils/geometry/geometry.hpp" - -#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" -#include "autoware_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_planning_msgs/msg/lanelet_route.hpp" -#include "autoware_planning_msgs/msg/path.hpp" -#include "autoware_planning_msgs/msg/path_point.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_planning_msgs/msg/trajectory_point.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - -namespace autoware::static_centerline_generator -{ -using autoware::route_handler::RouteHandler; -using autoware::universe_utils::LinearRing2d; -using autoware::universe_utils::LineString2d; -using autoware::universe_utils::Point2d; -using autoware_map_msgs::msg::LaneletMapBin; -using autoware_perception_msgs::msg::PredictedObjects; -using autoware_planning_msgs::msg::LaneletRoute; -using autoware_planning_msgs::msg::Path; -using autoware_planning_msgs::msg::PathPoint; -using autoware_planning_msgs::msg::Trajectory; -using autoware_planning_msgs::msg::TrajectoryPoint; -using tier4_planning_msgs::msg::PathWithLaneId; -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; -} // namespace autoware::static_centerline_generator - -#endif // TYPE_ALIAS_HPP_ diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp deleted file mode 100644 index f84fe79cec288..0000000000000 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ /dev/null @@ -1,263 +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 "utils.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/universe_utils/geometry/geometry.hpp" -#include "autoware/universe_utils/ros/marker_helper.hpp" - -#include -#include - -#include -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace -{ -nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs::msg::Pose & pose) -{ - auto odometry_ptr = std::make_shared(); - odometry_ptr->pose.pose = pose; - return odometry_ptr; -} - -lanelet::Point3d createPoint3d(const double x, const double y, const double z) -{ - lanelet::Point3d point(lanelet::utils::getId()); - - // position - point.x() = x; - point.y() = y; - point.z() = z; - - // attributes - point.setAttribute("local_x", x); - point.setAttribute("local_y", y); - // NOTE: It seems that the attribute "ele" is assigned automatically. - - return point; -} -} // namespace - -namespace utils -{ -rclcpp::QoS create_transient_local_qos() -{ - return rclcpp::QoS{1}.transient_local(); -} - -lanelet::ConstLanelets get_lanelets_from_ids( - const RouteHandler & route_handler, const std::vector & lane_ids) -{ - lanelet::ConstLanelets lanelets; - for (const lanelet::Id lane_id : lane_ids) { - const auto lanelet = route_handler.getLaneletsFromId(lane_id); - lanelets.push_back(lanelet); - } - return lanelets; -} - -geometry_msgs::msg::Pose get_center_pose( - const RouteHandler & route_handler, const size_t lanelet_id) -{ - // get middle idx of the lanelet - const auto lanelet = route_handler.getLaneletsFromId(lanelet_id); - const auto center_line = lanelet.centerline(); - const size_t middle_point_idx = std::floor(center_line.size() / 2.0); - - // get middle position of the lanelet - geometry_msgs::msg::Point middle_pos; - middle_pos.x = center_line[middle_point_idx].x(); - middle_pos.y = center_line[middle_point_idx].y(); - middle_pos.z = center_line[middle_point_idx].z(); - - // get next middle position of the lanelet - geometry_msgs::msg::Point next_middle_pos; - next_middle_pos.x = center_line[middle_point_idx + 1].x(); - next_middle_pos.y = center_line[middle_point_idx + 1].y(); - next_middle_pos.z = center_line[middle_point_idx + 1].z(); - - // calculate middle pose - geometry_msgs::msg::Pose middle_pose; - middle_pose.position = middle_pos; - const double yaw = autoware::universe_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw); - - return middle_pose; -} - -PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, - const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, - const double ego_nearest_yaw_threshold) -{ - // get center line - constexpr double s_start = 0.0; - constexpr double s_end = std::numeric_limits::max(); - auto path_with_lane_id = route_handler.getCenterLinePath(lanelets, s_start, s_end); - path_with_lane_id.header.frame_id = "map"; - - // create planner data - auto planner_data = std::make_shared(); - planner_data->route_handler = std::make_shared(route_handler); - planner_data->self_odometry = convert_to_odometry(start_pose); - planner_data->parameters.ego_nearest_dist_threshold = ego_nearest_dist_threshold; - planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; - - // generate drivable area and store it in path with lane id - constexpr double vehicle_length = 0.0; - const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); - behavior_path_planner::utils::generateDrivableArea( - path_with_lane_id, drivable_lanes, false, false, vehicle_length, planner_data); - - return path_with_lane_id; -} - -void update_centerline( - lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, - const std::vector & new_centerline) -{ - // get lanelet as reference to update centerline - lanelet::Lanelets lanelets_ref; - for (const auto & lanelet : lanelets) { - for (auto & lanelet_ref : lanelet_map_ptr->laneletLayer) { - if (lanelet_ref.id() == lanelet.id()) { - lanelets_ref.push_back(lanelet_ref); - } - } - } - - // store new centerline in lanelets - size_t lanelet_idx = 0; - lanelet::LineString3d centerline(lanelet::utils::getId()); - for (size_t traj_idx = 0; traj_idx < new_centerline.size(); ++traj_idx) { - const auto & traj_pos = new_centerline.at(traj_idx).pose.position; - - for (; lanelet_idx < lanelets_ref.size(); ++lanelet_idx) { - auto & lanelet_ref = lanelets_ref.at(lanelet_idx); - - const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y); - // TODO(murooka) This does not work with L-crank map. - const bool is_inside = lanelet::geometry::inside(lanelet_ref, point); - if (is_inside) { - const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z); - - // set center point - centerline.push_back(center_point); - lanelet_map_ptr->add(center_point); - break; - } - - if (!centerline.empty()) { - // set centerline - lanelet_map_ptr->add(centerline); - lanelet_ref.setCenterline(centerline); - - // prepare new centerline - centerline = lanelet::LineString3d(lanelet::utils::getId()); - } - } - - if (traj_idx == new_centerline.size() - 1 && !centerline.empty()) { - auto & lanelet_ref = lanelets_ref.at(lanelet_idx); - - // set centerline - lanelet_map_ptr->add(centerline); - lanelet_ref.setCenterline(centerline); - } - } -} - -Marker create_footprint_marker( - const LinearRing2d & footprint_poly, const double width, const double r, const double g, - const double b, const double a, const rclcpp::Time & now, const size_t idx) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints", idx, - visualization_msgs::msg::Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.header.stamp = now; - // TODO(murooka) Ideally, the following is unnecessary for the topic of transient local. - marker.lifetime = rclcpp::Duration(0, 0); - - for (const auto & point : footprint_poly) { - geometry_msgs::msg::Point geom_point; - geom_point.x = point.x(); - geom_point.y = point.y(); - // geom_point.z = point.z(); - - marker.points.push_back(geom_point); - } - marker.points.push_back(marker.points.front()); - return marker; -} - -Marker create_text_marker( - const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, - const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), ns, idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, - autoware::universe_utils::createMarkerScale(0.5, 0.5, 0.5), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.pose = pose; - marker.header.stamp = now; - marker.lifetime = rclcpp::Duration(0, 0); - - std::stringstream ss; - ss << std::setprecision(2) << value; - marker.text = ss.str(); - - return marker; -} - -Marker create_points_marker( - const std::string & ns, const std::vector & points, const double width, - const double r, const double g, const double b, const double a, const rclcpp::Time & now) -{ - auto marker = autoware::universe_utils::createDefaultMarker( - "map", now, ns, 1, Marker::LINE_STRIP, - autoware::universe_utils::createMarkerScale(width, 0.0, 0.0), - autoware::universe_utils::createMarkerColor(r, g, b, a)); - marker.lifetime = rclcpp::Duration(0, 0); - marker.points = points; - return marker; -} - -MarkerArray create_delete_all_marker_array( - const std::vector & ns_vec, const rclcpp::Time & now) -{ - Marker marker; - marker.header.stamp = now; - marker.action = visualization_msgs::msg::Marker::DELETEALL; - - MarkerArray marker_array; - for (const auto & ns : ns_vec) { - marker.ns = ns; - marker_array.markers.push_back(marker); - } - - return marker_array; -} - -} // namespace utils -} // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp deleted file mode 100644 index f4d15d3dcfd4f..0000000000000 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ /dev/null @@ -1,67 +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 UTILS_HPP_ -#define UTILS_HPP_ - -#include "autoware/route_handler/route_handler.hpp" -#include "type_alias.hpp" - -#include - -#include -#include -#include -#include - -namespace autoware::static_centerline_generator -{ -namespace utils -{ -rclcpp::QoS create_transient_local_qos(); - -lanelet::ConstLanelets get_lanelets_from_ids( - const RouteHandler & route_handler, const std::vector & lane_ids); - -geometry_msgs::msg::Pose get_center_pose( - const RouteHandler & route_handler, const size_t lanelet_id); - -PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, - const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, - const double ego_nearest_yaw_threshold); - -void update_centerline( - lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, - const std::vector & new_centerline); - -Marker create_footprint_marker( - const LinearRing2d & footprint_poly, const double width, const double r, const double g, - const double b, const double a, const rclcpp::Time & now, const size_t idx); - -Marker create_text_marker( - const std::string & ns, const geometry_msgs::msg::Pose & pose, const double value, const double r, - const double g, const double b, const double a, const rclcpp::Time & now, const size_t idx); - -Marker create_points_marker( - const std::string & ns, const std::vector & points, const double width, - const double r, const double g, const double b, const double a, const rclcpp::Time & now); - -MarkerArray create_delete_all_marker_array( - const std::vector & ns_vec, const rclcpp::Time & now); - -} // namespace utils -} // namespace autoware::static_centerline_generator - -#endif // UTILS_HPP_ diff --git a/planning/autoware_static_centerline_generator/srv/LoadMap.srv b/planning/autoware_static_centerline_generator/srv/LoadMap.srv deleted file mode 100644 index 02142e60c0035..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/LoadMap.srv +++ /dev/null @@ -1,3 +0,0 @@ -string map ---- -string message diff --git a/planning/autoware_static_centerline_generator/srv/PlanPath.srv b/planning/autoware_static_centerline_generator/srv/PlanPath.srv deleted file mode 100644 index 3a8d0321ffb9a..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/PlanPath.srv +++ /dev/null @@ -1,6 +0,0 @@ -uint32 map_id -int64[] route ---- -autoware_static_centerline_generator/PointsWithLaneId[] points_with_lane_ids -string message -int64[] unconnected_lane_ids diff --git a/planning/autoware_static_centerline_generator/srv/PlanRoute.srv b/planning/autoware_static_centerline_generator/srv/PlanRoute.srv deleted file mode 100644 index fb50c04b0ff26..0000000000000 --- a/planning/autoware_static_centerline_generator/srv/PlanRoute.srv +++ /dev/null @@ -1,5 +0,0 @@ -int64 start_lane_id -int64 end_lane_id ---- -int64[] lane_ids -string message diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 deleted file mode 100644 index ed3448772b794..0000000000000 Binary files a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-left-right.db3 and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 deleted file mode 100644 index 0883307acbae0..0000000000000 Binary files a/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory_turn-right.db3 and /dev/null differ diff --git a/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm b/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm deleted file mode 100644 index 406cd85c151ea..0000000000000 --- a/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py deleted file mode 100644 index ca2621212ef83..0000000000000 --- a/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 TIER IV, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import unittest - -from ament_index_python import get_package_share_directory -import launch -from launch import LaunchDescription -from launch_ros.actions import Node -import launch_testing -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - lanelet2_map_path = os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "test/data/lanelet2_map.osm", - ) - - static_centerline_generator_node = Node( - package="autoware_static_centerline_generator", - executable="main", - output="screen", - parameters=[ - {"lanelet2_map_path": lanelet2_map_path}, - {"mode": "AUTO"}, - {"rviz": False}, - {"centerline_source": "optimization_trajectory_base"}, - {"lanelet2_input_file_path": lanelet2_map_path}, - {"lanelet2_output_file_path": "/tmp/lanelet2_map.osm"}, - {"start_lanelet_id": 215}, - {"end_lanelet_id": 216}, - os.path.join( - get_package_share_directory("autoware_mission_planner"), - "config", - "mission_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/static_centerline_generator.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_behavior_path_planner"), - "config/behavior_path_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_behavior_velocity_planner"), - "config/behavior_velocity_planner.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_path_smoother"), - "config/elastic_band_smoother.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_path_optimizer"), - "config/path_optimizer.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_map_loader"), - "config/lanelet2_map_loader.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/common.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/nearest_search.param.yaml", - ), - os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "config/vehicle_info.param.yaml", - ), - ], - ) - - context = {} - - return ( - LaunchDescription( - [ - static_centerline_generator_node, - # Start test after 1s - gives time for the autoware_static_centerline_generator to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/planning/autoware_surround_obstacle_checker/CHANGELOG.rst b/planning/autoware_surround_obstacle_checker/CHANGELOG.rst index c6e1a13a59aa2..2a497056dc480 100644 --- a/planning/autoware_surround_obstacle_checker/CHANGELOG.rst +++ b/planning/autoware_surround_obstacle_checker/CHANGELOG.rst @@ -2,6 +2,41 @@ Changelog for package autoware_surround_obstacle_checker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(autoware_surround_obstacle_checker): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_surround_obstacle_checker (`#9915 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files planning/autoware_surround_obstacle_checker +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 4a6f946b8eb90..e5ee999aa24d8 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -2,7 +2,7 @@ autoware_surround_obstacle_checker - 0.40.0 + 0.41.0 The autoware_surround_obstacle_checker package Satoshi Ota Go Sakayori @@ -14,9 +14,9 @@ autoware_cmake eigen3_cmake_module - autoware_adapi_v1_msgs autoware_motion_utils autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager autoware_universe_utils diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index bbd1cac04ed89..e34315ad986a5 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -67,7 +67,10 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, const rclcpp::Clock::SharedPtr clock, rclcpp::Node & node) -: vehicle_info_(vehicle_info), +: planning_factor_interface_{std::make_unique< + autoware::planning_factor_interface::PlanningFactorInterface>( + &node, "surround_obstacle_checker")}, + vehicle_info_(vehicle_info), object_label_(object_label), surround_check_front_distance_(surround_check_front_distance), surround_check_side_distance_(surround_check_side_distance), @@ -77,8 +80,6 @@ SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( clock_(clock) { debug_viz_pub_ = node.create_publisher("~/debug/marker", 1); - velocity_factor_pub_ = - node.create_publisher("/planning/velocity_factors/surround_obstacle", 1); vehicle_footprint_pub_ = node.create_publisher("~/debug/footprint", 1); vehicle_footprint_offset_pub_ = node.create_publisher("~/debug/footprint_offset", 1); @@ -143,8 +144,12 @@ void SurroundObstacleCheckerDebugNode::publish() debug_viz_pub_->publish(visualization_msg); /* publish stop reason for autoware api */ - const auto velocity_factor_msg = makeVelocityFactorArray(); - velocity_factor_pub_->publish(velocity_factor_msg); + if (stop_pose_ptr_ != nullptr) { + planning_factor_interface_->add( + 0.0, *stop_pose_ptr_, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); + } + planning_factor_interface_->publish(); /* reset variables */ stop_pose_ptr_ = nullptr; @@ -170,25 +175,6 @@ MarkerArray SurroundObstacleCheckerDebugNode::makeVisualizationMarker() return msg; } -VelocityFactorArray SurroundObstacleCheckerDebugNode::makeVelocityFactorArray() -{ - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - if (stop_pose_ptr_) { - using distance_type = VelocityFactor::_distance_type; - VelocityFactor velocity_factor; - velocity_factor.behavior = PlanningBehavior::SURROUNDING_OBSTACLE; - velocity_factor.pose = *stop_pose_ptr_; - velocity_factor.distance = std::numeric_limits::quiet_NaN(); - velocity_factor.status = VelocityFactor::UNKNOWN; - velocity_factor.detail = std::string(); - velocity_factor_array.factors.push_back(velocity_factor); - } - return velocity_factor_array; -} - PolygonStamped SurroundObstacleCheckerDebugNode::boostPolygonToPolygonStamped( const Polygon2d & boost_polygon, const double & z) { diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp index b2c350c1b4698..0569c6815a252 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.hpp @@ -15,13 +15,13 @@ #ifndef DEBUG_MARKER_HPP_ #define DEBUG_MARKER_HPP_ +#include #include #include -#include -#include #include #include +#include #include #include @@ -34,10 +34,10 @@ namespace autoware::surround_obstacle_checker { using autoware::vehicle_info_utils::VehicleInfo; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using geometry_msgs::msg::PolygonStamped; +using tier4_planning_msgs::msg::ControlPoint; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -65,12 +65,14 @@ class SurroundObstacleCheckerDebugNode private: rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr velocity_factor_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; + std::unique_ptr + planning_factor_interface_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::string object_label_; double surround_check_front_distance_; @@ -80,7 +82,6 @@ class SurroundObstacleCheckerDebugNode geometry_msgs::msg::Pose self_pose_; MarkerArray makeVisualizationMarker(); - VelocityFactorArray makeVelocityFactorArray(); PolygonStamped boostPolygonToPolygonStamped(const Polygon2d & boost_polygon, const double & z); diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 0adca312ca733..b3881a8f4fbd9 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -81,8 +81,8 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio "~/output/velocity_limit_clear_command", rclcpp::QoS{1}.transient_local()); pub_velocity_limit_ = this->create_publisher( "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); - pub_processing_time_ = - this->create_publisher("~/debug/processing_time_ms", 1); + pub_processing_time_ = this->create_publisher( + "~/debug/processing_time_ms", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -231,7 +231,7 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->pushPose(odometry_ptr_->pose.pose, PoseType::NoStart); } - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = stop_watch.toc(); pub_processing_time_->publish(processing_time_msg); diff --git a/planning/autoware_surround_obstacle_checker/src/node.hpp b/planning/autoware_surround_obstacle_checker/src/node.hpp index f84ad3a8f5987..b6d6bffefed1a 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.hpp +++ b/planning/autoware_surround_obstacle_checker/src/node.hpp @@ -24,12 +24,12 @@ #include #include +#include #include #include #include #include #include -#include #include #include #include @@ -106,7 +106,8 @@ class SurroundObstacleCheckerNode : public rclcpp::Node this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; - rclcpp::Publisher::SharedPtr pub_processing_time_; + rclcpp::Publisher::SharedPtr + pub_processing_time_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; diff --git a/planning/autoware_velocity_smoother/CHANGELOG.rst b/planning/autoware_velocity_smoother/CHANGELOG.rst index 4655c9658ef2b..82283cfca5b13 100644 --- a/planning/autoware_velocity_smoother/CHANGELOG.rst +++ b/planning/autoware_velocity_smoother/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package autoware_velocity_smoother ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(velocity_smoother): introduce diagnostics (`#9933 `_) + * feat(velocity_smoother): introduce diagnostics + * fix + --------- +* feat(velocity_smoother): use autoware internal Stamped messages (`#9749 `_) + * feat(velocity_smoother): use autoware internal Stamped messages + * fix + --------- +* Contributors: Fumiya Watanabe, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp index 069363d2f65e0..d519356c7aa18 100644 --- a/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware/velocity_smoother/node.hpp @@ -20,6 +20,7 @@ #include "autoware/osqp_interface/osqp_interface.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +#include "autoware/universe_utils/ros/diagnostics_interface.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/ros/self_pose_listener.hpp" @@ -39,14 +40,13 @@ #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float64_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary -#include "tier4_debug_msgs/msg/float64_stamped.hpp" // temporary -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" // temporary -#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary +#include "tier4_planning_msgs/msg/velocity_limit.hpp" // temporary #include "visualization_msgs/msg/marker_array.hpp" #include @@ -61,15 +61,15 @@ namespace autoware::velocity_smoother using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; +using autoware::universe_utils::DiagnosticsInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_internal_debug_msgs::msg::Float32Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32Stamped; // temporary -using tier4_debug_msgs::msg::Float64Stamped; // temporary -using tier4_planning_msgs::msg::StopSpeedExceeded; // temporary -using tier4_planning_msgs::msg::VelocityLimit; // temporary +using tier4_planning_msgs::msg::VelocityLimit; // temporary using visualization_msgs::msg::MarkerArray; struct Motion @@ -89,7 +89,6 @@ class VelocitySmootherNode : public rclcpp::Node private: rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; - rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; autoware::universe_utils::InterProcessPollingSubscriber sub_current_odometry_{ this, "/localization/kinematic_state"}; @@ -290,6 +289,8 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr published_time_publisher_; mutable std::shared_ptr time_keeper_{nullptr}; + + std::unique_ptr diagnostics_interface_{nullptr}; }; } // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 96462602d8da0..dc49d947c058d 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -2,7 +2,7 @@ autoware_velocity_smoother - 0.40.0 + 0.41.0 The autoware_velocity_smoother package Fumiya Watanabe @@ -21,6 +21,7 @@ autoware_cmake eigen3_cmake_module + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_osqp_interface @@ -34,7 +35,6 @@ rclcpp tf2 tf2_ros - tier4_debug_msgs tier4_planning_msgs ament_cmake_ros diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 1e0f6119ac21e..1f2b1cad6e42a 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -36,7 +36,8 @@ namespace autoware::velocity_smoother { VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) -: Node("velocity_smoother", node_options) +: Node("velocity_smoother", node_options), + diagnostics_interface_(std::make_unique(this, "velocity_smoother")) { using std::placeholders::_1; @@ -63,7 +64,6 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti pub_velocity_limit_ = create_publisher( "~/output/current_velocity_limit_mps", rclcpp::QoS{1}.transient_local()); pub_dist_to_stopline_ = create_publisher("~/distance_to_stopline", 1); - pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); @@ -444,6 +444,7 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr RCLCPP_DEBUG(get_logger(), "========================= run start ========================="); stop_watch_.tic(); + diagnostics_interface_->clear(); base_traj_raw_ptr_ = msg; // receive data @@ -524,6 +525,10 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr // Publish Calculation Time publishStopWatchTime(); + + // Publish diagnostics + diagnostics_interface_->publish(now()); + RCLCPP_DEBUG(get_logger(), "========================== run() end ==========================\n\n"); } @@ -906,12 +911,8 @@ void VelocitySmootherNode::overwriteStopPoint( input_stop_vel, output_stop_vel, over_stop_velocity_warn_thr_); } - { - StopSpeedExceeded msg{}; - msg.stamp = this->now(); - msg.stop_speed_exceeded = is_stop_velocity_exceeded; - pub_over_stop_velocity_->publish(msg); - } + diagnostics_interface_->add_key_value( + "The velocity on the stop point is larger than 0.", is_stop_velocity_exceeded); } void VelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & traj) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/CHANGELOG.rst index 0b2d004e1f02b..83cc289b049f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package autoware_behavior_path_avoidance_by_lane_change_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(behavior_path_planner): common test functions (`#9963 `_) + * feat: common test code in behavior_path_planner + * deal with other modules + * fix typo + * update + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* Contributors: Fumiya Watanabe, Mamoru Sobue, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index 83c361d064fb0..5350adf23fa26 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_avoidance_by_lane_change_module - 0.40.0 + 0.41.0 The behavior_path_avoidance_by_lane_change_module package Satoshi Ota diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index 3080ba1045b41..37bee1a178f90 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -32,13 +32,15 @@ AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( 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) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) : LaneChangeInterface{ name, node, parameters, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, + planning_factor_interface, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index c7fbba34b1adb..6e524f3c91341 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -40,7 +40,8 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface 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); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index c54774a575332..2bf8a60676d72 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -190,7 +190,7 @@ SMIPtr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index f679efacb6a44..349d92377d091 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,11 +12,7 @@ // 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 "autoware/behavior_path_planner/test_utils.hpp" #include @@ -25,84 +21,18 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::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 autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_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); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_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"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode( + {"lane_change", "static_obstacle_avoidance", "avoidance_by_lane_change"}, + {"autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory @@ -119,7 +49,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"lane_change", "static_obstacle_avoidance", "avoidance_by_lane_change"}, + {"autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/CHANGELOG.rst index 5b5179f250d8d..79f10763f452e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package autoware_behavior_path_dynamic_obstacle_avoidance_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(behavior_path_planner): common test functions (`#9963 `_) + * feat: common test code in behavior_path_planner + * deal with other modules + * fix typo + * update + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* Contributors: Fumiya Watanabe, Mamoru Sobue, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index f09196b2cc8e1..ea8f09c4f9336 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -42,7 +42,7 @@ class DynamicObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 44b56f4ea0990..8987e7c0446ee 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -351,7 +351,8 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); void updateModuleParams(const std::any & parameters) override { diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index e3aefb01c797d..bc52f39a030d4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_dynamic_obstacle_avoidance_module - 0.40.0 + 0.41.0 The autoware_behavior_path_dynamic_obstacle_avoidance_module package Takayuki Murooka diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index dc7efebe88fba..145e0df2675b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -331,8 +331,9 @@ DynamicObstacleAvoidanceModule::DynamicObstacleAvoidanceModule( 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 + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{std::move(parameters)}, target_objects_manager_{TargetObjectsManager( parameters_->successive_num_to_entry_dynamic_avoidance_condition, diff --git a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index a18a2440dcb3a..488475079613f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_dynamic_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,87 +12,23 @@ // 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 "autoware/behavior_path_planner/test_utils.hpp" #include #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::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 autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - - std::vector module_names; - module_names.emplace_back("autoware::behavior_path_planner::DynamicAvoidanceModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_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_dynamic_obstacle_avoidance_module") + - "/config/dynamic_obstacle_avoidance.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"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"dynamic_obstacle_avoidance"}, + {"autoware::behavior_path_planner::DynamicObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); @@ -110,7 +46,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"dynamic_obstacle_avoidance"}, + {"autoware::behavior_path_planner::DynamicObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/CHANGELOG.rst index 0a315105e5bf1..4d907f1d64a20 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package autoware_behavior_path_external_request_lane_change_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(behavior_path_planner): common test functions (`#9963 `_) + * feat: common test code in behavior_path_planner + * deal with other modules + * fix typo + * update + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* Contributors: Fumiya Watanabe, Mamoru Sobue, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml index af46393d9b9ae..30b1938bb9427 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_external_request_lane_change_module - 0.40.0 + 0.41.0 The autoware_behavior_path_external_request_lane_change_module package Fumiya Watanabe diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 45c997f364633..0b438d1a1758e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -28,7 +28,7 @@ ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, direction_)); } @@ -37,7 +37,7 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, direction_)); } diff --git a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index ad37a90605233..506e43a94de1d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,7 @@ // 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 "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware/behavior_path_planner/test_utils.hpp" #include @@ -24,83 +21,19 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::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 autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_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::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back( - "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, { - autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_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"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode( + {"lane_change"}, + {"autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager", + "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory @@ -117,7 +50,10 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"lane_change"}, + {"autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager", + "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CHANGELOG.rst index 627f216b6d7c9..f09d91b2c87ae 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CHANGELOG.rst @@ -2,6 +2,46 @@ Changelog for package autoware_behavior_path_goal_planner_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(goal_planner): introduce bezier based pullover for bus stop area (`#10027 `_) +* fix(goal_planner): fix waiting approval path of backward parking (`#10015 `_) +* refactor(goal_planner): divide extract_dynamic_object/is_goal_in_lanes util function (`#10019 `_) +* fix(start_planner, goal_planner): refactor lane departure checker initialization (`#9944 `_) +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix(goal_planner): fix geometric pull over (`#9932 `_) +* test(autoware_behavior_path_start_planner_module): add test helper and implement unit tests for FreespacePullOut (`#9832 `_) + * refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners + * refactor(autoware_behavior_path_start_planner_module): remove TimeKeeper parameter from pull-out planners + * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling + * refactor(planner): add planner_data parameter to plan methods in pull out planners + * refactor(autoware_behavior_path_start_planner_module): remove LaneDepartureChecker dependency from pull-out planners + --------- +* feat(goal_planner): update lateral_deviation_thresh from `0.3` to `0.1` (`#9850 `_) + * fix(goal_planner): Update lateral_deviation_thresh from 0.3 to 0.1 + * unified hasDeviatedFrom{Last|Current}PreviousModulePath + * style(pre-commit): autofix + * hasDeviatedFromPath argument modification + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat(goal_planner): cut stop path to goal (`#9829 `_) +* refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling (`#9791 `_) + * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling + --------- +* fix(goal_planner): fix usage of last_previous_module_output (`#9811 `_) +* fix(behavior_path_planner): add freespace_planning_algorithms dependency (`#9800 `_) +* chore(autoware_test_utils): update test map (`#9664 `_) +* refactor(goal_planner): divide sort function (`#9650 `_) +* perf(goal_planner): remove unnecessary call to setMap on freespace planning (`#9644 `_) +* feat(goal_planner): add bezier based pull over planner (`#9642 `_) +* feat(goal_planner): divide Planners to isolated threads (`#9514 `_) +* Contributors: Fumiya Watanabe, Kazunori-Nakajima, Kosuke Takeuchi, Kyoichi Sugahara, Mamoru Sobue, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index 8e945093659c1..bc8705dc76174 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt @@ -14,11 +14,13 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/pull_over_planner/freespace_pull_over.cpp src/pull_over_planner/geometric_pull_over.cpp src/pull_over_planner/shift_pull_over.cpp + src/pull_over_planner/bezier_pull_over.cpp src/default_fixed_goal_planner.cpp src/goal_searcher.cpp src/util.cpp src/goal_planner_module.cpp src/manager.cpp + src/thread_data.cpp src/decision_state.cpp ) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 5eb71126bb838..95b2095f87164 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -25,8 +25,8 @@ high_curvature_threshold: 0.1 bus_stop_area: use_bus_stop_area: false - goal_search_interval: 0.5 - lateral_offset_interval: 0.25 + goal_search_interval: 0.75 + lateral_offset_interval: 0.3 # occupancy grid map occupancy_grid: @@ -52,6 +52,7 @@ # pull over pull_over: minimum_request_length: 0.0 + pull_over_prepare_length: 100.0 pull_over_velocity: 3.0 pull_over_minimum_velocity: 1.38 decide_path_distance: 10.0 @@ -59,7 +60,7 @@ maximum_jerk: 1.0 path_priority: "efficient_path" # "efficient_path" or "close_goal" efficient_path_order: ["SHIFT", "ARC_FORWARD", "ARC_BACKWARD"] # only lane based pull over(exclude freespace parking) - lane_departure_check_expansion_margin: 0.0 + lane_departure_check_expansion_margin: 0.2 # shift parking shift_parking: @@ -73,21 +74,21 @@ # parallel parking path parallel_parking: path_interval: 1.0 - max_steer_angle: 0.35 # 20deg + max_steer_angle: 0.4 #22.9deg forward: enable_arc_forward_parking: true after_forward_parking_straight_distance: 2.0 forward_parking_velocity: 1.38 forward_parking_lane_departure_margin: 0.0 forward_parking_path_interval: 1.0 - forward_parking_max_steer_angle: 0.35 # 20deg + forward_parking_max_steer_angle: 0.4 # 22.9deg backward: enable_arc_backward_parking: true after_backward_parking_straight_distance: 2.0 backward_parking_velocity: -1.38 backward_parking_lane_departure_margin: 0.0 backward_parking_path_interval: 1.0 - backward_parking_max_steer_angle: 0.35 # 20deg + backward_parking_max_steer_angle: 0.4 # 22.9deg # freespace parking freespace_parking: @@ -130,6 +131,10 @@ neighbor_radius: 8.0 margin: 1.0 + bezier_parking: + pull_over_angle_threshold: 0.5 + after_shift_straight_distance: 1.5 + stop_condition: maximum_deceleration_for_stop: 1.0 maximum_jerk_for_stop: 1.0 @@ -139,13 +144,13 @@ min_velocity: 1.0 min_acceleration: 1.0 max_velocity: 1.0 - time_horizon_for_front_object: 10.0 - time_horizon_for_rear_object: 10.0 + time_horizon_for_front_object: 5.0 + time_horizon_for_rear_object: 5.0 time_resolution: 0.5 delay_until_departure: 1.0 # For target object filtering target_filtering: - safety_check_time_horizon: 10.0 + safety_check_time_horizon: 5.0 safety_check_time_resolution: 1.0 # detection range object_check_forward_distance: 100.0 @@ -179,7 +184,7 @@ # safety check configuration enable_safety_check: true method: "integral_predicted_polygon" - keep_unsafe_time: 3.0 + keep_unsafe_time: 0.5 # collision check parameters publish_debug_marker: false rss_params: @@ -192,7 +197,7 @@ forward_margin: 1.0 backward_margin: 1.0 lat_margin: 1.0 - time_horizon: 10.0 + time_horizon: 5.0 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 collision_check_yaw_diff_threshold: 3.1416 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case1.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case1.yaml new file mode 100644 index 0000000000000..7bbc4b54e7f08 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case1.yaml @@ -0,0 +1,267 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm +dynamic_object: + header: + stamp: + sec: 909 + nanosec: 742399532 + frame_id: map + objects: [] +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 125 + nanosec: 213336488 + frame_id: map + start_pose: + position: + x: 748.132 + y: 708.703 + z: 381.187 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.235076 + w: 0.971977 + goal_pose: + position: + x: 798.040 + y: 689.386 + z: 381.187 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.659713 + w: 0.751517 + segments: + - preferred_primitive: + id: 15214 + primitive_type: "" + primitives: + - id: 15214 + primitive_type: lane + - id: 15213 + primitive_type: lane + - preferred_primitive: + id: 15226 + primitive_type: "" + primitives: + - id: 15226 + primitive_type: lane + - id: 15225 + primitive_type: lane + - preferred_primitive: + id: 15228 + primitive_type: "" + primitives: + - id: 15228 + primitive_type: lane + - id: 15229 + primitive_type: lane + - preferred_primitive: + id: 15231 + primitive_type: "" + primitives: + - id: 15231 + primitive_type: lane + uuid: + uuid: + - 100 + - 171 + - 232 + - 218 + - 65 + - 244 + - 197 + - 172 + - 167 + - 40 + - 148 + - 169 + - 170 + - 48 + - 29 + - 68 + allow_modification: false +operation_mode: + stamp: + sec: 902 + nanosec: 525199106 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 909 + nanosec: 769937073 + frame_id: /base_link + accel: + accel: + linear: + x: -0.581360 + y: -0.290347 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 909 + nanosec: 769937073 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 760.155 + y: 713.417 + z: 381.187 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.116943 + w: 0.993139 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 3.83825 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.0756457 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case2.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case2.yaml new file mode 100644 index 0000000000000..ab86692166164 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case2.yaml @@ -0,0 +1,261 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm +dynamic_object: + header: + stamp: + sec: 57 + nanosec: 642011543 + frame_id: map + objects: [] +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 49 + nanosec: 727009929 + frame_id: map + start_pose: + position: + x: 266.506 + y: 343.545 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695321 + w: 0.718700 + goal_pose: + position: + x: 291.295 + y: 346.135 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999225 + w: 0.0393747 + segments: + - preferred_primitive: + id: 697 + primitive_type: "" + primitives: + - id: 697 + primitive_type: lane + - preferred_primitive: + id: 759 + primitive_type: "" + primitives: + - id: 759 + primitive_type: lane + - preferred_primitive: + id: 675 + primitive_type: "" + primitives: + - id: 675 + primitive_type: lane + - preferred_primitive: + id: 676 + primitive_type: "" + primitives: + - id: 676 + primitive_type: lane + uuid: + uuid: + - 126 + - 92 + - 126 + - 245 + - 182 + - 56 + - 251 + - 150 + - 125 + - 65 + - 22 + - 180 + - 59 + - 40 + - 250 + - 241 + allow_modification: false +operation_mode: + stamp: + sec: 40 + nanosec: 229462653 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 57 + nanosec: 675161172 + frame_id: /base_link + accel: + accel: + linear: + x: -0.0735332 + y: -0.539254 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 57 + nanosec: 675161172 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 288.819 + y: 363.189 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0508455 + w: 0.998707 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 1.95939 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.275215 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case3.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case3.yaml new file mode 100644 index 0000000000000..48a84480dd3a4 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/sample_planner_data_case3.yaml @@ -0,0 +1,261 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/road_shoulder/lanelet2_map.osm +dynamic_object: + header: + stamp: + sec: 59 + nanosec: 863135063 + frame_id: map + objects: [] +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 49 + nanosec: 727009929 + frame_id: map + start_pose: + position: + x: 266.506 + y: 343.545 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695321 + w: 0.718700 + goal_pose: + position: + x: 291.295 + y: 346.135 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999225 + w: 0.0393747 + segments: + - preferred_primitive: + id: 697 + primitive_type: "" + primitives: + - id: 697 + primitive_type: lane + - preferred_primitive: + id: 759 + primitive_type: "" + primitives: + - id: 759 + primitive_type: lane + - preferred_primitive: + id: 675 + primitive_type: "" + primitives: + - id: 675 + primitive_type: lane + - preferred_primitive: + id: 676 + primitive_type: "" + primitives: + - id: 676 + primitive_type: lane + uuid: + uuid: + - 126 + - 92 + - 126 + - 245 + - 182 + - 56 + - 251 + - 150 + - 125 + - 65 + - 22 + - 180 + - 59 + - 40 + - 250 + - 241 + allow_modification: false +operation_mode: + stamp: + sec: 40 + nanosec: 229462653 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 59 + nanosec: 863135063 + frame_id: /base_link + accel: + accel: + linear: + x: 0.0180450 + y: -0.573187 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 59 + nanosec: 863135063 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 292.928 + y: 362.175 + z: 100.000 + orientation: + x: 0.00000 + y: -0.00000 + z: -0.283955 + w: 0.958837 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 2.01175 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.284920 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp new file mode 100644 index 0000000000000..6172fb75978cd --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case1.cpp @@ -0,0 +1,705 @@ +// 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_goal_planner_module/goal_searcher.hpp" +#include "autoware/behavior_path_goal_planner_module/util.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +using autoware::behavior_path_planner::BehaviorModuleOutput; +using autoware::behavior_path_planner::GoalCandidate; +using autoware::behavior_path_planner::GoalCandidates; +using autoware::behavior_path_planner::GoalPlannerParameters; +using autoware::behavior_path_planner::PlannerData; +using autoware::behavior_path_planner::PullOverPath; +using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDecelDistance; +using autoware_planning_msgs::msg::LaneletRoute; +using tier4_planning_msgs::msg::PathWithLaneId; + +std::vector g_colors = { + "#F0F8FF", "#FAEBD7", "#00FFFF", "#7FFFD4", "#F0FFFF", "#F5F5DC", "#FFE4C4", "#000000", "#FFEBCD", + "#0000FF", "#8A2BE2", "#A52A2A", "#DEB887", "#5F9EA0", "#7FFF00", "#D2691E", "#FF7F50", "#6495ED", + "#FFF8DC", "#DC143C", "#00FFFF", "#00008B", "#008B8B", "#B8860B", "#A9A9A9", "#006400", "#A9A9A9", + "#BDB76B", "#8B008B", "#556B2F", "#FF8C00", "#9932CC", "#8B0000", "#E9967A", "#8FBC8F", "#483D8B", + "#2F4F4F", "#2F4F4F", "#00CED1", "#9400D3", "#FF1493", "#00BFFF", "#696969", "#696969", "#1E90FF", + "#B22222", "#FFFAF0", "#228B22", "#FF00FF", "#DCDCDC", "#F8F8FF", "#FFD700", "#DAA520", "#808080", + "#008000", "#ADFF2F", "#808080", "#F0FFF0", "#FF69B4", "#CD5C5C", "#4B0082", "#FFFFF0", "#F0E68C", + "#E6E6FA", "#FFF0F5", "#7CFC00", "#FFFACD", "#ADD8E6", "#F08080", "#E0FFFF", "#FAFAD2", "#D3D3D3", + "#90EE90", "#D3D3D3", "#FFB6C1", "#FFA07A", "#20B2AA", "#87CEFA", "#778899", "#778899", "#B0C4DE", + "#FFFFE0", "#00FF00", "#32CD32", "#FAF0E6", "#FF00FF", "#800000", "#66CDAA", "#0000CD", "#BA55D3", + "#9370DB", "#3CB371", "#7B68EE", "#00FA9A", "#48D1CC", "#C71585", "#191970", "#F5FFFA", "#FFE4E1", + "#FFE4B5", "#FFDEAD", "#000080", "#FDF5E6", "#808000", "#6B8E23", "#FFA500", "#FF4500", "#DA70D6", + "#EEE8AA", "#98FB98", "#AFEEEE", "#DB7093", "#FFEFD5", "#FFDAB9", "#CD853F", "#FFC0CB", "#DDA0DD", + "#B0E0E6", "#800080", "#663399", "#FF0000", "#BC8F8F", "#4169E1", "#8B4513", "#FA8072", "#F4A460", + "#2E8B57", "#FFF5EE", "#A0522D", "#C0C0C0", "#87CEEB", "#6A5ACD", "#708090", "#708090", "#FFFAFA", + "#00FF7F", "#4682B4", "#D2B48C", "#008080", "#D8BFD8", "#FF6347", "#40E0D0", "#EE82EE", "#F5DEB3", + "#FFFFFF", "#F5F5F5", "#FFFF00", "#9ACD32"}; + +void plot_footprint( + matplotlibcpp17::axes::Axes & axes, const autoware::universe_utils::LinearRing2d & footprint, + const std::string & color) +{ + std::vector xs, ys; + for (const auto & pt : footprint) { + xs.push_back(pt.x()); + ys.push_back(pt.y()); + } + xs.push_back(xs.front()); + ys.push_back(ys.front()); + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linestyle"_a = "dotted")); +} + +void plot_lanelet_polygon(matplotlibcpp17::axes::Axes & axes, const lanelet::BasicPolygon2d polygon) +{ + std::vector xs, ys; + for (const auto & p : polygon) { + xs.push_back(p.x()); + ys.push_back(p.y()); + } + xs.push_back(xs.front()); + ys.push_back(ys.front()); + axes.fill(Args(xs, ys), Kwargs("color"_a = "grey", "alpha"_a = 0.5)); +} + +void plot_goal_candidate( + matplotlibcpp17::axes::Axes & axes, const GoalCandidate & goal, const size_t prio, + const autoware::universe_utils::LinearRing2d & local_footprint, const std::string & color) +{ + std::vector xs, ys; + std::vector yaw_cos, yaw_sin; + const auto goal_footprint = + transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); + plot_footprint(axes, goal_footprint, color); + xs.push_back(goal.goal_pose.position.x); + ys.push_back(goal.goal_pose.position.y); + axes.text(Args(xs.back(), ys.back(), std::to_string(prio))); + const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; + yaw_cos.push_back(std::cos(yaw)); + yaw_sin.push_back(std::sin(yaw)); + axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 2.0)); +} + +void plot_goal_candidates( + matplotlibcpp17::axes::Axes & axes, const GoalCandidates & goals, + const std::map & goal_id2prio, + const autoware::universe_utils::LinearRing2d & local_footprint, + const std::string & color = "green") +{ + for (const auto & goal : goals) { + const auto it = goal_id2prio.find(goal.id); + if (it != goal_id2prio.end()) { + plot_goal_candidate(axes, goal, it->second, local_footprint, color); + } + } +} + +void plot_path_with_lane_id( + matplotlibcpp17::axes::Axes & axes, const PathWithLaneId & path, + const std::string & color = "red", const std::string & label = "", const double linewidth = 1.0) +{ + std::vector xs, ys; + std::vector yaw_cos, yaw_sin; + for (const auto & point : path.points) { + xs.push_back(point.point.pose.position.x); + ys.push_back(point.point.pose.position.y); + const double yaw = autoware::universe_utils::getRPY(point.point.pose).z; + yaw_cos.push_back(std::cos(yaw)); + yaw_sin.push_back(std::sin(yaw)); + axes.scatter( + Args(xs.back(), ys.back()), Kwargs("marker"_a = "o", "color"_a = "blue", "s"_a = 10)); + } + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 2.0)); + + if (label == "") { + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = linewidth)); + } else { + axes.plot( + Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = linewidth, "label"_a = label)); + } +} + +void plot_lanelet( + matplotlibcpp17::axes::Axes & axes, lanelet::ConstLanelet lanelet, + const std::string & color = "blue", const double linewidth = 0.5) +{ + const auto lefts = lanelet.leftBound(); + const auto rights = lanelet.rightBound(); + std::vector xs_left, ys_left; + for (const auto & point : lefts) { + xs_left.push_back(point.x()); + ys_left.push_back(point.y()); + } + + std::vector xs_right, ys_right; + for (const auto & point : rights) { + xs_right.push_back(point.x()); + ys_right.push_back(point.y()); + } + + std::vector xs_center, ys_center; + for (const auto & point : lanelet.centerline()) { + xs_center.push_back(point.x()); + ys_center.push_back(point.y()); + } + + axes.plot(Args(xs_left, ys_left), Kwargs("color"_a = color, "linewidth"_a = linewidth)); + axes.plot(Args(xs_right, ys_right), Kwargs("color"_a = color, "linewidth"_a = linewidth)); + axes.plot( + Args(xs_center, ys_center), + Kwargs("color"_a = "black", "linewidth"_a = linewidth, "linestyle"_a = "dashed")); +} + +std::shared_ptr instantiate_planner_data( + rclcpp::Node::SharedPtr node, const std::string & map_path, + const std::string & sample_planner_data_yaml_path) +{ + lanelet::ErrorMessages errors{}; + lanelet::projection::MGRSProjector projector{}; + const lanelet::LaneletMapPtr lanelet_map_ptr = lanelet::load(map_path, projector, &errors); + if (!errors.empty()) { + for (const auto & error : errors) { + std::cout << error << std::endl; + } + return nullptr; + } + autoware_map_msgs::msg::LaneletMapBin map_bin; + lanelet::utils::conversion::toBinMsg( + lanelet_map_ptr, &map_bin); // TODO(soblin): pass lanelet_map_ptr to RouteHandler + + YAML::Node config = YAML::LoadFile(sample_planner_data_yaml_path); + + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node); + planner_data->route_handler->setMap(map_bin); + planner_data->route_handler->setRoute( + autoware::test_utils::parse(config["route"])); + + auto odometry = autoware::test_utils::create_const_shared_ptr( + autoware::test_utils::parse(config["self_odometry"])); + planner_data->self_odometry = odometry; + + auto accel_ptr = autoware::test_utils::create_const_shared_ptr( + autoware::test_utils::parse( + config["self_acceleration"])); + planner_data->self_acceleration = accel_ptr; + return planner_data; +} + +bool hasEnoughDistance( + const PullOverPath & pull_over_path, const PathWithLaneId & long_tail_reference_path, + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + using autoware::motion_utils::calcSignedArcLength; + + const Pose & current_pose = planner_data->self_odometry->pose.pose; + const double current_vel = planner_data->self_odometry->twist.twist.linear.x; + + // when the path is separated and start_pose is close, + // once stopped, the vehicle cannot start again. + // so need enough distance to restart. + // distance to restart should be less than decide_path_distance. + // otherwise, the goal would change immediately after departure. + const bool is_separated_path = pull_over_path.partial_paths().size() > 1; + const double distance_to_start = calcSignedArcLength( + long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose().position); + const double distance_to_restart = parameters.decide_path_distance / 2; + const double eps_vel = 0.01; + const bool is_stopped = std::abs(current_vel) < eps_vel; + if (is_separated_path && is_stopped && distance_to_start < distance_to_restart) { + return false; + } + + const auto current_to_stop_distance = calcFeasibleDecelDistance( + planner_data, parameters.maximum_deceleration, parameters.maximum_jerk, 0.0); + if (!current_to_stop_distance) { + return false; + } + + /* + // If the stop line is subtly exceeded, it is assumed that there is not enough distance to the + // starting point of parking, so to prevent this, once the vehicle has stopped, it also has a + // stop_distance_buffer to allow for the amount exceeded. + const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; + if (distance_to_start + buffer < *current_to_stop_distance) { + return false; + }*/ + + return true; +} + +std::vector selectPullOverPaths( + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output) +{ + using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; + using autoware::motion_utils::calcSignedArcLength; + + const auto & goal_pose = planner_data->route_handler->getOriginalGoalPose(); + const double backward_length = + parameters.backward_goal_search_length + parameters.decide_path_distance; + + std::vector sorted_path_indices; + sorted_path_indices.reserve(pull_over_path_candidates.size()); + + std::unordered_map goal_candidate_map; + for (const auto & goal_candidate : goal_candidates) { + goal_candidate_map[goal_candidate.id] = goal_candidate; + } + for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { + const auto & path = pull_over_path_candidates[i]; + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); + if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { + sorted_path_indices.push_back(i); + } + } + + const double prev_path_front_to_goal_dist = calcSignedArcLength( + upstream_module_output.path.points, + upstream_module_output.path.points.front().point.pose.position, goal_pose.position); + const auto & long_tail_reference_path = [&]() { + if (prev_path_front_to_goal_dist > backward_length) { + return upstream_module_output.path; + } + // get road lanes which is at least backward_length[m] behind the goal + const auto road_lanes = getExtendedCurrentLanesFromPath( + upstream_module_output.path, planner_data, backward_length, 0.0, false); + const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + return planner_data->route_handler->getCenterLinePath( + road_lanes, std::max(0.0, goal_pose_length - backward_length), + goal_pose_length + parameters.forward_goal_search_length); + }(); + + sorted_path_indices.erase( + std::remove_if( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t i) { + return !hasEnoughDistance( + pull_over_path_candidates[i], long_tail_reference_path, planner_data, parameters); + }), + sorted_path_indices.end()); + + const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins; + const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins; + + const auto [margins, margins_with_zero] = + std::invoke([&]() -> std::tuple, std::vector> { + std::vector margins = soft_margins; + margins.insert(margins.end(), hard_margins.begin(), hard_margins.end()); + std::vector margins_with_zero = margins; + margins_with_zero.push_back(0.0); + return std::make_tuple(margins, margins_with_zero); + }); + + // Create a map of PullOverPath pointer to largest collision check margin + std::map path_id_to_rough_margin_map; + const auto & target_objects = autoware_perception_msgs::msg::PredictedObjects{}; + for (const size_t i : sorted_path_indices) { + const auto & path = pull_over_path_candidates[i]; // cppcheck-suppress containerOutOfBounds + const double distance = + autoware::behavior_path_planner::utils::path_safety_checker::calculateRoughDistanceToObjects( + path.parking_path(), target_objects, planner_data->parameters, false, "max"); + auto it = std::lower_bound( + margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); + if (it == margins_with_zero.end()) { + path_id_to_rough_margin_map[path.id()] = margins_with_zero.back(); + } else { + path_id_to_rough_margin_map[path.id()] = *it; + } + } + + // sorts in descending order so the item with larger margin comes first + std::stable_sort( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; + if ( + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01) { + return false; + } + return path_id_to_rough_margin_map[a.id()] > path_id_to_rough_margin_map[b.id()]; + }); + + // STEP2-3: Sort by curvature + // If the curvature is less than the threshold, prioritize the path. + const auto isHighCurvature = [&](const PullOverPath & path) -> bool { + return path.parking_path_max_curvature() >= parameters.high_curvature_threshold; + }; + + const auto isSoftMargin = [&](const PullOverPath & path) -> bool { + const double margin = path_id_to_rough_margin_map[path.id()]; + return std::any_of( + soft_margins.begin(), soft_margins.end(), + [margin](const double soft_margin) { return std::abs(margin - soft_margin) < 0.01; }); + }; + const auto isSameHardMargin = [&](const PullOverPath & a, const PullOverPath & b) -> bool { + return !isSoftMargin(a) && !isSoftMargin(b) && + std::abs(path_id_to_rough_margin_map[a.id()] - path_id_to_rough_margin_map[b.id()]) < + 0.01; + }; + + // NOTE: this is just partition sort based on curvature threshold within each sub partitions + std::stable_sort( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; + // if both are soft margin or both are same hard margin, prioritize the path with lower + // curvature. + if ((isSoftMargin(a) && isSoftMargin(b)) || isSameHardMargin(a, b)) { + return !isHighCurvature(a) && isHighCurvature(b); + } + // otherwise, keep the order based on the margin. + return false; + }); + + // STEP2-4: Sort pull_over_path_candidates based on the order in efficient_path_order keeping + // the collision check margin and curvature priority. + if (parameters.path_priority == "efficient_path") { + std::stable_sort( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t a_i, const size_t b_i) { + // if any of following conditions are met, sort by path type priority + // - both are soft margin + // - both are same hard margin + const auto & a = pull_over_path_candidates[a_i]; + const auto & b = pull_over_path_candidates[b_i]; + // otherwise, keep the order. + return false; + }); + } + + std::vector selected; + for (const auto & sorted_index : sorted_path_indices) { + selected.push_back(pull_over_path_candidates.at(sorted_index)); + } + + return selected; +} + +std::optional calculate_centerline_path( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + const auto original_goal_pose = planner_data->route_handler->getOriginalGoalPose(); + const auto refined_goal_opt = + autoware::behavior_path_planner::goal_planner_utils::calcRefinedGoal( + original_goal_pose, planner_data->route_handler, true, + planner_data->parameters.vehicle_length, planner_data->parameters.base_link2front, + planner_data->parameters.base_link2front, parameters); + if (!refined_goal_opt) { + return std::nullopt; + } + const auto & refined_goal = refined_goal_opt.value(); + + const auto & route_handler = planner_data->route_handler; + const double forward_length = parameters.forward_goal_search_length; + const double backward_length = parameters.backward_goal_search_length; + const bool use_bus_stop_area = parameters.bus_stop_area.use_bus_stop_area; + /* + const double margin_from_boundary = parameters.margin_from_boundary; + const double lateral_offset_interval = use_bus_stop_area + ? parameters.bus_stop_area.lateral_offset_interval + : parameters.lateral_offset_interval; + const double max_lateral_offset = parameters.max_lateral_offset; + const double ignore_distance_from_lane_start = parameters.ignore_distance_from_lane_start; + */ + + const auto pull_over_lanes = + autoware::behavior_path_planner::goal_planner_utils::getPullOverLanes( + *route_handler, true, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + const auto departure_check_lane = + autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *route_handler, true); + const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, refined_goal); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + const double longitudinal_interval = use_bus_stop_area + ? parameters.bus_stop_area.goal_search_interval + : parameters.goal_search_interval; + auto center_line_path = autoware::behavior_path_planner::utils::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), longitudinal_interval); + return center_line_path; +} + +std::vector getBusStopAreaPolygons( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + const auto pull_over_lanes = + autoware::behavior_path_planner::goal_planner_utils::getPullOverLanes( + *(planner_data->route_handler), true, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + return autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons( + pull_over_lanes); +} + +struct SortByWeightedDistance +{ + double lateral_cost{0.0}; + bool prioritize_goals_before_objects{false}; + + SortByWeightedDistance(double cost, bool prioritize_goals_before_objects) + : lateral_cost(cost), prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } + + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < + b.distance_from_original_goal + lateral_cost * b.lateral_offset; + } +}; + +int main(int argc, char ** argv) +{ + using autoware::behavior_path_planner::utils::getReferencePath; + rclcpp::init(argc, argv); + + auto node_options = rclcpp::NodeOptions{}; + node_options.parameter_overrides( + std::vector{{"launch_modules", std::vector{}}}); + node_options.arguments(std::vector{ + "--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + + "/config/behavior_path_planner.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + + "/config/drivable_area_expansion.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner") + + "/config/scene_module_manager.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_common.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_nearest_search.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_goal_planner_module") + + "/config/goal_planner.param.yaml"}); + auto node = rclcpp::Node::make_shared("plot_map", node_options); + + auto planner_data = instantiate_planner_data( + node, + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/test_map/road_shoulder/lanelet2_map.osm", + ament_index_cpp::get_package_share_directory("autoware_behavior_path_goal_planner_module") + + "/config/sample_planner_data_case1.yaml"); + + lanelet::ConstLanelet current_route_lanelet; + planner_data->route_handler->getClosestLaneletWithinRoute( + planner_data->self_odometry->pose.pose, ¤t_route_lanelet); + const auto reference_path = getReferencePath(current_route_lanelet, planner_data); + auto goal_planner_parameter = + autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters( + node.get(), "goal_planner."); + goal_planner_parameter.bus_stop_area.use_bus_stop_area = true; + goal_planner_parameter.lane_departure_check_expansion_margin = 0.2; + autoware::lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + goal_planner_parameter.lane_departure_check_expansion_margin; + autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker( + lane_departure_checker_params, vehicle_info); + + const auto footprint = vehicle_info.createFootprint(); + autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); + auto goal_candidates = goal_searcher.search(planner_data); + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance( + goal_planner_parameter.minimum_weighted_distance_lateral_weight, + goal_planner_parameter.prioritize_goals_before_objects)); + std::map goal_id2prio{}; + for (auto i = 0; i < goal_candidates.size(); ++i) { + goal_id2prio[goal_candidates.at(i).id] = i; + } + + pybind11::scoped_interpreter guard{}; + auto plt = matplotlibcpp17::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 2); + + auto & ax1 = axes[0]; + auto & ax2 = axes[1]; + // auto & ax3 = axes[2]; + const std::vector ids{/*15213, 15214, */ 15225, + 15226, + 15224, + 15227, + 15228, + 15229, + 15230, + 15231, + 15232}; + for (const auto & id : ids) { + const auto lanelet = planner_data->route_handler->getLaneletMapPtr()->laneletLayer.get(id); + plot_lanelet(ax1, lanelet); + plot_lanelet(ax2, lanelet); + // plot_lanelet(ax3, lanelet); + } + + // plot_goal_candidates(ax1, goal_candidates, footprint); + + // plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); + + const auto start = std::chrono::steady_clock::now(); + std::vector candidates; + for (auto i = 0; i < goal_candidates.size(); ++i) { + const auto & goal_candidate = goal_candidates.at(i); + auto shift_pull_over_planner = autoware::behavior_path_planner::BezierPullOver( + *node, goal_planner_parameter, lane_departure_checker); + auto pull_over_paths = + shift_pull_over_planner.plans(goal_candidate, 0, planner_data, reference_path); + if (!pull_over_paths.empty()) { + std::copy( + std::make_move_iterator(pull_over_paths.begin()), + std::make_move_iterator(pull_over_paths.end()), std::back_inserter(candidates)); + } + } + + const auto filtered_paths = selectPullOverPaths( + candidates, goal_candidates, planner_data, goal_planner_parameter, reference_path); + std::cout << filtered_paths.size() << std::endl; + const auto end = std::chrono::steady_clock::now(); + std::cout << "computed candidate bezier paths in " + << std::chrono::duration_cast(end - start).count() * 1.0 / + 1000000 + << "msecs" << std::endl; + std::cout << "filtered " << filtered_paths.size() << "/" << candidates.size() << " paths" + << std::endl; + /* + for (auto i = 0; i < filtered_paths.size(); ++i) { + const auto & filtered_path = filtered_paths.at(i); + const auto goal_id = filtered_path.goal_id(); + const auto prio = goal_id2prio[goal_id]; + const auto & color = (i == 0) ? "red" : g_colors.at(i % g_colors.size()); + const auto max_parking_curvature = filtered_path.parking_path_max_curvature(); + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + if (i == 0) { + plot_path_with_lane_id(ax1, filtered_path.parking_path(), color, "most prio", 2.0); + } else { + plot_path_with_lane_id( + ax1, filtered_path.full_path(), color, + std::to_string(prio) + "-th goal(id=" + std::to_string(goal_id) + + "): k_max=" + std::to_string(max_parking_curvature), + 0.5); + } + } + */ + const auto original_goal_pos = planner_data->route_handler->getOriginalGoalPose().position; + ax1.plot( + Args(original_goal_pos.x, original_goal_pos.y), + Kwargs("marker"_a = "x", "label"_a = "goal", "markersize"_a = 20, "color"_a = "red")); + if (goal_planner_parameter.bus_stop_area.use_bus_stop_area) { + const auto bus_stop_area_polygons = + getBusStopAreaPolygons(planner_data, goal_planner_parameter); + for (const auto & bus_stop_area_polygon : bus_stop_area_polygons) { + plot_lanelet_polygon(ax1, bus_stop_area_polygon); + } + } + + for (auto i = 0; i < filtered_paths.size(); ++i) { + const auto & filtered_path = filtered_paths.at(i); + const auto goal_id = filtered_path.goal_id(); + const auto prio = goal_id2prio[goal_id]; + const auto & color = (i == 0) ? "red" : g_colors.at(i % g_colors.size()); + const auto max_parking_curvature = filtered_path.parking_path_max_curvature(); + if (i == 0) { + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + plot_path_with_lane_id(ax2, filtered_path.full_path(), color, "most prio", 2.0); + for (const auto & path_point : filtered_path.full_path().points) { + const auto pose_footprint = transformVector( + footprint, autoware::universe_utils::pose2transform(path_point.point.pose)); + plot_footprint(ax2, pose_footprint, "blue"); + } + } else if (i % 50 == 0) { + std::cout << "plotting " << i << "-th filtered path" << std::endl; + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + plot_path_with_lane_id(ax1, filtered_path.full_path(), color, "", 2.0); + } + } + ax2.plot( + Args(original_goal_pos.x, original_goal_pos.y), + Kwargs("marker"_a = "x", "label"_a = "goal", "markersize"_a = 20, "color"_a = "red")); + + const auto centerline_path = calculate_centerline_path(planner_data, goal_planner_parameter); + if (centerline_path) { + // plot_path_with_lane_id(ax2, centerline_path.value(), "red", "centerline_path"); + } + + ax1.set_aspect(Args("equal")); + ax2.set_aspect(Args("equal")); + ax2.legend(); + plt.show(); + + rclcpp::shutdown(); + return 0; +} diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp similarity index 65% rename from planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp rename to planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp index 40c2bd201abc7..f7fefa8ba9dc0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/examples/plot_map_case2.cpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -28,6 +29,7 @@ #include #include #include +#include #include #include @@ -40,6 +42,8 @@ #include #include +#include +#include #include #include @@ -52,7 +56,6 @@ #include #include #include - using namespace std::chrono_literals; // NOLINT using autoware::behavior_path_planner::BehaviorModuleOutput; @@ -65,9 +68,28 @@ using autoware::behavior_path_planner::utils::parking_departure::calcFeasibleDec using autoware_planning_msgs::msg::LaneletRoute; using tier4_planning_msgs::msg::PathWithLaneId; +std::vector g_colors = { + "#F0F8FF", "#FAEBD7", "#00FFFF", "#7FFFD4", "#F0FFFF", "#F5F5DC", "#FFE4C4", "#000000", "#FFEBCD", + "#0000FF", "#8A2BE2", "#A52A2A", "#DEB887", "#5F9EA0", "#7FFF00", "#D2691E", "#FF7F50", "#6495ED", + "#FFF8DC", "#DC143C", "#00FFFF", "#00008B", "#008B8B", "#B8860B", "#A9A9A9", "#006400", "#A9A9A9", + "#BDB76B", "#8B008B", "#556B2F", "#FF8C00", "#9932CC", "#8B0000", "#E9967A", "#8FBC8F", "#483D8B", + "#2F4F4F", "#2F4F4F", "#00CED1", "#9400D3", "#FF1493", "#00BFFF", "#696969", "#696969", "#1E90FF", + "#B22222", "#FFFAF0", "#228B22", "#FF00FF", "#DCDCDC", "#F8F8FF", "#FFD700", "#DAA520", "#808080", + "#008000", "#ADFF2F", "#808080", "#F0FFF0", "#FF69B4", "#CD5C5C", "#4B0082", "#FFFFF0", "#F0E68C", + "#E6E6FA", "#FFF0F5", "#7CFC00", "#FFFACD", "#ADD8E6", "#F08080", "#E0FFFF", "#FAFAD2", "#D3D3D3", + "#90EE90", "#D3D3D3", "#FFB6C1", "#FFA07A", "#20B2AA", "#87CEFA", "#778899", "#778899", "#B0C4DE", + "#FFFFE0", "#00FF00", "#32CD32", "#FAF0E6", "#FF00FF", "#800000", "#66CDAA", "#0000CD", "#BA55D3", + "#9370DB", "#3CB371", "#7B68EE", "#00FA9A", "#48D1CC", "#C71585", "#191970", "#F5FFFA", "#FFE4E1", + "#FFE4B5", "#FFDEAD", "#000080", "#FDF5E6", "#808000", "#6B8E23", "#FFA500", "#FF4500", "#DA70D6", + "#EEE8AA", "#98FB98", "#AFEEEE", "#DB7093", "#FFEFD5", "#FFDAB9", "#CD853F", "#FFC0CB", "#DDA0DD", + "#B0E0E6", "#800080", "#663399", "#FF0000", "#BC8F8F", "#4169E1", "#8B4513", "#FA8072", "#F4A460", + "#2E8B57", "#FFF5EE", "#A0522D", "#C0C0C0", "#87CEEB", "#6A5ACD", "#708090", "#708090", "#FFFAFA", + "#00FF7F", "#4682B4", "#D2B48C", "#008080", "#D8BFD8", "#FF6347", "#40E0D0", "#EE82EE", "#F5DEB3", + "#FFFFFF", "#F5F5F5", "#FFFF00", "#9ACD32"}; + void plot_footprint( matplotlibcpp17::axes::Axes & axes, const autoware::universe_utils::LinearRing2d & footprint, - const std::string & color = "blue") + const std::string & color) { std::vector xs, ys; for (const auto & pt : footprint) { @@ -79,43 +101,77 @@ void plot_footprint( axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linestyle"_a = "dotted")); } +void plot_lanelet_polygon(matplotlibcpp17::axes::Axes & axes, const lanelet::BasicPolygon2d polygon) +{ + std::vector xs, ys; + for (const auto & p : polygon) { + xs.push_back(p.x()); + ys.push_back(p.y()); + } + xs.push_back(xs.front()); + ys.push_back(ys.front()); + axes.fill(Args(xs, ys), Kwargs("color"_a = "grey", "alpha"_a = 0.5)); +} + +void plot_goal_candidate( + matplotlibcpp17::axes::Axes & axes, const GoalCandidate & goal, const size_t prio, + const autoware::universe_utils::LinearRing2d & local_footprint, const std::string & color) +{ + std::vector xs, ys; + std::vector yaw_cos, yaw_sin; + const auto goal_footprint = + transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); + plot_footprint(axes, goal_footprint, color); + xs.push_back(goal.goal_pose.position.x); + ys.push_back(goal.goal_pose.position.y); + axes.text(Args(xs.back(), ys.back(), std::to_string(prio))); + const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; + yaw_cos.push_back(std::cos(yaw)); + yaw_sin.push_back(std::sin(yaw)); + axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 2.0)); +} + void plot_goal_candidates( matplotlibcpp17::axes::Axes & axes, const GoalCandidates & goals, + const std::map & goal_id2prio, const autoware::universe_utils::LinearRing2d & local_footprint, const std::string & color = "green") { - std::vector xs, ys; - std::vector yaw_cos, yaw_sin; for (const auto & goal : goals) { - const auto goal_footprint = - transformVector(local_footprint, autoware::universe_utils::pose2transform(goal.goal_pose)); - plot_footprint(axes, goal_footprint); - xs.push_back(goal.goal_pose.position.x); - ys.push_back(goal.goal_pose.position.y); - axes.text(Args(xs.back(), ys.back(), std::to_string(goal.id))); - const double yaw = autoware::universe_utils::getRPY(goal.goal_pose).z; - yaw_cos.push_back(std::cos(yaw)); - yaw_sin.push_back(std::sin(yaw)); + const auto it = goal_id2prio.find(goal.id); + if (it != goal_id2prio.end()) { + plot_goal_candidate(axes, goal, it->second, local_footprint, color); + } } - axes.scatter(Args(xs, ys), Kwargs("color"_a = color)); - axes.quiver( - Args(xs, ys, yaw_cos, yaw_sin), - Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 1.0)); } void plot_path_with_lane_id( matplotlibcpp17::axes::Axes & axes, const PathWithLaneId & path, - const std::string & color = "red", const std::string & label = "") + const std::string & color = "red", const std::string & label = "", const double linewidth = 1.0) { std::vector xs, ys; + std::vector yaw_cos, yaw_sin; for (const auto & point : path.points) { xs.push_back(point.point.pose.position.x); ys.push_back(point.point.pose.position.y); + const double yaw = autoware::universe_utils::getRPY(point.point.pose).z; + yaw_cos.push_back(std::cos(yaw)); + yaw_sin.push_back(std::sin(yaw)); + axes.scatter( + Args(xs.back(), ys.back()), Kwargs("marker"_a = "o", "color"_a = "blue", "s"_a = 10)); } + axes.quiver( + Args(xs, ys, yaw_cos, yaw_sin), + Kwargs("angles"_a = "xy", "scale_units"_a = "xy", "scale"_a = 2.0)); + if (label == "") { - axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0)); + axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = linewidth)); } else { - axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = 1.0, "label"_a = label)); + axes.plot( + Args(xs, ys), Kwargs("color"_a = color, "linewidth"_a = linewidth, "label"_a = label)); } } @@ -151,7 +207,8 @@ void plot_lanelet( } std::shared_ptr instantiate_planner_data( - rclcpp::Node::SharedPtr node, const std::string & map_path, const LaneletRoute & route_msg) + rclcpp::Node::SharedPtr node, const std::string & map_path, + const std::string & sample_planner_data_yaml_path) { lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; @@ -166,20 +223,21 @@ std::shared_ptr instantiate_planner_data( lanelet::utils::conversion::toBinMsg( lanelet_map_ptr, &map_bin); // TODO(soblin): pass lanelet_map_ptr to RouteHandler + YAML::Node config = YAML::LoadFile(sample_planner_data_yaml_path); + auto planner_data = std::make_shared(); planner_data->init_parameters(*node); planner_data->route_handler->setMap(map_bin); - planner_data->route_handler->setRoute(route_msg); + planner_data->route_handler->setRoute( + autoware::test_utils::parse(config["route"])); - nav_msgs::msg::Odometry odom; - odom.pose.pose = route_msg.start_pose; - auto odometry = std::make_shared(odom); + auto odometry = autoware::test_utils::create_const_shared_ptr( + autoware::test_utils::parse(config["self_odometry"])); planner_data->self_odometry = odometry; - geometry_msgs::msg::AccelWithCovarianceStamped accel; - accel.accel.accel.linear.x = 0.537018510955429; - accel.accel.accel.linear.y = -0.2435352815388478; - auto accel_ptr = std::make_shared(accel); + auto accel_ptr = autoware::test_utils::create_const_shared_ptr( + autoware::test_utils::parse( + config["self_acceleration"])); planner_data->self_acceleration = accel_ptr; return planner_data; } @@ -229,7 +287,7 @@ bool hasEnoughDistance( std::vector selectPullOverPaths( const std::vector & pull_over_path_candidates, const GoalCandidates & goal_candidates, const std::shared_ptr planner_data, - const GoalPlannerParameters & parameters, const BehaviorModuleOutput & previous_module_output) + const GoalPlannerParameters & parameters, const BehaviorModuleOutput & upstream_module_output) { using autoware::behavior_path_planner::utils::getExtendedCurrentLanesFromPath; using autoware::motion_utils::calcSignedArcLength; @@ -254,15 +312,15 @@ std::vector selectPullOverPaths( } const double prev_path_front_to_goal_dist = calcSignedArcLength( - previous_module_output.path.points, - previous_module_output.path.points.front().point.pose.position, goal_pose.position); + upstream_module_output.path.points, + upstream_module_output.path.points.front().point.pose.position, goal_pose.position); const auto & long_tail_reference_path = [&]() { if (prev_path_front_to_goal_dist > backward_length) { - return previous_module_output.path; + return upstream_module_output.path; } // get road lanes which is at least backward_length[m] behind the goal const auto road_lanes = getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_length, 0.0, false); + upstream_module_output.path, planner_data, backward_length, 0.0, false); const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; return planner_data->route_handler->getCenterLinePath( road_lanes, std::max(0.0, goal_pose_length - backward_length), @@ -302,7 +360,7 @@ std::vector selectPullOverPaths( std::map path_id_to_rough_margin_map; const auto & target_objects = autoware_perception_msgs::msg::PredictedObjects{}; for (const size_t i : sorted_path_indices) { - const auto & path = pull_over_path_candidates[i]; + const auto & path = pull_over_path_candidates[i]; // cppcheck-suppress containerOutOfBounds const double distance = autoware::behavior_path_planner::utils::path_safety_checker::calculateRoughDistanceToObjects( path.parking_path(), target_objects, planner_data->parameters, false, "max"); @@ -390,9 +448,9 @@ std::vector selectPullOverPaths( } std::optional calculate_centerline_path( - const geometry_msgs::msg::Pose & original_goal_pose, const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) { + const auto original_goal_pose = planner_data->route_handler->getOriginalGoalPose(); const auto refined_goal_opt = autoware::behavior_path_planner::goal_planner_utils::calcRefinedGoal( original_goal_pose, planner_data->route_handler, true, @@ -434,78 +492,45 @@ std::optional calculate_centerline_path( return center_line_path; } +std::vector getBusStopAreaPolygons( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters) +{ + const auto pull_over_lanes = + autoware::behavior_path_planner::goal_planner_utils::getPullOverLanes( + *(planner_data->route_handler), true, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + return autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons( + pull_over_lanes); +} + +struct SortByWeightedDistance +{ + double lateral_cost{0.0}; + bool prioritize_goals_before_objects{false}; + + SortByWeightedDistance(double cost, bool prioritize_goals_before_objects) + : lateral_cost(cost), prioritize_goals_before_objects(prioritize_goals_before_objects) + { + } + + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + if (prioritize_goals_before_objects) { + if (a.num_objects_to_avoid != b.num_objects_to_avoid) { + return a.num_objects_to_avoid < b.num_objects_to_avoid; + } + } + + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < + b.distance_from_original_goal + lateral_cost * b.lateral_offset; + } +}; + int main(int argc, char ** argv) { using autoware::behavior_path_planner::utils::getReferencePath; rclcpp::init(argc, argv); - autoware_planning_msgs::msg::LaneletRoute route_msg; - route_msg.start_pose = - geometry_msgs::build() - .position(geometry_msgs::build().x(729.944).y(695.124).z(381.18)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(0.437138).w( - 0.899395)); - route_msg.goal_pose = - geometry_msgs::build() - .position(geometry_msgs::build().x(797.526).y(694.105).z(381.18)) - .orientation( - geometry_msgs::build().x(0.0).y(0.0).z(-0.658723).w( - 0.752386)); - - route_msg.segments = std::vector{ - autoware_planning_msgs::build() - .preferred_primitive( - autoware_planning_msgs::build() - .id(15214) - .primitive_type("")) - .primitives(std::vector{ - autoware_planning_msgs::build() - .id(15214) - .primitive_type("lane"), - autoware_planning_msgs::build() - .id(15213) - .primitive_type("lane"), - }), - autoware_planning_msgs::build() - .preferred_primitive( - autoware_planning_msgs::build() - .id(15226) - .primitive_type("")) - .primitives(std::vector{ - autoware_planning_msgs::build() - .id(15226) - .primitive_type("lane"), - autoware_planning_msgs::build() - .id(15225) - .primitive_type("lane"), - }), - autoware_planning_msgs::build() - .preferred_primitive( - autoware_planning_msgs::build() - .id(15228) - .primitive_type("")) - .primitives(std::vector{ - autoware_planning_msgs::build() - .id(15228) - .primitive_type("lane"), - autoware_planning_msgs::build() - .id(15229) - .primitive_type("lane"), - }), - autoware_planning_msgs::build() - .preferred_primitive( - autoware_planning_msgs::build() - .id(15231) - .primitive_type("")) - .primitives(std::vector{ - autoware_planning_msgs::build() - .id(15231) - .primitive_type("lane"), - }), - }; - route_msg.allow_modification = false; - auto node_options = rclcpp::NodeOptions{}; node_options.parameter_overrides( std::vector{{"launch_modules", std::vector{}}}); @@ -537,25 +562,34 @@ int main(int argc, char ** argv) node, ament_index_cpp::get_package_share_directory("autoware_test_utils") + "/test_map/road_shoulder/lanelet2_map.osm", - route_msg); + ament_index_cpp::get_package_share_directory("autoware_behavior_path_goal_planner_module") + + "/config/sample_planner_data_case2.yaml"); lanelet::ConstLanelet current_route_lanelet; planner_data->route_handler->getClosestLaneletWithinRoute( - route_msg.start_pose, ¤t_route_lanelet); + planner_data->self_odometry->pose.pose, ¤t_route_lanelet); const auto reference_path = getReferencePath(current_route_lanelet, planner_data); auto goal_planner_parameter = autoware::behavior_path_planner::GoalPlannerModuleManager::initGoalPlannerParameters( node.get(), "goal_planner."); const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); - autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info); autoware::lane_departure_checker::Param lane_departure_checker_params; lane_departure_checker_params.footprint_extra_margin = goal_planner_parameter.lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); + autoware::lane_departure_checker::LaneDepartureChecker lane_departure_checker( + lane_departure_checker_params, vehicle_info); const auto footprint = vehicle_info.createFootprint(); autoware::behavior_path_planner::GoalSearcher goal_searcher(goal_planner_parameter, footprint); - const auto goal_candidates = goal_searcher.search(planner_data); + auto goal_candidates = goal_searcher.search(planner_data); + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance( + goal_planner_parameter.minimum_weighted_distance_lateral_weight, + goal_planner_parameter.prioritize_goals_before_objects)); + std::map goal_id2prio{}; + for (auto i = 0; i < goal_candidates.size(); ++i) { + goal_id2prio[goal_candidates.at(i).id] = i; + } pybind11::scoped_interpreter guard{}; auto plt = matplotlibcpp17::pyplot::import(); @@ -563,42 +597,107 @@ int main(int argc, char ** argv) auto & ax1 = axes[0]; auto & ax2 = axes[1]; - const std::vector ids{15213, 15214, 15225, 15226, 15224, 15227, - 15228, 15229, 15230, 15231, 15232}; + // auto & ax3 = axes[2]; + const std::vector ids{759, 675, 676, 1303, 677, 678}; for (const auto & id : ids) { const auto lanelet = planner_data->route_handler->getLaneletMapPtr()->laneletLayer.get(id); plot_lanelet(ax1, lanelet); plot_lanelet(ax2, lanelet); + // plot_lanelet(ax3, lanelet); } - plot_goal_candidates(ax1, goal_candidates, footprint); + // plot_goal_candidates(ax1, goal_candidates, footprint); - plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); + // plot_path_with_lane_id(ax2, reference_path.path, "green", "reference_path"); + const auto start = std::chrono::steady_clock::now(); std::vector candidates; - for (const auto & goal_candidate : goal_candidates) { - auto shift_pull_over_planner = autoware::behavior_path_planner::ShiftPullOver( + for (auto i = 0; i < goal_candidates.size(); ++i) { + const auto & goal_candidate = goal_candidates.at(i); + auto shift_pull_over_planner = autoware::behavior_path_planner::BezierPullOver( *node, goal_planner_parameter, lane_departure_checker); - const auto pull_over_path_opt = - shift_pull_over_planner.plan(goal_candidate, 0, planner_data, reference_path); - if (pull_over_path_opt) { - const auto & pull_over_path = pull_over_path_opt.value(); - const auto & full_path = pull_over_path.full_path(); - candidates.push_back(pull_over_path); - plot_path_with_lane_id(ax1, full_path); + auto pull_over_paths = + shift_pull_over_planner.plans(goal_candidate, 0, planner_data, reference_path); + if (!pull_over_paths.empty()) { + std::copy( + std::make_move_iterator(pull_over_paths.begin()), + std::make_move_iterator(pull_over_paths.end()), std::back_inserter(candidates)); } } - [[maybe_unused]] const auto filtered_paths = selectPullOverPaths( + + const auto filtered_paths = selectPullOverPaths( candidates, goal_candidates, planner_data, goal_planner_parameter, reference_path); + std::cout << filtered_paths.size() << std::endl; + const auto end = std::chrono::steady_clock::now(); + std::cout << "computed candidate bezier paths in " + << std::chrono::duration_cast(end - start).count() * 1.0 / + 1000000 + << "msecs" << std::endl; + std::cout << "filtered " << filtered_paths.size() << "/" << candidates.size() << " paths" + << std::endl; + + /* + for (auto i = 0; i < filtered_paths.size(); ++i) { + const auto & filtered_path = filtered_paths.at(i); + const auto goal_id = filtered_path.goal_id(); + const auto prio = goal_id2prio[goal_id]; + const auto & color = (i == 0) ? "red" : g_colors.at(i % g_colors.size()); + const auto max_parking_curvature = filtered_path.parking_path_max_curvature(); + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + if (i == 0) { + plot_path_with_lane_id(ax1, filtered_path.parking_path(), color, "most prio", 2.0); + } else { + plot_path_with_lane_id( + ax1, filtered_path.full_path(), color, + std::to_string(prio) + "-th goal(id=" + std::to_string(goal_id) + + "): k_max=" + std::to_string(max_parking_curvature), + 0.5); + } + } + */ + const auto original_goal_pos = planner_data->route_handler->getOriginalGoalPose().position; + ax1.plot( + Args(original_goal_pos.x, original_goal_pos.y), + Kwargs("marker"_a = "x", "label"_a = "goal", "markersize"_a = 20, "color"_a = "red")); + if (goal_planner_parameter.bus_stop_area.use_bus_stop_area) { + const auto bus_stop_area_polygons = + getBusStopAreaPolygons(planner_data, goal_planner_parameter); + for (const auto & bus_stop_area_polygon : bus_stop_area_polygons) { + plot_lanelet_polygon(ax1, bus_stop_area_polygon); + } + } - const auto centerline_path = - calculate_centerline_path(route_msg.goal_pose, planner_data, goal_planner_parameter); + for (auto i = 0; i < filtered_paths.size(); ++i) { + const auto & filtered_path = filtered_paths.at(i); + const auto goal_id = filtered_path.goal_id(); + const auto prio = goal_id2prio[goal_id]; + const auto & color = (i == 0) ? "red" : g_colors.at(i % g_colors.size()); + const auto max_parking_curvature = filtered_path.parking_path_max_curvature(); + if (i == 0) { + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + plot_path_with_lane_id(ax2, filtered_path.full_path(), color, "most prio", 2.0); + for (const auto & path_point : filtered_path.full_path().points) { + const auto pose_footprint = transformVector( + footprint, autoware::universe_utils::pose2transform(path_point.point.pose)); + plot_footprint(ax2, pose_footprint, "blue"); + } + } else if (i % 50 == 0) { + std::cout << "plotting " << i << "-th filtered path" << std::endl; + plot_goal_candidate(ax1, filtered_path.modified_goal(), prio, footprint, color); + plot_path_with_lane_id(ax1, filtered_path.full_path(), color, "", 2.0); + } + } + ax2.plot( + Args(original_goal_pos.x, original_goal_pos.y), + Kwargs("marker"_a = "x", "label"_a = "goal", "markersize"_a = 20, "color"_a = "red")); + + const auto centerline_path = calculate_centerline_path(planner_data, goal_planner_parameter); if (centerline_path) { - plot_path_with_lane_id(ax2, centerline_path.value(), "red", "centerline_path"); + // plot_path_with_lane_id(ax2, centerline_path.value(), "red", "centerline_path"); } + ax1.set_aspect(Args("equal")); ax2.set_aspect(Args("equal")); - ax1.legend(); ax2.legend(); plt.show(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index f670e3b05fa77..201b7c2d33bf6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -38,15 +38,15 @@ class FixedGoalPlannerBase virtual BehaviorModuleOutput plan( const std::shared_ptr & planner_data) const = 0; - void setPreviousModuleOutput(const BehaviorModuleOutput & previous_module_output) + void setPreviousModuleOutput(const BehaviorModuleOutput & upstream_module_output) { - previous_module_output_ = previous_module_output; + upstream_module_output_ = upstream_module_output; } - BehaviorModuleOutput getPreviousModuleOutput() const { return previous_module_output_; } + BehaviorModuleOutput getPreviousModuleOutput() const { return upstream_module_output_; } protected: - BehaviorModuleOutput previous_module_output_; + BehaviorModuleOutput upstream_module_output_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index 5e6afa16d1f9f..f2cc7e2f4ef65 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -18,14 +18,12 @@ #include "autoware/behavior_path_goal_planner_module/decision_state.hpp" #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.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/pull_over_planner/bezier_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/thread_data.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/path_safety_checker/path_safety_checker_parameters.hpp" -#include - #include #include @@ -41,7 +39,6 @@ namespace autoware::behavior_path_planner { -using autoware::lane_departure_checker::LaneDepartureChecker; using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; using nav_msgs::msg::OccupancyGrid; @@ -89,27 +86,44 @@ struct PullOverContextData PullOverContextData() = delete; explicit PullOverContextData( const bool is_stable_safe_path, const PredictedObjects & static_objects, - const PredictedObjects & dynamic_objects, std::optional && pull_over_path_opt, - const std::vector & pull_over_path_candidates, - const PathDecisionState & prev_state) + const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state, + const bool is_stopped, LaneParkingResponse && lane_parking_response, + FreespaceParkingResponse && freespace_parking_response) : is_stable_safe_path(is_stable_safe_path), static_target_objects(static_objects), dynamic_target_objects(dynamic_objects), - pull_over_path_opt(pull_over_path_opt), - pull_over_path_candidates(pull_over_path_candidates), prev_state_for_debug(prev_state), - last_path_idx_increment_time(std::nullopt) + is_stopped(is_stopped), + lane_parking_response(lane_parking_response), + freespace_parking_response(freespace_parking_response) { } - const bool is_stable_safe_path; - const PredictedObjects static_target_objects; - const PredictedObjects dynamic_target_objects; - // TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it) + // TODO(soblin): make following variables private + bool is_stable_safe_path; + PredictedObjects static_target_objects; + PredictedObjects dynamic_target_objects; + PathDecisionState prev_state_for_debug; + bool is_stopped; + LaneParkingResponse lane_parking_response; + FreespaceParkingResponse freespace_parking_response; std::optional pull_over_path_opt; - const std::vector pull_over_path_candidates; - // TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose - const PathDecisionState prev_state_for_debug; + std::optional last_path_update_time; std::optional last_path_idx_increment_time; + + void update( + const bool is_stable_safe_path_, const PredictedObjects static_target_objects_, + const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_, + const bool is_stopped_, LaneParkingResponse && lane_parking_response_, + FreespaceParkingResponse && freespace_parking_response_) + { + is_stable_safe_path = is_stable_safe_path_; + static_target_objects = static_target_objects_; + dynamic_target_objects = dynamic_target_objects_; + prev_state_for_debug = prev_state_for_debug_; + is_stopped = is_stopped_; + lane_parking_response = lane_parking_response_; + freespace_parking_response = freespace_parking_response_; + } }; bool isOnModifiedGoal( @@ -117,12 +131,10 @@ bool isOnModifiedGoal( const GoalPlannerParameters & parameters); bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output); -bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output); -bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output); + const BehaviorModuleOutput & upstream_module_output, + const BehaviorModuleOutput & last_upstream_module_output); +bool hasDeviatedFromPath( + const Point & ego_position, const BehaviorModuleOutput & upstream_module_output); bool needPathUpdate( const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now, @@ -133,11 +145,22 @@ bool checkOccupancyGridCollision( const PathWithLaneId & path, const std::shared_ptr occupancy_grid_map); +// freespace parking +std::optional planFreespacePath( + const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects, + std::shared_ptr freespace_planner); + bool isStopped( std::deque & odometry_buffer, const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, const double velocity_upper); +void sortPullOverPaths( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects, + rclcpp::Logger logger, std::vector & sorted_path_indices); + // Flag class for managing whether a certain callback is running in multi-threading class ScopedFlag { @@ -150,6 +173,81 @@ class ScopedFlag std::atomic & flag_; }; +class LaneParkingPlanner +{ +public: + LaneParkingPlanner( + rclcpp::Node & node, std::mutex & lane_parking_mutex, + const std::optional & request, LaneParkingResponse & response, + std::atomic & is_lane_parking_cb_running, const rclcpp::Logger & logger, + const GoalPlannerParameters & parameters); + rclcpp::Logger getLogger() const { return logger_; } + void onTimer(); + +private: + const GoalPlannerParameters parameters_; + std::mutex & mutex_; + const std::optional & request_; + LaneParkingResponse & response_; + std::atomic & is_lane_parking_cb_running_; + rclcpp::Logger logger_; + + std::vector> pull_over_planners_; + BehaviorModuleOutput + original_upstream_module_output_; // bezier_pull_over_planner_; + const double pull_over_angle_threshold; + + bool switch_bezier_{false}; + void normal_pullover_planning_helper( + const std::shared_ptr planner_data, const GoalCandidates & goal_candidates, + const BehaviorModuleOutput & upstream_module_output, + const lanelet::ConstLanelets current_lanelets, std::optional & closest_start_pose, + std::vector & path_candidates); + void bezier_planning_helper( + const std::shared_ptr planner_data, const GoalCandidates & goal_candidates, + const BehaviorModuleOutput & upstream_module_output, + const lanelet::ConstLanelets current_lanelets, std::optional & closest_start_pose, + std::vector & path_candidates, + std::optional> & sorted_indices) const; +}; + +class FreespaceParkingPlanner +{ +public: + FreespaceParkingPlanner( + std::mutex & freespace_parking_mutex, const std::optional & request, + FreespaceParkingResponse & response, std::atomic & is_freespace_parking_cb_running, + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr freespace_planner) + : mutex_(freespace_parking_mutex), + request_(request), + response_(response), + is_freespace_parking_cb_running_(is_freespace_parking_cb_running), + logger_(logger), + clock_(clock), + freespace_planner_(freespace_planner) + { + } + void onTimer(); + +private: + std::mutex & mutex_; + const std::optional & request_; + FreespaceParkingResponse & response_; + std::atomic & is_freespace_parking_cb_running_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + std::shared_ptr freespace_planner_; + + bool isStuck( + const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, + const FreespaceParkingRequest & req) const; +}; + class GoalPlannerModule : public SceneModuleInterface { public: @@ -158,7 +256,8 @@ class GoalPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); ~GoalPlannerModule() { @@ -217,48 +316,17 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: - /** - * @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this) - */ - struct GoalPlannerData - { - GoalPlannerData( - const PlannerData & planner_data, const GoalPlannerParameters & parameters, - const BehaviorModuleOutput & previous_module_output_) - { - initializeOccupancyGridMap(planner_data, parameters); - previous_module_output = previous_module_output_; - last_previous_module_output = previous_module_output_; - }; - GoalPlannerParameters parameters; - autoware::universe_utils::LinearRing2d vehicle_footprint; - - PlannerData planner_data; - ModuleStatus current_status; - BehaviorModuleOutput previous_module_output; - BehaviorModuleOutput last_previous_module_output; // occupancy_grid_map; - - const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; } - const ModuleStatus & getCurrentStatus() const { return current_status; } - void updateOccupancyGrid(); - GoalPlannerData clone() const; - void update( - const GoalPlannerParameters & parameters, const PlannerData & planner_data, - const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output, - const autoware::universe_utils::LinearRing2d & vehicle_footprint, - const GoalCandidates & goal_candidates); - - private: - void initializeOccupancyGridMap( - const PlannerData & planner_data, const GoalPlannerParameters & parameters); - }; - std::optional gp_planner_data_{std::nullopt}; - std::mutex gp_planner_data_mutex_; + std::pair syncWithThreads(); + + bool trigger_thread_on_approach_{false}; + // NOTE: never access to following variables except in updateData()!!! + std::mutex lane_parking_mutex_; + std::optional lane_parking_request_; + LaneParkingResponse lane_parking_response_; + + std::mutex freespace_parking_mutex_; + std::optional freespace_parking_request_; + FreespaceParkingResponse freespace_parking_response_; /* * state transitions and plan function used in each state @@ -297,9 +365,6 @@ class GoalPlannerModule : public SceneModuleInterface autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; - // planner - std::vector> pull_over_planners_; - std::unique_ptr freespace_planner_; std::unique_ptr fixed_goal_planner_; // goal searcher @@ -317,11 +382,6 @@ class GoalPlannerModule : public SceneModuleInterface autoware::universe_utils::LinearRing2d vehicle_footprint_; - std::recursive_mutex mutex_; - mutable ThreadSafeData thread_safe_data_; - - // TODO(soblin): organize part of thread_safe_data and previous data to PullOverContextData - // context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess() std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() PathDecisionStateController path_decision_controller_{getLogger()}; @@ -369,12 +429,7 @@ class GoalPlannerModule : public SceneModuleInterface // status bool hasFinishedCurrentPath(const PullOverContextData & ctx_data); double calcModuleRequestLength() const; - bool isStuck( - const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters); - void decideVelocity(); + void decideVelocity(PullOverPath & pull_over_path); void updateStatus(const BehaviorModuleOutput & output); // validation @@ -385,25 +440,18 @@ class GoalPlannerModule : public SceneModuleInterface bool isCrossingPossible( const Pose & start_pose, const Pose & end_pose, const lanelet::ConstLanelets lanes) const; bool isCrossingPossible(const PullOverPath & pull_over_path) const; - bool hasEnoughTimePassedSincePathUpdate(const double duration) const; - - // freespace parking - bool planFreespacePath( - std::shared_ptr planner_data, - const std::shared_ptr goal_searcher, const GoalCandidates & goal_candidates, - const std::shared_ptr occupancy_grid_map, - const PredictedObjects & static_target_objects); bool canReturnToLaneParking(const PullOverContextData & context_data); // plan pull over path - BehaviorModuleOutput planPullOver(const PullOverContextData & context_data); - BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data); + BehaviorModuleOutput planPullOver(PullOverContextData & context_data); + BehaviorModuleOutput planPullOverAsOutput(PullOverContextData & context_data); BehaviorModuleOutput planPullOverAsCandidate( - const PullOverContextData & context_data, const std::string & detail); + PullOverContextData & context_data, const std::string & detail); std::optional selectPullOverPath( const PullOverContextData & context_data, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const; + const GoalCandidates & goal_candidates, + const std::optional> sorted_bezier_indices_opt) const; // lanes and drivable area std::vector generateDrivableLanes() const; @@ -411,7 +459,9 @@ class GoalPlannerModule : public SceneModuleInterface const PullOverContextData & context_data, BehaviorModuleOutput & output) const; // output setter - void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output); + void setOutput( + const std::optional selected_pull_over_path_with_velocity_opt, + const PullOverContextData & context_data, BehaviorModuleOutput & output); void setModifiedGoal( const PullOverContextData & context_data, BehaviorModuleOutput & output) const; @@ -421,14 +471,10 @@ class GoalPlannerModule : public SceneModuleInterface TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data); std::optional ignore_signal_{std::nullopt}; - // timer for generating pull over path candidates in a separate thread - void onTimer(); - void onFreespaceParkingTimer(); - // steering factor void updateSteeringFactor( const PullOverContextData & context_data, const std::array & pose, - const std::array distance, const uint16_t type); + const std::array distance); // rtc std::pair calcDistanceToPathChange( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 440e57b53d07f..814cffd388703 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -88,6 +88,7 @@ struct GoalPlannerParameters // pull over general params double pull_over_minimum_request_length{0.0}; + double pull_over_prepare_length{0.0}; double pull_over_velocity{0.0}; double pull_over_minimum_velocity{0.0}; double decide_path_distance{0.0}; @@ -119,6 +120,12 @@ struct GoalPlannerParameters AstarParam astar_parameters{}; RRTStarParam rrt_star_parameters{}; + struct BezierParking + { + double pull_over_angle_threshold; + double after_shift_straight_distance; + } bezier_parking; + // stop condition double maximum_deceleration_for_stop{0.0}; double maximum_jerk_for_stop{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp new file mode 100644 index 0000000000000..9b97f87c933a8 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/bezier_pull_over.hpp @@ -0,0 +1,64 @@ +// 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_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__BEZIER_PULL_OVER_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__BEZIER_PULL_OVER_HPP_ + +#include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" + +#include + +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::lane_departure_checker::LaneDepartureChecker; + +class BezierPullOver : public PullOverPlannerBase +{ +public: + BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); + PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::BEZIER; } + std::optional plan( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & upstream_module_output) override; + std::vector plans( + const GoalCandidate & modified_goal_pose, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & upstream_module_output); + +private: + const LaneDepartureChecker lane_departure_checker_; + + const bool left_side_parking_; + + std::vector generateBezierPath( + const GoalCandidate & goal_candidate, const size_t id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const; + + PathWithLaneId generateReferencePath( + const std::shared_ptr planner_data, + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; + + static double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); +}; + +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER__BEZIER_PULL_OVER_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp index ee370fd96b9a0..06ff47224dd44 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/freespace_pull_over.hpp @@ -30,16 +30,16 @@ using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; class FreespacePullOver : public PullOverPlannerBase { public: - FreespacePullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } + void setMap(const nav_msgs::msg::OccupancyGrid & costmap) { planner_->setMap(costmap); } + std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; protected: const double velocity_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp index 89181b258fbea..e12a759eb2137 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp @@ -32,8 +32,7 @@ class GeometricPullOver : public PullOverPlannerBase { public: GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, const bool is_forward); + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward); PullOverPlannerType getPlannerType() const override { @@ -45,7 +44,7 @@ class GeometricPullOver : public PullOverPlannerBase std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; std::vector generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp index e061a0203602f..7f404a4055b26 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp @@ -33,6 +33,7 @@ namespace autoware::behavior_path_planner { enum class PullOverPlannerType { SHIFT, + BEZIER, ARC_FORWARD, ARC_BACKWARD, FREESPACE, @@ -67,6 +68,10 @@ struct PullOverPath const std::vector & parking_path_curvatures() const { return parking_path_curvatures_; } double full_path_max_curvature() const { return full_path_max_curvature_; } double parking_path_max_curvature() const { return parking_path_max_curvature_; } + double parking_path_curvature_total_derivative() const + { + return parking_path_curvature_total_derivative_; + } size_t path_idx() const { return path_idx_; } bool incrementPathIndex(); @@ -93,7 +98,7 @@ struct PullOverPath const PathWithLaneId & full_path, const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, const std::vector & parking_path_curvatures, const double full_path_max_curvature, - const double parking_path_max_curvature, + const double parking_path_max_curvature, const double parking_path_curvature_total_derivative, const std::vector> & pairs_terminal_velocity_and_accel); PullOverPlannerType type_; @@ -108,6 +113,7 @@ struct PullOverPath std::vector parking_path_curvatures_; double full_path_max_curvature_; double parking_path_max_curvature_; + double parking_path_curvature_total_derivative_; // accelerate with constant acceleration to the target velocity size_t path_idx_; @@ -129,7 +135,7 @@ class PullOverPlannerBase virtual std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) = 0; + const BehaviorModuleOutput & upstream_module_output) = 0; protected: const autoware::vehicle_info_utils::VehicleInfo vehicle_info_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp index 08e0b8508c991..994d8283fe36c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp @@ -31,14 +31,12 @@ using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOver : public PullOverPlannerBase { public: - ShiftPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker); + ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; std::optional plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) override; + const BehaviorModuleOutput & upstream_module_output) override; protected: PathWithLaneId generateReferencePath( @@ -49,7 +47,7 @@ class ShiftPullOver : public PullOverPlannerBase std::optional generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index 549319b42ee4c..af20c5f6debe5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -16,143 +16,118 @@ #define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__THREAD_DATA_HPP_ #include "autoware/behavior_path_goal_planner_module/decision_state.hpp" -#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include #include -#include #include +#include #include namespace autoware::behavior_path_planner { -#define DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - void set_##NAME(const TYPE & value) \ - { \ - const std::lock_guard lock(mutex_); \ - NAME##_ = value; \ - } - -#define DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) \ -public: \ - TYPE get_##NAME() const \ - { \ - const std::lock_guard lock(mutex_); \ - return NAME##_; \ - } - -#define DEFINE_SETTER_GETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_SETTER_WITH_MUTEX(TYPE, NAME) \ - DEFINE_GETTER_WITH_MUTEX(TYPE, NAME) - -class ThreadSafeData +class LaneParkingRequest { public: - ThreadSafeData(std::recursive_mutex & mutex, rclcpp::Clock::SharedPtr clock) - : mutex_(mutex), clock_(clock) + LaneParkingRequest( + const autoware::universe_utils::LinearRing2d & vehicle_footprint, + const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output) + : vehicle_footprint_(vehicle_footprint), + goal_candidates_(goal_candidates), + upstream_module_output_(upstream_module_output) { } - bool incrementPathIndex() - { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } + void update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const BehaviorModuleOutput & upstream_module_output, + const std::optional & pull_over_path, const PathDecisionState & prev_data, + const bool trigger_thread_on_approach); - return pull_over_path_->incrementPathIndex(); - } + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const GoalCandidates goal_candidates_; - void set_pull_over_path(const PullOverPath & path) + const std::shared_ptr & get_planner_data() const { return planner_data_; } + const ModuleStatus & get_current_status() const { return current_status_; } + const BehaviorModuleOutput & get_upstream_module_output() const { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); + return upstream_module_output_; } + const std::optional & get_pull_over_path() const { return pull_over_path_; } + const PathDecisionState & get_prev_data() const { return prev_data_; } + bool trigger_thread_on_approach() const { return trigger_thread_on_approach_; } - void set_pull_over_path(const std::shared_ptr & path) - { - const std::lock_guard lock(mutex_); - set_pull_over_path_no_lock(path); - } +private: + std::shared_ptr planner_data_; + ModuleStatus current_status_; + BehaviorModuleOutput upstream_module_output_; + std::optional pull_over_path_; // - void set(Args... args) - { - std::lock_guard lock(mutex_); - (..., set_no_lock(args)); - } +struct LaneParkingResponse +{ + std::vector pull_over_path_candidates; + std::optional closest_start_pose; + std::optional> sorted_bezier_indices_opt; +}; - void clearPullOverPath() +class FreespaceParkingRequest +{ +public: + FreespaceParkingRequest( + const GoalPlannerParameters & parameters, + const autoware::universe_utils::LinearRing2d & vehicle_footprint, + const GoalCandidates & goal_candidates, const PlannerData & planner_data) + : parameters_(parameters), + vehicle_footprint_(vehicle_footprint), + goal_candidates_(goal_candidates) { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - } - - bool foundPullOverPath() const + initializeOccupancyGridMap(planner_data, parameters_); + }; + + const ModuleStatus & getCurrentStatus() const { return current_status_; } + void update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const std::optional & pull_over_path, + const std::optional & last_path_update_time, const bool is_stopped); + + const GoalPlannerParameters parameters_; + const autoware::universe_utils::LinearRing2d vehicle_footprint_; + const GoalCandidates goal_candidates_; + + const std::shared_ptr & get_planner_data() const { return planner_data_; } + const ModuleStatus & get_current_status() const { return current_status_; } + std::shared_ptr get_occupancy_grid_map() const { - const std::lock_guard lock(mutex_); - if (!pull_over_path_) { - return false; - } - - return true; + return occupancy_grid_map_; } - - void reset() + const std::optional & get_pull_over_path() const { return pull_over_path_; } + const std::optional & get_last_path_update_time() const { - const std::lock_guard lock(mutex_); - pull_over_path_ = nullptr; - pull_over_path_candidates_.clear(); - last_path_update_time_ = std::nullopt; - closest_start_pose_ = std::nullopt; - prev_data_ = PathDecisionState{}; + return last_path_update_time_; } - - // main --> lane - DEFINE_SETTER_GETTER_WITH_MUTEX(PathDecisionState, prev_data) - - // lane --> main - DEFINE_SETTER_GETTER_WITH_MUTEX(std::optional, closest_start_pose) - DEFINE_SETTER_GETTER_WITH_MUTEX(std::vector, pull_over_path_candidates) - - // main <--> lane/freespace - DEFINE_GETTER_WITH_MUTEX(std::shared_ptr, pull_over_path) - DEFINE_GETTER_WITH_MUTEX(std::optional, last_path_update_time) + bool is_stopped() const { return is_stopped_; } private: - void set_pull_over_path_no_lock(const PullOverPath & path) - { - pull_over_path_ = std::make_shared(path); - - last_path_update_time_ = clock_->now(); - } - - void set_pull_over_path_no_lock(const std::shared_ptr & path) - { - pull_over_path_ = path; - last_path_update_time_ = clock_->now(); - } - - void set_no_lock(const std::vector & arg) { pull_over_path_candidates_ = arg; } - void set_no_lock(const std::shared_ptr & arg) { set_pull_over_path_no_lock(arg); } - void set_no_lock(const PullOverPath & arg) { set_pull_over_path_no_lock(arg); } - - std::shared_ptr pull_over_path_{nullptr}; - std::vector pull_over_path_candidates_; + std::shared_ptr planner_data_; + ModuleStatus current_status_; + std::shared_ptr occupancy_grid_map_; + std::optional pull_over_path_; std::optional last_path_update_time_; - std::optional closest_start_pose_{}; - PathDecisionState prev_data_{}; + bool is_stopped_; - std::recursive_mutex & mutex_; - rclcpp::Clock::SharedPtr clock_; + void initializeOccupancyGridMap( + const PlannerData & planner_data, const GoalPlannerParameters & parameters); }; -#undef DEFINE_SETTER_WITH_MUTEX -#undef DEFINE_GETTER_WITH_MUTEX -#undef DEFINE_SETTER_GETTER_WITH_MUTEX +struct FreespaceParkingResponse +{ + std::optional freespace_pull_over_path; +}; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index f00d9d41622e9..7f375da295fb9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -185,6 +185,15 @@ std::optional calcRefinedGoal( std::optional calcClosestPose( const lanelet::ConstLineString3d line, const Point & query_point); +autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects( + const autoware_perception_msgs::msg::PredictedObjects & original_objects, + const route_handler::RouteHandler & route_handler, const GoalPlannerParameters & parameters, + const double vehicle_width); + +bool is_goal_reachable_on_path( + const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking); + } // namespace autoware::behavior_path_planner::goal_planner_utils #endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index dead1e4fe0f37..feb42cdee92b6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_goal_planner_module - 0.40.0 + 0.41.0 The autoware_behavior_path_goal_planner_module package Kosuke Takeuchi @@ -22,8 +22,11 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_bezier_sampler + autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface + autoware_test_utils autoware_universe_utils pluginlib rclcpp @@ -34,7 +37,6 @@ ament_index_cpp ament_lint_auto autoware_lint_common - autoware_test_utils ament_cmake diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index e1f667d91b692..448429b9a3b0b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -15,7 +15,7 @@ #include "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/pull_over_planner/freespace_pull_over.hpp" +#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/geometric_pull_over.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/pull_over_planner_base.hpp" #include "autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp" @@ -29,6 +29,8 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include +#include #include #include #include @@ -38,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -66,21 +69,14 @@ GoalPlannerModule::GoalPlannerModule( 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 + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters}, 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} { - LaneDepartureChecker lane_departure_checker{}; - lane_departure_checker.setVehicleInfo(vehicle_info_); - lane_departure_checker::Param lane_departure_checker_params; - lane_departure_checker_params.footprint_extra_margin = - parameters->lane_departure_check_expansion_margin; - lane_departure_checker.setParam(lane_departure_checker_params); - occupancy_grid_map_ = std::make_shared(); left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; @@ -88,26 +84,6 @@ GoalPlannerModule::GoalPlannerModule( // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); - for (const std::string & planner_type : parameters_->efficient_path_order) { - if (planner_type == "SHIFT" && parameters_->enable_shift_parking) { - pull_over_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker)); - } else if (planner_type == "ARC_FORWARD" && parameters_->enable_arc_forward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, /*is_forward*/ true)); - } else if (planner_type == "ARC_BACKWARD" && parameters_->enable_arc_backward_parking) { - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, /*is_forward*/ false)); - } - } - - if (pull_over_planners_.empty()) { - RCLCPP_WARN( - getLogger(), - "No enabled planner found. The vehicle will stop in the road lane without pull over. Please " - "check the parameters if this is the intended behavior."); - } - // set selected goal searcher // currently there is only one goal_searcher_type const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); @@ -119,18 +95,27 @@ GoalPlannerModule::GoalPlannerModule( lane_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); lane_parking_timer_ = rclcpp::create_timer( - &node, clock_, lane_parking_period_ns, std::bind(&GoalPlannerModule::onTimer, this), + &node, clock_, lane_parking_period_ns, + [lane_parking_executor = std::make_unique( + node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_, + is_lane_parking_cb_running_, getLogger(), *parameters_)]() { + lane_parking_executor->onTimer(); + }, lane_parking_timer_cb_group_); // freespace parking if (parameters_->enable_freespace_parking) { - freespace_planner_ = std::make_unique(node, *parameters, vehicle_info); + auto freespace_planner = std::make_shared(node, *parameters); const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); freespace_parking_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); freespace_parking_timer_ = rclcpp::create_timer( &node, clock_, freespace_parking_period_ns, - std::bind(&GoalPlannerModule::onFreespaceParkingTimer, this), + [freespace_parking_executor = std::make_unique( + freespace_parking_mutex_, freespace_parking_request_, freespace_parking_response_, + is_freespace_parking_cb_running_, getLogger(), clock_, freespace_planner)]() { + freespace_parking_executor->onTimer(); + }, freespace_parking_timer_cb_group_); } @@ -139,9 +124,6 @@ GoalPlannerModule::GoalPlannerModule( initializeSafetyCheckParameters(); } - steering_factor_interface_.init(PlanningBehavior::GOAL_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::GOAL_PLANNER); - /** * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called * from the main thread only. @@ -169,17 +151,17 @@ bool isOnModifiedGoal( } bool hasPreviousModulePathShapeChanged( - const BehaviorModuleOutput & previous_module_output, - const BehaviorModuleOutput & last_previous_module_output) + const BehaviorModuleOutput & upstream_module_output, + const BehaviorModuleOutput & last_upstream_module_output) { // Calculate the lateral distance between each point of the current path and the nearest point of // the last path - constexpr double LATERAL_DEVIATION_THRESH = 0.3; - for (const auto & p : previous_module_output.path.points) { + constexpr double LATERAL_DEVIATION_THRESH = 0.1; + for (const auto & p : upstream_module_output.path.points) { const size_t nearest_seg_idx = autoware::motion_utils::findNearestSegmentIndex( - last_previous_module_output.path.points, p.point.pose.position); - const auto seg_front = last_previous_module_output.path.points.at(nearest_seg_idx); - const auto seg_back = last_previous_module_output.path.points.at(nearest_seg_idx + 1); + last_upstream_module_output.path.points, p.point.pose.position); + const auto seg_front = last_upstream_module_output.path.points.at(nearest_seg_idx); + const auto seg_back = last_upstream_module_output.path.points.at(nearest_seg_idx + 1); // Check if the target point is within the segment const Eigen::Vector3d segment_vec{ seg_back.point.pose.position.x - seg_front.point.pose.position.x, @@ -195,7 +177,7 @@ bool hasPreviousModulePathShapeChanged( continue; } const double lateral_distance = std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output.path.points, p.point.pose.position, nearest_seg_idx)); + last_upstream_module_output.path.points, p.point.pose.position, nearest_seg_idx)); if (lateral_distance > LATERAL_DEVIATION_THRESH) { return true; } @@ -203,21 +185,12 @@ bool hasPreviousModulePathShapeChanged( return false; } -bool hasDeviatedFromLastPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output) -{ - return std::abs(autoware::motion_utils::calcLateralOffset( - last_previous_module_output.path.points, - planner_data.self_odometry->pose.pose.position)) > 0.3; -} - -bool hasDeviatedFromCurrentPreviousModulePath( - const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output) +bool hasDeviatedFromPath( + const Point & ego_position, const BehaviorModuleOutput & upstream_module_output) { - constexpr double LATERAL_DEVIATION_THRESH = 0.3; + constexpr double LATERAL_DEVIATION_THRESH = 0.1; return std::abs(autoware::motion_utils::calcLateralOffset( - previous_module_output.path.points, planner_data.self_odometry->pose.pose.position)) > - LATERAL_DEVIATION_THRESH; + upstream_module_output.path.points, ego_position)) > LATERAL_DEVIATION_THRESH; } bool needPathUpdate( @@ -241,6 +214,35 @@ bool checkOccupancyGridCollision( return occupancy_grid_map->hasObstacleOnPath(path, check_out_of_range); } +std::optional planFreespacePath( + const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects, + std::shared_ptr freespace_planner) +{ + auto goal_candidates = req.goal_candidates_; + auto goal_searcher = std::make_shared(req.parameters_, req.vehicle_footprint_); + goal_searcher->update( + goal_candidates, req.get_occupancy_grid_map(), req.get_planner_data(), static_target_objects); + + for (size_t i = 0; i < goal_candidates.size(); i++) { + const auto goal_candidate = goal_candidates.at(i); + + if (!goal_candidate.is_safe) { + continue; + } + + freespace_planner->setMap(*(req.get_planner_data()->costmap)); + const auto freespace_path = freespace_planner->plan( + goal_candidate, 0, req.get_planner_data(), BehaviorModuleOutput{} + // NOTE: not used so passing {} is OK + ); + if (!freespace_path) { + continue; + } + return freespace_path; + } + return std::nullopt; +} + bool isStopped( std::deque & odometry_buffer, const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower, @@ -267,52 +269,67 @@ bool isStopped( return is_stopped; } +LaneParkingPlanner::LaneParkingPlanner( + rclcpp::Node & node, std::mutex & lane_parking_mutex, + const std::optional & request, LaneParkingResponse & response, + std::atomic & is_lane_parking_cb_running, const rclcpp::Logger & logger, + const GoalPlannerParameters & parameters) +: parameters_(parameters), + mutex_(lane_parking_mutex), + request_(request), + response_(response), + is_lane_parking_cb_running_(is_lane_parking_cb_running), + logger_(logger), + pull_over_angle_threshold(parameters.bezier_parking.pull_over_angle_threshold) +{ + for (const std::string & planner_type : parameters.efficient_path_order) { + if (planner_type == "SHIFT" && parameters.enable_shift_parking) { + pull_over_planners_.push_back(std::make_shared(node, parameters)); + } else if (planner_type == "ARC_FORWARD" && parameters.enable_arc_forward_parking) { + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ true)); + } else if (planner_type == "ARC_BACKWARD" && parameters.enable_arc_backward_parking) { + pull_over_planners_.push_back( + std::make_shared(node, parameters, /*is_forward*/ false)); + } + } + + bezier_pull_over_planner_ = std::make_shared(node, parameters); + + if (pull_over_planners_.empty()) { + RCLCPP_ERROR(logger_, "Not found enabled planner"); + } +} + // generate pull over candidate paths -void GoalPlannerModule::onTimer() +void LaneParkingPlanner::onTimer() { const ScopedFlag flag(is_lane_parking_cb_running_); - std::shared_ptr local_planner_data{nullptr}; - std::optional current_status_opt{std::nullopt}; - std::optional previous_module_output_opt{std::nullopt}; - std::optional last_previous_module_output_opt{std::nullopt}; - std::shared_ptr occupancy_grid_map{nullptr}; - std::optional parameters_opt{std::nullopt}; - std::optional goal_candidates_opt{std::nullopt}; + std::optional local_request_opt; // begin of critical section { - std::lock_guard guard(gp_planner_data_mutex_); - if (gp_planner_data_) { - const auto & gp_planner_data = gp_planner_data_.value(); - local_planner_data = std::make_shared(gp_planner_data.planner_data); - current_status_opt = gp_planner_data.current_status; - previous_module_output_opt = gp_planner_data.previous_module_output; - last_previous_module_output_opt = gp_planner_data.last_previous_module_output; - occupancy_grid_map = gp_planner_data.occupancy_grid_map; - parameters_opt = gp_planner_data.parameters; - goal_candidates_opt = gp_planner_data.goal_candidates; + std::lock_guard guard(mutex_); + if (request_) { + auto & request = request_.value(); + local_request_opt.emplace(request); } } // end of critical section - if ( - !local_planner_data || !current_status_opt || !previous_module_output_opt || - !last_previous_module_output_opt || !occupancy_grid_map || !parameters_opt || - !goal_candidates_opt) { - RCLCPP_INFO_THROTTLE( - getLogger(), *clock_, 5000, - "failed to get valid " - "local_planner_data/current_status/previous_module_output/occupancy_grid_map/parameters_opt " - "in onTimer"); + if (!local_request_opt) { + RCLCPP_DEBUG(logger_, "main thread has not yet set request for LaneParkingPlanner"); return; } - const auto & current_status = current_status_opt.value(); - const auto & previous_module_output = previous_module_output_opt.value(); - const auto & last_previous_module_output = last_previous_module_output_opt.value(); - const auto & parameters = parameters_opt.value(); - const auto & goal_candidates = goal_candidates_opt.value(); + const auto & local_request = local_request_opt.value(); + const auto & goal_candidates = local_request.goal_candidates_; + const auto & local_planner_data = local_request.get_planner_data(); + const auto & upstream_module_output = local_request.get_upstream_module_output(); + const auto & pull_over_path_opt = local_request.get_pull_over_path(); + const auto & prev_data = local_request.get_prev_data(); + const auto trigger_thread_on_approach = local_request.trigger_thread_on_approach(); - if (current_status == ModuleStatus::IDLE) { + if (!trigger_thread_on_approach) { return; } @@ -326,34 +343,35 @@ void GoalPlannerModule::onTimer() } // check if new pull over path candidates are needed to be generated - const auto current_state = thread_safe_data_.get_prev_data().state; + const auto current_state = prev_data.state; const bool need_update = std::invoke([&]() { - const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - const std::optional pull_over_path_opt = - found_pull_over_path - ? std::make_optional(*thread_safe_data_.get_pull_over_path()) - : std::nullopt; + { + std::lock_guard guard(mutex_); + if (response_.pull_over_path_candidates.empty()) { + return true; + } + } const std::optional modified_goal_opt = pull_over_path_opt ? std::make_optional(pull_over_path_opt.value().modified_goal()) : std::nullopt; if (isOnModifiedGoal( - local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { + local_planner_data->self_odometry->pose.pose, modified_goal_opt, parameters_)) { return false; } - if (hasDeviatedFromCurrentPreviousModulePath(*local_planner_data, previous_module_output)) { + if (hasDeviatedFromPath( + local_planner_data->self_odometry->pose.pose.position, upstream_module_output)) { RCLCPP_DEBUG(getLogger(), "has deviated from current previous module path"); return false; } - if (thread_safe_data_.get_pull_over_path_candidates().empty()) { - return true; - } - if (hasPreviousModulePathShapeChanged(previous_module_output, last_previous_module_output)) { + if (hasPreviousModulePathShapeChanged( + upstream_module_output, original_upstream_module_output_)) { RCLCPP_DEBUG(getLogger(), "has previous module path shape changed"); return true; } if ( - hasDeviatedFromLastPreviousModulePath(*local_planner_data, last_previous_module_output) && + hasDeviatedFromPath( + local_planner_data->self_odometry->pose.pose.position, original_upstream_module_output_) && current_state != PathDecisionState::DecisionKind::DECIDED) { RCLCPP_DEBUG(getLogger(), "has deviated from last previous module path"); return true; @@ -370,24 +388,64 @@ void GoalPlannerModule::onTimer() // generate valid pull over path candidates and calculate closest start pose const auto current_lanes = utils::getExtendedCurrentLanes( - local_planner_data, parameters.backward_goal_search_length, - parameters.forward_goal_search_length, + local_planner_data, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); std::vector path_candidates{}; std::optional closest_start_pose{}; - double min_start_arc_length = std::numeric_limits::max(); + std::optional> sorted_indices_opt{std::nullopt}; + if (parameters_.bus_stop_area.use_bus_stop_area && switch_bezier_) { + bezier_planning_helper( + local_planner_data, goal_candidates, upstream_module_output, current_lanes, + closest_start_pose, path_candidates, sorted_indices_opt); + } else { + normal_pullover_planning_helper( + local_planner_data, goal_candidates, upstream_module_output, current_lanes, + closest_start_pose, path_candidates); + } + + // set response + { + original_upstream_module_output_ = upstream_module_output; + std::lock_guard guard(mutex_); + response_.pull_over_path_candidates = path_candidates; + if (closest_start_pose) { + response_.closest_start_pose = closest_start_pose; + } + RCLCPP_INFO( + getLogger(), "generated %lu pull over path candidates", + response_.pull_over_path_candidates.size()); + response_.sorted_bezier_indices_opt = std::move(sorted_indices_opt); + } +} + +void LaneParkingPlanner::normal_pullover_planning_helper( + const std::shared_ptr planner_data, const GoalCandidates & goal_candidates, + const BehaviorModuleOutput & upstream_module_output, + const lanelet::ConstLanelets current_lanelets, std::optional & closest_start_pose, + std::vector & path_candidates) +{ + // todo: currently non centerline input path is supported only by shift pull over + const bool is_center_line_input_path = goal_planner_utils::isReferencePath( + upstream_module_output.reference_path, upstream_module_output.path, 0.1); + RCLCPP_DEBUG( + getLogger(), "the input path of pull over planner is center line: %d", + is_center_line_input_path); + + double min_start_arc_length = std::numeric_limits::infinity(); const auto planCandidatePaths = [&]( const std::shared_ptr & planner, const GoalCandidate & goal_candidate) { - const auto pull_over_path = planner->plan( - goal_candidate, path_candidates.size(), local_planner_data, previous_module_output); + // normal pull_over + const auto pull_over_path = + planner->plan(goal_candidate, path_candidates.size(), planner_data, upstream_module_output); if (pull_over_path) { // calculate absolute maximum curvature of parking path(start pose to end pose) for path // priority path_candidates.push_back(*pull_over_path); // calculate closest pull over start pose for stop path const double start_arc_length = - lanelet::utils::getArcCoordinates(current_lanes, pull_over_path->start_pose()).length; + lanelet::utils::getArcCoordinates(current_lanelets, pull_over_path->start_pose()).length; if (start_arc_length < min_start_arc_length) { min_start_arc_length = start_arc_length; // closest start pose is stop point when not finding safe path @@ -396,15 +454,8 @@ void GoalPlannerModule::onTimer() } }; - // todo: currently non centerline input path is supported only by shift pull over - const bool is_center_line_input_path = goal_planner_utils::isReferencePath( - previous_module_output.reference_path, previous_module_output.path, 0.1); - RCLCPP_DEBUG( - getLogger(), "the input path of pull over planner is center line: %d", - is_center_line_input_path); - // plan candidate paths and set them to the member variable - if (parameters.path_priority == "efficient_path") { + if (parameters_.path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line if (!is_center_line_input_path && planner->getPlannerType() != PullOverPlannerType::SHIFT) { @@ -414,7 +465,7 @@ void GoalPlannerModule::onTimer() planCandidatePaths(planner, goal_candidate); } } - } else if (parameters.path_priority == "close_goal") { + } else if (parameters_.path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { // todo: temporary skip NON SHIFT planner when input path is not center line @@ -424,58 +475,132 @@ void GoalPlannerModule::onTimer() planCandidatePaths(planner, goal_candidate); } } - } else { - RCLCPP_ERROR( - getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", - parameters.path_priority.c_str()); - throw std::domain_error("[pull_over] invalid path_priority"); } - // set member variables - thread_safe_data_.set_pull_over_path_candidates(path_candidates); - thread_safe_data_.set_closest_start_pose(closest_start_pose); - RCLCPP_INFO(getLogger(), "generated %lu pull over path candidates", path_candidates.size()); + if (closest_start_pose) { + const auto original_pose = planner_data->route_handler->getOriginalGoalPose(); + if ( + parameters_.bus_stop_area.use_bus_stop_area && + std::fabs(autoware::universe_utils::normalizeRadian( + autoware::universe_utils::getRPY(original_pose).z - + autoware::universe_utils::getRPY(closest_start_pose.value()).z)) > + pull_over_angle_threshold) { + // reset and try bezier next time + switch_bezier_ = true; + path_candidates.clear(); + RCLCPP_INFO(getLogger(), "will generate Bezier Paths next"); + } + } else if (parameters_.bus_stop_area.use_bus_stop_area && path_candidates.size() == 0) { + switch_bezier_ = true; + RCLCPP_INFO( + getLogger(), "Could not find any shift pull over paths, will generate Bezier Paths next"); + } } -void GoalPlannerModule::onFreespaceParkingTimer() +void LaneParkingPlanner::bezier_planning_helper( + const std::shared_ptr planner_data, const GoalCandidates & goal_candidates, + const BehaviorModuleOutput & upstream_module_output, + const lanelet::ConstLanelets current_lanelets, std::optional & closest_start_pose, + std::vector & path_candidates, + std::optional> & sorted_indices_opt) const +{ + autoware::universe_utils::StopWatch timer; + timer.tic("bezier"); + std::vector path_candidates_all; + for (const auto & goal_candidate : goal_candidates) { + auto bezier_pull_over_paths = bezier_pull_over_planner_->plans( + goal_candidate, path_candidates_all.size(), planner_data, upstream_module_output); + std::copy( + std::make_move_iterator(bezier_pull_over_paths.begin()), + std::make_move_iterator(bezier_pull_over_paths.end()), + std::back_inserter(path_candidates_all)); + } + RCLCPP_INFO( + getLogger(), "there are %lu bezier paths (calculated in %f [sec])", path_candidates_all.size(), + timer.toc("bezier")); + + sorted_indices_opt = std::vector(); + auto & sorted_indices = sorted_indices_opt.value(); + sorted_indices.reserve(path_candidates_all.size()); + + std::unordered_map goal_candidate_map; + for (const auto & goal_candidate : goal_candidates) { + goal_candidate_map[goal_candidate.id] = goal_candidate; + } + for (size_t i = 0; i < path_candidates_all.size(); ++i) { + const auto & path = path_candidates_all[i]; + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); + if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { + sorted_indices.push_back(i); + } + } + const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects( + *(planner_data->dynamic_object), *(planner_data->route_handler), parameters_, + planner_data->parameters.vehicle_width); + const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( + dynamic_target_objects, parameters_.th_moving_object_velocity); + sortPullOverPaths( + planner_data, parameters_, path_candidates_all, goal_candidates, static_target_objects, + getLogger(), sorted_indices); + + const auto clip_size = std::min(path_candidates_all.size(), 100); + // take upto 100 elements + sorted_indices.resize(clip_size); + for (const auto sorted_index : sorted_indices) { + path_candidates.push_back(path_candidates_all[sorted_index]); + } + sorted_indices.clear(); + // now path_candidates are sorted from 0 to 100 + for (unsigned i = 0; i < clip_size; ++i) { + sorted_indices.push_back(i); + } + + std::stable_sort( + std::execution::par, sorted_indices.begin(), sorted_indices.end(), + [&](const size_t a_i, const size_t b_i) { + const auto & a = path_candidates[a_i]; + const auto & b = path_candidates[b_i]; + return a.parking_path_curvature_total_derivative() < + b.parking_path_curvature_total_derivative(); + }); + + double min_start_arc_length = std::numeric_limits::infinity(); + for (const auto & bezier_pull_over_path : path_candidates) { + const double start_arc_length = + lanelet::utils::getArcCoordinates(current_lanelets, bezier_pull_over_path.start_pose()) + .length; + if (start_arc_length < min_start_arc_length) { + min_start_arc_length = start_arc_length; + closest_start_pose = bezier_pull_over_path.start_pose(); + } + } +} + +void FreespaceParkingPlanner::onTimer() { const ScopedFlag flag(is_freespace_parking_cb_running_); - std::shared_ptr local_planner_data{nullptr}; - std::optional current_status_opt{std::nullopt}; - std::shared_ptr occupancy_grid_map{nullptr}; - std::optional parameters_opt{std::nullopt}; - std::optional vehicle_footprint_opt{std::nullopt}; - std::optional goal_candidates_opt{std::nullopt}; + std::optional local_request_opt; // begin of critical section { - std::lock_guard guard(gp_planner_data_mutex_); - if (gp_planner_data_) { - const auto & gp_planner_data = gp_planner_data_.value(); - local_planner_data = std::make_shared(gp_planner_data.planner_data); - current_status_opt = gp_planner_data.current_status; - occupancy_grid_map = gp_planner_data.occupancy_grid_map; - parameters_opt = gp_planner_data.parameters; - vehicle_footprint_opt = gp_planner_data.vehicle_footprint; - goal_candidates_opt = gp_planner_data.goal_candidates; + std::lock_guard guard(mutex_); + if (request_) { + auto & request = request_.value(); + local_request_opt.emplace(request); } } // end of critical section - if ( - !local_planner_data || !current_status_opt || !occupancy_grid_map || !parameters_opt || - !vehicle_footprint_opt || !goal_candidates_opt) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 5000, - "failed to get valid planner_data/current_status/parameters in " - "onFreespaceParkingTimer"); + if (!local_request_opt) { + RCLCPP_DEBUG(logger_, "main thread has not yet set request for FreespaceParkingPlanner"); return; } - - const auto & current_status = current_status_opt.value(); - const auto & parameters = parameters_opt.value(); - const auto & vehicle_footprint = vehicle_footprint_opt.value(); - const auto & goal_candidates = goal_candidates_opt.value(); + const auto & local_request = local_request_opt.value(); + const auto & parameters = local_request.parameters_; + const auto & local_planner_data = local_request.get_planner_data(); + const auto & current_status = local_request.get_current_status(); + const auto & pull_over_path_opt = local_request.get_pull_over_path(); + const auto & last_path_update_time = local_request.get_last_path_update_time(); if (current_status == ModuleStatus::IDLE) { return; @@ -489,10 +614,6 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } - const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - const std::optional pull_over_path_opt = - found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) - : std::nullopt; const std::optional modified_goal_opt = pull_over_path_opt ? std::make_optional(pull_over_path_opt.value().modified_goal()) @@ -502,24 +623,9 @@ void GoalPlannerModule::onFreespaceParkingTimer() return; } - const double vehicle_width = planner_data_->parameters.vehicle_width; - const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE; - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(local_planner_data->route_handler), left_side_parking, parameters.backward_goal_search_length, - parameters.forward_goal_search_length); - const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( - pull_over_lanes, left_side_parking, parameters.detection_bound_offset, - parameters.margin_from_boundary + parameters.max_lateral_offset + vehicle_width); - - PredictedObjects dynamic_target_objects{}; - for (const auto & object : local_planner_data->dynamic_object->objects) { - const auto object_polygon = universe_utils::toPolygon2d(object); - if ( - objects_extraction_polygon.has_value() && - boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { - dynamic_target_objects.objects.push_back(object); - } - } + const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects( + *(local_planner_data->dynamic_object), *(local_planner_data->route_handler), parameters, + local_planner_data->parameters.vehicle_width); const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( dynamic_target_objects, parameters.th_moving_object_velocity); @@ -527,81 +633,52 @@ void GoalPlannerModule::onFreespaceParkingTimer() (clock_->now() - local_planner_data->costmap->header.stamp).seconds() < 1.0; constexpr double path_update_duration = 1.0; if ( - isStuck( - static_target_objects, dynamic_target_objects, local_planner_data, occupancy_grid_map, - parameters) && - is_new_costmap && + isStuck(static_target_objects, dynamic_target_objects, local_request) && is_new_costmap && needPathUpdate( local_planner_data->self_odometry->pose.pose, path_update_duration, clock_->now(), - modified_goal_opt, thread_safe_data_.get_last_path_update_time(), parameters)) { - auto goal_searcher = std::make_shared(parameters, vehicle_footprint); - - planFreespacePath( - local_planner_data, goal_searcher, goal_candidates, occupancy_grid_map, - static_target_objects); + modified_goal_opt, last_path_update_time, parameters)) { + const auto freespace_path_opt = + planFreespacePath(local_request, static_target_objects, freespace_planner_); + if (freespace_path_opt) { + std::lock_guard guard(mutex_); + response_.freespace_pull_over_path = freespace_path_opt.value(); + } } } -void GoalPlannerModule::updateData() +std::pair GoalPlannerModule::syncWithThreads() { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - - // extract static and dynamic objects in extraction polygon for path collision check - const auto & p = parameters_; - const auto & rh = *(planner_data_->route_handler); - const auto objects = *(planner_data_->dynamic_object); - const double vehicle_width = planner_data_->parameters.vehicle_width; - const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length); - const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( - pull_over_lanes, left_side_parking_, p->detection_bound_offset, - p->margin_from_boundary + p->max_lateral_offset + vehicle_width); - if (objects_extraction_polygon.has_value()) { - debug_data_.objects_extraction_polygon = objects_extraction_polygon.value(); - } - PredictedObjects dynamic_target_objects{}; - for (const auto & object : objects.objects) { - const auto object_polygon = universe_utils::toPolygon2d(object); - if ( - objects_extraction_polygon.has_value() && - boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { - dynamic_target_objects.objects.push_back(object); - } - } - const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_target_objects, p->th_moving_object_velocity); - - // update goal searcher and generate goal candidates - if (goal_candidates_.empty()) { - goal_candidates_ = generateGoalCandidates(); - } - // In PlannerManager::run(), it calls SceneModuleInterface::setData and // SceneModuleInterface::setPreviousModuleOutput before module_ptr->run(). // Then module_ptr->run() invokes GoalPlannerModule::updateData and then - // planWaitingApproval()/plan(), so we can copy latest current_status/previous_module_output to - // gp_planner_data_ here + // planWaitingApproval()/plan(), so we can copy latest current_status/upstream_module_output to + // lane_parking_request/freespace_parking_request - // NOTE: onTimer/onFreespaceParkingTimer copies gp_planner_data_ to their local clone, so we need - // to lock gp_planner_data_ here to avoid data race. But the following clone process is - // lightweight because most of the member variables of PlannerData/RouteHandler is - // shared_ptrs/bool + std::optional pull_over_path = + context_data_ ? context_data_.value().pull_over_path_opt : std::nullopt; + std::optional last_path_update_time = + context_data_ ? context_data_.value().last_path_update_time : std::nullopt; + + // NOTE: Following clone process is rather lightweight because most of the member variables of + // PlannerData/RouteHandler is shared_ptrs // begin of critical section + LaneParkingResponse lane_parking_response; { - std::lock_guard guard(gp_planner_data_mutex_); - if (!gp_planner_data_) { - gp_planner_data_ = GoalPlannerData(*planner_data_, *parameters_, getPreviousModuleOutput()); + std::lock_guard guard(lane_parking_mutex_); + if (!lane_parking_request_) { + lane_parking_request_.emplace( + vehicle_footprint_, goal_candidates_, getPreviousModuleOutput()); } - auto & gp_planner_data = gp_planner_data_.value(); // NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that // planner_data_ is not nullptr, so it is OK to copy as value // By copying PlannerData as value, the internal shared member variables are also copied - // (reference count is incremented), so `gp_planner_data_.foo` is now thread-safe from the - // **re-pointing** by `planner_data_->foo = msg` in behavior_path_planner::onCallbackFor(msg) - // and if these two coincided, only the reference count is affected - gp_planner_data.update( - *parameters_, *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), - vehicle_footprint_, goal_candidates_); + // (reference count is incremented), so `lane_parking_request_local.planner_data_.foo` is now + // thread-safe from the **re-pointing** by `planner_data_->foo = msg` in + // behavior_path_planner::onCallbackFor(msg) and if these two coincided, only the reference + // count is affected + lane_parking_request_.value().update( + *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path, + path_decision_controller_.get_current_state(), trigger_thread_on_approach_); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since // behavior_path_planner::run() updates @@ -609,19 +686,65 @@ void GoalPlannerModule::updateData() // to copy route_handler as value to use lanelet_map_ptr_/routing_graph_ptr_ thread-safely in // onTimer/onFreespaceParkingTimer // TODO(Mamoru Sobue): If the copy of RouteHandler.road_lanelets/shoulder_lanelets is not - // lightweight, we should update gp_planner_data_.route_handler only when + // lightweight, we should update lane_parking_request.planner_data.route_handler only when // `planner_data_.is_route_handler_updated` variable is set true by behavior_path_planner - // (although this flag is not implemented yet). In that case, gp_planner_data members except - // for route_handler should be copied from planner_data_ + // (although this flag is not implemented yet). In that case, lane_parking_request members + // except for route_handler should be copied from planner_data_ + lane_parking_response = lane_parking_response_; + } - // GoalPlannerModule::occupancy_grid_map_ and gp_planner_data.occupancy_grid_map share the - // ownership, and gp_planner_data.occupancy_grid_map maybe also shared by the local - // planner_data on onFreespaceParkingTimer thread local memory space. So following operation - // is thread-safe because gp_planner_data.occupancy_grid_map is only re-pointed here and its - // prior resource is still owned by the onFreespaceParkingTimer thread locally. - occupancy_grid_map_ = gp_planner_data.occupancy_grid_map; + FreespaceParkingResponse freespace_parking_response; + { + std::lock_guard guard(freespace_parking_mutex_); + if (!freespace_parking_request_) { + freespace_parking_request_.emplace( + *parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); + } + constexpr double stuck_time = 5.0; + freespace_parking_request_.value().update( + *planner_data_, getCurrentStatus(), pull_over_path, last_path_update_time, + isStopped( + odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time, + parameters_->th_stopped_velocity)); + // TODO(soblin): currently, ogm-based-collision-detector is updated to latest in + // freespace_parking_request_.value().update, and it is shared with goal_planner_module. Next, + // goal_planner_module update it and pass it to freespace_parking_request. + occupancy_grid_map_ = freespace_parking_request_.value().get_occupancy_grid_map(); + freespace_parking_response = freespace_parking_response_; } // end of critical section + return {lane_parking_response, freespace_parking_response}; +} + +void GoalPlannerModule::updateData() +{ + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + // update goal searcher and generate goal candidates + if (goal_candidates_.empty()) { + goal_candidates_ = generateGoalCandidates(); + } + + const lanelet::ConstLanelets current_lanes = + utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); + + if ( + !trigger_thread_on_approach_ && + utils::isAllowedGoalModification(planner_data_->route_handler) && + goal_planner_utils::is_goal_reachable_on_path( + current_lanes, *(planner_data_->route_handler), left_side_parking_)) { + const double self_to_goal_arc_length = utils::getSignedDistance( + planner_data_->self_odometry->pose.pose, planner_data_->route_handler->getOriginalGoalPose(), + current_lanes); + if (self_to_goal_arc_length < parameters_->pull_over_prepare_length) { + trigger_thread_on_approach_ = true; + [[maybe_unused]] const auto send_only_request = syncWithThreads(); + RCLCPP_INFO( + getLogger(), "start preparing goal candidates once for goal ahead of %f meter", + self_to_goal_arc_length); + return; + } + } if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { return; @@ -631,17 +754,18 @@ void GoalPlannerModule::updateData() resetPathReference(); path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); + const bool found_pull_over_path = + context_data_ ? context_data_.value().pull_over_path_opt.has_value() : false; std::optional pull_over_path_recv = - found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) - : std::nullopt; + found_pull_over_path + ? std::make_optional(context_data_.value().pull_over_path_opt.value()) + : std::nullopt; const auto modified_goal_pose = [&]() -> std::optional { if (!pull_over_path_recv) { return std::nullopt; } - const auto & pull_over_path = pull_over_path_recv.value(); - return pull_over_path.modified_goal(); + return pull_over_path_recv.value().modified_goal(); }(); // save "old" state @@ -651,26 +775,48 @@ void GoalPlannerModule::updateData() ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); debug_data_.collision_check = collision_check_map; // update to latest state + // extract static and dynamic objects in extraction polygon for path collision check + const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects( + *(planner_data_->dynamic_object), *(planner_data_->route_handler), *parameters_, + planner_data_->parameters.vehicle_width); + const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( + dynamic_target_objects, parameters_->th_moving_object_velocity); + path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); - context_data_.emplace( - path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, - dynamic_target_objects, std::move(pull_over_path_recv), - thread_safe_data_.get_pull_over_path_candidates(), prev_decision_state); + auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); + if (context_data_) { + context_data_.value().update( + path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, + dynamic_target_objects, prev_decision_state, + isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity), + std::move(lane_parking_response), std::move(freespace_parking_response)); + } else { + context_data_.emplace( + path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, + dynamic_target_objects, prev_decision_state, + isStopped( + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, + parameters_->th_stopped_velocity), + std::move(lane_parking_response), std::move(freespace_parking_response)); + } auto & ctx_data = context_data_.value(); - thread_safe_data_.set_prev_data(path_decision_controller_.get_current_state()); - if (!isActivated()) { return; } if (hasFinishedCurrentPath(ctx_data)) { - if (thread_safe_data_.incrementPathIndex()) { - ctx_data.last_path_idx_increment_time = clock_->now(); + if (ctx_data.pull_over_path_opt) { + auto & pull_over_path = ctx_data.pull_over_path_opt.value(); + if (pull_over_path.incrementPathIndex()) { + ctx_data.last_path_idx_increment_time = clock_->now(); + } } } @@ -678,7 +824,7 @@ void GoalPlannerModule::updateData() last_approval_data_ = std::make_unique(clock_->now(), planner_data_->self_odometry->pose.pose); // TODO(soblin): do not "plan" in updateData - decideVelocity(); + if (ctx_data.pull_over_path_opt) decideVelocity(ctx_data.pull_over_path_opt.value()); } } @@ -696,7 +842,7 @@ void GoalPlannerModule::processOnExit() resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); - thread_safe_data_.reset(); + context_data_ = std::nullopt; last_approval_data_.reset(); } @@ -715,39 +861,9 @@ bool GoalPlannerModule::isExecutionRequested() const // check if goal_pose is in current_lanes or neighboring road lanes const lanelet::ConstLanelets current_lanes = utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); - const auto getNeighboringLane = - [&](const lanelet::ConstLanelet & lane) -> std::optional { - return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true) - : route_handler->getRightLanelet(lane, false, true); - }; - lanelet::ConstLanelets goal_check_lanes = current_lanes; - for (const auto & lane : current_lanes) { - auto neighboring_lane = getNeighboringLane(lane); - while (neighboring_lane) { - goal_check_lanes.push_back(neighboring_lane.value()); - neighboring_lane = getNeighboringLane(neighboring_lane.value()); - } - } - const bool goal_is_in_current_segment_lanes = std::any_of( - goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) { - return lanelet::utils::isInLanelet(goal_pose, lane); - }); - - // check that goal is in current neighbor shoulder lane - const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() { - for (const auto & lane : current_lanes) { - const auto shoulder_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane) - : route_handler->getRightShoulderLanelet(lane); - if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) { - return true; - } - } - return false; - }); - - // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner, - // because goal arc coordinates cannot be calculated. - if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) { + const bool is_goal_reachable = goal_planner_utils::is_goal_reachable_on_path( + current_lanes, *(planner_data_->route_handler), left_side_parking_); + if (!is_goal_reachable) { return false; } @@ -755,7 +871,7 @@ bool GoalPlannerModule::isExecutionRequested() const // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner if (!utils::isAllowedGoalModification(route_handler)) { - return goal_is_in_current_segment_lanes; + return is_goal_reachable; } // if goal arc coordinates can be calculated, check if goal is in request_length @@ -833,48 +949,6 @@ double GoalPlannerModule::calcModuleRequestLength() const return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); } -bool GoalPlannerModule::planFreespacePath( - std::shared_ptr planner_data, - const std::shared_ptr goal_searcher, const GoalCandidates & goal_candidates_arg, - const std::shared_ptr occupancy_grid_map, - const PredictedObjects & static_target_objects) -{ - auto goal_candidates = goal_candidates_arg; - goal_searcher->update(goal_candidates, occupancy_grid_map, planner_data, static_target_objects); - debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size(); - debug_data_.freespace_planner.is_planning = true; - - for (size_t i = 0; i < goal_candidates.size(); i++) { - const auto goal_candidate = goal_candidates.at(i); - { - const std::lock_guard lock(mutex_); - debug_data_.freespace_planner.current_goal_idx = i; - } - - if (!goal_candidate.is_safe) { - continue; - } - const auto freespace_path = freespace_planner_->plan( - goal_candidate, 0, planner_data, BehaviorModuleOutput{} // NOTE: not used so passing {} is OK - ); - if (!freespace_path) { - continue; - } - - { - const std::lock_guard lock(mutex_); - thread_safe_data_.set_pull_over_path(*freespace_path); - debug_data_.freespace_planner.is_planning = false; - } - - return true; - } - - const std::lock_guard lock(mutex_); - debug_data_.freespace_planner.is_planning = false; - return false; -} - bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & context_data) { // return only before starting free space parking @@ -890,6 +964,11 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte if (context_data.pull_over_path_opt.value().type() == PullOverPlannerType::FREESPACE) { return false; } + // TODO(soblin): return from freespace to lane is disabled temporarily, because if + // context_data_with_velocity contained freespace path, since lane_parking_pull_over_path is + // deleted, freespace path is set again + // So context_data need to have old_selected_lane_pull_over_path also, which is only updated + // against lane_pull_over_path in selectPullOverPath() const auto & lane_parking_path = context_data.pull_over_path_opt.value(); const auto & path = lane_parking_path.full_path(); @@ -954,7 +1033,7 @@ BehaviorModuleOutput GoalPlannerModule::plan() RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, " [pull_over] plan() is called without valid context_data"); } else { - const auto & context_data = context_data_.value(); + auto & context_data = context_data_.value(); return planPullOver(context_data); } } @@ -963,63 +1042,14 @@ BehaviorModuleOutput GoalPlannerModule::plan() return fixed_goal_planner_->plan(planner_data_); } -std::optional GoalPlannerModule::selectPullOverPath( - const PullOverContextData & context_data, +void sortPullOverPaths( + const std::shared_ptr planner_data, const GoalPlannerParameters & parameters, const std::vector & pull_over_path_candidates, - const GoalCandidates & goal_candidates) const + const GoalCandidates & goal_candidates, const PredictedObjects & static_target_objects, + rclcpp::Logger logger, std::vector & sorted_path_indices) { - universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - - const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); - const double backward_length = - parameters_->backward_goal_search_length + parameters_->decide_path_distance; - const auto & prev_module_output_path = getPreviousModuleOutput().path; - const auto & soft_margins = parameters_->object_recognition_collision_check_soft_margins; - const auto & hard_margins = parameters_->object_recognition_collision_check_hard_margins; - - // STEP1: Filter valid paths before sorting - // NOTE: Since copying pull over path takes time, it is handled by indices - std::vector sorted_path_indices; - sorted_path_indices.reserve(pull_over_path_candidates.size()); - - // STEP1-1: Extract paths which have safe goal - // Create a map of goal_id to GoalCandidate for quick access - std::unordered_map goal_candidate_map; - for (const auto & goal_candidate : goal_candidates) { - goal_candidate_map[goal_candidate.id] = goal_candidate; - } - for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { - const auto & path = pull_over_path_candidates[i]; - const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); - if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { - sorted_path_indices.push_back(i); - } - } - - // STEP1-2: Remove paths which do not have enough distance - const double prev_path_front_to_goal_dist = calcSignedArcLength( - prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position, - goal_pose.position); - const auto & long_tail_reference_path = [&]() { - if (prev_path_front_to_goal_dist > backward_length) { - return prev_module_output_path; - } - // get road lanes which is at least backward_length[m] behind the goal - const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - prev_module_output_path, planner_data_, backward_length, 0.0, false); - const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; - return planner_data_->route_handler->getCenterLinePath( - road_lanes, std::max(0.0, goal_pose_length - backward_length), - goal_pose_length + parameters_->forward_goal_search_length); - }(); - - sorted_path_indices.erase( - std::remove_if( - sorted_path_indices.begin(), sorted_path_indices.end(), - [&](const size_t i) { - return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path); - }), - sorted_path_indices.end()); + const auto & soft_margins = parameters.object_recognition_collision_check_soft_margins; + const auto & hard_margins = parameters.object_recognition_collision_check_hard_margins; // STEP2: Sort pull over path candidates // STEP2-1: Sort pull_over_path_candidates based on the order in goal_candidates @@ -1031,7 +1061,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // Sort pull_over_path_candidates based on the order in goal_candidates std::stable_sort( - sorted_path_indices.begin(), sorted_path_indices.end(), + std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; @@ -1047,14 +1077,14 @@ std::optional GoalPlannerModule::selectPullOverPath( // compare to sort pull_over_path_candidates based on the order in efficient_path_order const auto comparePathTypePriority = [&](const PullOverPath & a, const PullOverPath & b) -> bool { - const auto & order = parameters_->efficient_path_order; + const auto & order = parameters.efficient_path_order; const auto a_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(a.type())); const auto b_pos = std::find(order.begin(), order.end(), magic_enum::enum_name(b.type())); return a_pos < b_pos; }; // if object recognition is enabled, sort by collision check margin - if (parameters_->use_object_recognition) { + if (parameters.use_object_recognition) { // STEP2-2: Sort by collision check margins const auto [margins, margins_with_zero] = std::invoke([&]() -> std::tuple, std::vector> { @@ -1067,11 +1097,11 @@ std::optional GoalPlannerModule::selectPullOverPath( // Create a map of PullOverPath pointer to largest collision check margin std::map path_id_to_rough_margin_map; - const auto & target_objects = context_data.static_target_objects; + const auto & target_objects = static_target_objects; for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const double distance = utils::path_safety_checker::calculateRoughDistanceToObjects( - path.parking_path(), target_objects, planner_data_->parameters, false, "max"); + path.parking_path(), target_objects, planner_data->parameters, false, "max"); auto it = std::lower_bound( margins_with_zero.begin(), margins_with_zero.end(), distance, std::greater()); if (it == margins_with_zero.end()) { @@ -1083,7 +1113,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // sorts in descending order so the item with larger margin comes first std::stable_sort( - sorted_path_indices.begin(), sorted_path_indices.end(), + std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; @@ -1101,7 +1131,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // STEP2-3: Sort by curvature // If the curvature is less than the threshold, prioritize the path. const auto isHighCurvature = [&](const PullOverPath & path) -> bool { - return path.parking_path_max_curvature() >= parameters_->high_curvature_threshold; + return path.parking_path_max_curvature() >= parameters.high_curvature_threshold; }; const auto isSoftMargin = [&](const PullOverPath & path) -> bool { @@ -1118,7 +1148,7 @@ std::optional GoalPlannerModule::selectPullOverPath( // NOTE: this is just partition sort based on curvature threshold within each sub partitions std::stable_sort( - sorted_path_indices.begin(), sorted_path_indices.end(), + std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; @@ -1137,9 +1167,9 @@ std::optional GoalPlannerModule::selectPullOverPath( // STEP2-4: Sort pull_over_path_candidates based on the order in efficient_path_order keeping // the collision check margin and curvature priority. - if (parameters_->path_priority == "efficient_path") { + if (parameters.path_priority == "efficient_path") { std::stable_sort( - sorted_path_indices.begin(), sorted_path_indices.end(), + std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { // if any of following conditions are met, sort by path type priority // - both are soft margin @@ -1164,16 +1194,16 @@ std::optional GoalPlannerModule::selectPullOverPath( const std::string path_priority_info_str = goal_planner_utils::makePathPriorityDebugMessage( sorted_path_indices, pull_over_path_candidates, goal_id_to_index, goal_candidates, path_id_to_rough_margin_map, isSoftMargin, isHighCurvature); - RCLCPP_DEBUG_STREAM(getLogger(), path_priority_info_str); + RCLCPP_DEBUG_STREAM(logger, path_priority_info_str); } else { /** * NOTE: use_object_recognition=false is not recommended. This option will be deprecated in the * future. sort by curvature is not implemented yet. * Sort pull_over_path_candidates based on the order in efficient_path_order */ - if (parameters_->path_priority == "efficient_path") { + if (parameters.path_priority == "efficient_path") { std::stable_sort( - sorted_path_indices.begin(), sorted_path_indices.end(), + std::execution::par, sorted_path_indices.begin(), sorted_path_indices.end(), [&](const size_t a_i, const size_t b_i) { const auto & a = pull_over_path_candidates[a_i]; const auto & b = pull_over_path_candidates[b_i]; @@ -1184,6 +1214,71 @@ std::optional GoalPlannerModule::selectPullOverPath( }); } } +} + +std::optional GoalPlannerModule::selectPullOverPath( + const PullOverContextData & context_data, + const std::vector & pull_over_path_candidates, + const GoalCandidates & goal_candidates, + const std::optional> sorted_bezier_indices_opt) const +{ + universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); + + const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); + const double backward_length = + parameters_->backward_goal_search_length + parameters_->decide_path_distance; + const auto & prev_module_output_path = getPreviousModuleOutput().path; + + // STEP1: Filter valid paths before sorting + // NOTE: Since copying pull over path takes time, it is handled by indices + std::vector sorted_path_indices; + if (!sorted_bezier_indices_opt) { + sorted_path_indices.reserve(pull_over_path_candidates.size()); + } else { + sorted_path_indices = sorted_bezier_indices_opt.value(); + } + // STEP1-1: Extract paths which have safe goal + // Create a map of goal_id to GoalCandidate for quick access + std::unordered_map goal_candidate_map; + for (const auto & goal_candidate : goal_candidates) { + goal_candidate_map[goal_candidate.id] = goal_candidate; + } + for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) { + const auto & path = pull_over_path_candidates[i]; + const auto goal_candidate_it = goal_candidate_map.find(path.goal_id()); + if (goal_candidate_it != goal_candidate_map.end() && goal_candidate_it->second.is_safe) { + sorted_path_indices.push_back(i); + } + } + + // STEP1-2: Remove paths which do not have enough distance + const double prev_path_front_to_goal_dist = calcSignedArcLength( + prev_module_output_path.points, prev_module_output_path.points.front().point.pose.position, + goal_pose.position); + const auto & long_tail_reference_path = [&]() { + if (prev_path_front_to_goal_dist > backward_length) { + return prev_module_output_path; + } + // get road lanes which is at least backward_length[m] behind the goal + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + prev_module_output_path, planner_data_, backward_length, 0.0, false); + const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; + return planner_data_->route_handler->getCenterLinePath( + road_lanes, std::max(0.0, goal_pose_length - backward_length), + goal_pose_length + parameters_->forward_goal_search_length); + }(); + + sorted_path_indices.erase( + std::remove_if( + sorted_path_indices.begin(), sorted_path_indices.end(), + [&](const size_t i) { + return !hasEnoughDistance(pull_over_path_candidates[i], long_tail_reference_path); + }), + sorted_path_indices.end()); + + sortPullOverPaths( + planner_data_, *parameters_, pull_over_path_candidates, goal_candidates, + context_data.static_target_objects, getLogger(), sorted_path_indices); // STEP3: Select the final pull over path by checking collision to make it as high priority as // possible @@ -1228,13 +1323,14 @@ std::vector GoalPlannerModule::generateDrivableLanes() const } void GoalPlannerModule::setOutput( + const std::optional selected_pull_over_path_with_velocity_opt, const PullOverContextData & context_data, BehaviorModuleOutput & output) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); output.reference_path = getPreviousModuleOutput().reference_path; - if (!context_data.pull_over_path_opt) { + if (!selected_pull_over_path_with_velocity_opt) { // situation : not safe against static objects use stop_path output.path = generateStopPath( context_data, (goal_candidates_.empty() ? "no goal candidate" : "no static safe path")); @@ -1244,7 +1340,7 @@ void GoalPlannerModule::setOutput( return; } - const auto & pull_over_path = context_data.pull_over_path_opt.value(); + const auto & pull_over_path = selected_pull_over_path_with_velocity_opt.value(); if ( parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && isActivated()) { @@ -1328,30 +1424,30 @@ void GoalPlannerModule::setTurnSignalInfo( void GoalPlannerModule::updateSteeringFactor( const PullOverContextData & context_data, const std::array & pose, - const std::array distance, const uint16_t type) + const std::array distance) { - const uint16_t steering_factor_direction = std::invoke([&]() { + const uint16_t planning_factor_direction = std::invoke([&]() { const auto turn_signal = calcTurnSignalInfo(context_data); if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; } else if (turn_signal.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) { - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; } - return SteeringFactor::STRAIGHT; + return PlanningFactor::NONE; }); - steering_factor_interface_.set(pose, distance, steering_factor_direction, type, ""); + planning_factor_interface_->add( + distance[0], distance[1], pose[0], pose[1], planning_factor_direction, SafetyFactorArray{}); } -void GoalPlannerModule::decideVelocity() +void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; // partial_paths - // TODO(soblin): only update velocity on main thread side, use that on main thread side - auto & first_path = thread_safe_data_.get_pull_over_path()->partial_paths().front(); + auto & first_path = pull_over_path.partial_paths().front(); const auto vel = static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); for (auto & p : first_path.points) { @@ -1359,21 +1455,19 @@ void GoalPlannerModule::decideVelocity() } } -BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & context_data) +BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto current_state = path_decision_controller_.get_current_state(); if (current_state.state != PathDecisionState::DecisionKind::DECIDED) { const bool is_stable_safe = current_state.is_stable_safe; - // clang-format off const std::string detail = - goal_candidates_.empty() ? "no goal candidate" : - context_data.pull_over_path_candidates.empty() ? "no path candidate" : - !thread_safe_data_.foundPullOverPath() ? "no static safe path" : - !is_stable_safe ? "unsafe against dynamic objects" : - "too far goal"; - // clang-format on + goal_candidates_.empty() ? "no goal candidate" + : context_data.lane_parking_response.pull_over_path_candidates.empty() ? "no path candidate" + : !context_data.pull_over_path_opt ? "no static safe path" + : !is_stable_safe ? "unsafe against dynamic objects" + : "too far goal"; return planPullOverAsCandidate(context_data, detail); } @@ -1381,12 +1475,12 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(const PullOverContextData & } BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( - const PullOverContextData & context_data, const std::string & detail) + PullOverContextData & context_data, const std::string & detail) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // if pull over path candidates generation is not finished, use previous module output - if (context_data.pull_over_path_candidates.empty()) { + if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } @@ -1413,8 +1507,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsCandidate( return output; } -BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( - const PullOverContextData & context_data) +BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData & context_data) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1422,16 +1515,15 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( start = std::chrono::system_clock::now(); // if pull over path candidates generation is not finished, use previous module output - if (context_data.pull_over_path_candidates.empty()) { + if (context_data.lane_parking_response.pull_over_path_candidates.empty()) { return getPreviousModuleOutput(); } - auto context_data_with_velocity = context_data; /** NOTE(soblin): this path originates from the previously selected(by main thread) pull_over_path which was originally generated by either road_parking or freespace thread */ - auto & pull_over_path_with_velocity_opt = context_data_with_velocity.pull_over_path_opt; + auto pull_over_path_with_velocity_opt = context_data.pull_over_path_opt; const bool is_freespace = pull_over_path_with_velocity_opt && pull_over_path_with_velocity_opt.value().type() == PullOverPlannerType::FREESPACE; @@ -1439,19 +1531,22 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( pull_over_path_with_velocity_opt ? std::make_optional(pull_over_path_with_velocity_opt.value().modified_goal()) : std::nullopt; + const auto & last_path_update_time = context_data.last_path_update_time; if ( path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::NOT_DECIDED && !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), - modified_goal_opt, thread_safe_data_.get_last_path_update_time(), *parameters_)) { + modified_goal_opt, last_path_update_time, *parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ - RCLCPP_DEBUG(getLogger(), "Update pull over path candidates"); + RCLCPP_INFO(getLogger(), "Update pull over path candidates"); - thread_safe_data_.clearPullOverPath(); + context_data.pull_over_path_opt = std::nullopt; + context_data.last_path_update_time = std::nullopt; + context_data.last_path_idx_increment_time = std::nullopt; // update goal candidates auto goal_candidates = goal_candidates_; @@ -1459,13 +1554,18 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( goal_candidates, occupancy_grid_map_, planner_data_, context_data.static_target_objects); // Select a path that is as safe as possible and has a high priority. - const auto & pull_over_path_candidates = context_data.pull_over_path_candidates; - auto path_and_goal_opt = - selectPullOverPath(context_data, pull_over_path_candidates, goal_candidates); + const auto & pull_over_path_candidates = + context_data.lane_parking_response.pull_over_path_candidates; + const auto lane_pull_over_path_opt = selectPullOverPath( + context_data, pull_over_path_candidates, goal_candidates, + context_data.lane_parking_response.sorted_bezier_indices_opt); // update thread_safe_data_ - if (path_and_goal_opt) { - const auto & pull_over_path = path_and_goal_opt.value(); + const auto & pull_over_path_opt = + lane_pull_over_path_opt ? lane_pull_over_path_opt + : context_data.freespace_parking_response.freespace_pull_over_path; + if (pull_over_path_opt) { + const auto & pull_over_path = pull_over_path_opt.value(); /** TODO(soblin): since thread_safe_data::pull_over_path was used as a global variable, old * code was setting deceleration to thread_safe_data::pull_over_path and setOutput() accessed * to the velocity profile in thread_safe_data::pull_over_path, which is a very bad usage of @@ -1476,7 +1576,10 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( * As the next action item, only set this selected pull_over_path to only * FreespaceThreadSafeData. */ - thread_safe_data_.set(pull_over_path); + context_data.pull_over_path_opt = pull_over_path; + context_data.last_path_update_time = clock_->now(); + context_data.last_path_idx_increment_time = std::nullopt; + if (pull_over_path_with_velocity_opt) { auto & pull_over_path_with_velocity = pull_over_path_with_velocity_opt.value(); // copy the path for later setOutput() @@ -1492,20 +1595,19 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput( // set output and status BehaviorModuleOutput output{}; - setOutput(context_data_with_velocity, output); + setOutput(pull_over_path_with_velocity_opt, context_data, output); // return to lane parking if it is possible - if (is_freespace && canReturnToLaneParking(context_data_with_velocity)) { - // TODO(soblin): return from freespace to lane is disabled temporarily, because if - // context_data_with_velocity contained freespace path, since lane_parking_pull_over_path is - // deleted, freespace path is set again - if (context_data_with_velocity.pull_over_path_opt) { - thread_safe_data_.set_pull_over_path(context_data_with_velocity.pull_over_path_opt.value()); + if (is_freespace && canReturnToLaneParking(context_data)) { + if (pull_over_path_with_velocity_opt) { + context_data.pull_over_path_opt = pull_over_path_with_velocity_opt; + context_data.last_path_update_time = clock_->now(); + context_data.last_path_idx_increment_time = std::nullopt; } } // For debug - setDebugData(context_data_with_velocity); + setDebugData(context_data); if (!pull_over_path_with_velocity_opt) { return output; @@ -1526,8 +1628,9 @@ void GoalPlannerModule::postProcess() getLogger(), *clock_, 5000, " [pull_over] postProcess() is called without valid context_data. use dummy context data."); } - const auto context_data_dummy = - PullOverContextData(true, PredictedObjects{}, PredictedObjects{}, std::nullopt, {}, {}); + const auto context_data_dummy = PullOverContextData( + true, PredictedObjects{}, PredictedObjects{}, PathDecisionState{}, false /*is _stopped*/, + LaneParkingResponse{}, FreespaceParkingResponse{}); const auto & context_data = context_data_.has_value() ? context_data_.value() : context_data_dummy; @@ -1535,7 +1638,6 @@ void GoalPlannerModule::postProcess() path_decision_controller_.get_current_state().state == PathDecisionState::DecisionKind::DECIDED; if (!context_data.pull_over_path_opt) { - context_data_ = std::nullopt; return; } const auto & pull_over_path = context_data.pull_over_path_opt.value(); @@ -1548,12 +1650,9 @@ void GoalPlannerModule::postProcess() updateSteeringFactor( context_data, {pull_over_path.start_pose(), pull_over_path.modified_goal_pose()}, - {distance_to_path_change.first, distance_to_path_change.second}, - has_decided_path ? SteeringFactor::TURNING : SteeringFactor::APPROACHING); - - setVelocityFactor(pull_over_path.full_path()); + {distance_to_path_change.first, distance_to_path_change.second}); - context_data_ = std::nullopt; + set_longitudinal_planning_factor(pull_over_path.full_path()); } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1567,7 +1666,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() " [pull_over] planWaitingApproval() is called without valid context_data. use fixed goal " "planner"); } else { - const auto & context_data = context_data_.value(); + auto & context_data = context_data_.value(); return planPullOverAsCandidate(context_data, "waiting approval"); } } @@ -1586,7 +1685,7 @@ std::pair GoalPlannerModule::calcDistanceToPathChange( } const auto & pull_over_path = context_data.pull_over_path_opt.value(); - const auto & full_path = context_data.pull_over_path_opt.value().full_path(); + const auto & full_path = pull_over_path.full_path(); const auto ego_segment_idx = autoware::motion_utils::findNearestSegmentIndex( full_path.points, planner_data_->self_odometry->pose.pose, std::numeric_limits::max(), @@ -1634,11 +1733,22 @@ PathWithLaneId GoalPlannerModule::generateStopPath( return PathWithLaneId{}; } - // extend previous module path to generate reference path for stop path + // Generate reference_path to extend the previous path. + // If pull_over_path is ARC_BACKWARD, generate path to the start_pose of the pull_over_path, + // otherwise, generate path to the goal_pose. + const auto & pull_over_path_opt = context_data.pull_over_path_opt; const auto reference_path = std::invoke([&]() -> PathWithLaneId { const auto s_current = lanelet::utils::getArcCoordinates(current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); - const double s_end = s_current + common_parameters.forward_path_length; + const bool is_arc_backward = + pull_over_path_opt.has_value() && + pull_over_path_opt.value().type() == PullOverPlannerType::ARC_BACKWARD; + const Pose path_end_pose = + is_arc_backward ? pull_over_path_opt.value().start_pose() : route_handler->getGoalPose(); + const double s_end = std::clamp( + lanelet::utils::getArcCoordinates(current_lanes, path_end_pose).length, + s_current + std::numeric_limits::epsilon(), + s_current + common_parameters.forward_path_length); return route_handler->getCenterLinePath(current_lanes, s_start, s_end, true); }); @@ -1666,12 +1776,11 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // difference between the outer and inner sides) // 4. feasible stop const auto stop_pose_opt = std::invoke([&]() -> std::optional { - if (context_data.pull_over_path_opt) - return context_data.pull_over_path_opt.value().start_pose(); - if (thread_safe_data_.get_closest_start_pose()) - return thread_safe_data_.get_closest_start_pose().value(); - if (!decel_pose) return std::nullopt; - return decel_pose.value(); + if (pull_over_path_opt) + return std::make_optional(pull_over_path_opt.value().start_pose()); + if (context_data.lane_parking_response.closest_start_pose) + return context_data.lane_parking_response.closest_start_pose; + return decel_pose; }); if (!stop_pose_opt.has_value()) { const auto feasible_stop_path = @@ -1743,46 +1852,38 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath( return stop_path; } -bool GoalPlannerModule::isStuck( +bool FreespaceParkingPlanner::isStuck( const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects, - const std::shared_ptr planner_data, - const std::shared_ptr occupancy_grid_map, - const GoalPlannerParameters & parameters) + const FreespaceParkingRequest & req) const { - const bool found_pull_over_path = thread_safe_data_.foundPullOverPath(); - const std::optional pull_over_path_opt = - found_pull_over_path ? std::make_optional(*thread_safe_data_.get_pull_over_path()) - : std::nullopt; + const auto & parameters = req.parameters_; + const auto & planner_data = req.get_planner_data(); const std::optional modified_goal_opt = - pull_over_path_opt - ? std::make_optional(pull_over_path_opt.value().modified_goal()) + req.get_pull_over_path() + ? std::make_optional(req.get_pull_over_path().value().modified_goal()) : std::nullopt; - const std::lock_guard lock(mutex_); if (isOnModifiedGoal(planner_data->self_odometry->pose.pose, modified_goal_opt, parameters)) { return false; } - constexpr double stuck_time = 5.0; - if (!isStopped( - odometry_buffer_stuck_, planner_data->self_odometry, stuck_time, - parameters_->th_stopped_velocity)) { + if (!req.is_stopped()) { return false; } - if (!found_pull_over_path) { + if (!req.get_pull_over_path()) { return true; } - const auto & pull_over_path = pull_over_path_opt.value(); if (parameters.use_object_recognition) { - const auto & path = pull_over_path.getCurrentPath(); + const auto & path = req.get_pull_over_path().value().getCurrentPath(); const auto curvatures = autoware::motion_utils::calcCurvature(path.points); + std::vector ego_polygons_expanded; if (goal_planner_utils::checkObjectsCollision( path, curvatures, static_target_objects, dynamic_target_objects, planner_data->parameters, parameters.object_recognition_collision_check_hard_margins.back(), /*extract_static_objects=*/false, parameters.maximum_deceleration, parameters.object_recognition_collision_check_max_extra_stopping_margin, - debug_data_.ego_polygons_expanded)) { + ego_polygons_expanded)) { return true; } } @@ -1790,7 +1891,7 @@ bool GoalPlannerModule::isStuck( if ( parameters.use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision( - thread_safe_data_.get_pull_over_path()->getCurrentPath(), occupancy_grid_map)) { + req.get_pull_over_path().value().getCurrentPath(), req.get_occupancy_grid_map())) { return true; } @@ -1805,9 +1906,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d return false; } - if (!isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity)) { + if (!ctx_data.is_stopped) { return false; } @@ -1824,10 +1923,12 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d // to prevent increment before driving // when the end of the current path is close to the current pose // this value should be `keep_stop_time` in keepStoppedWithCurrentPath + if (!ctx_data.last_path_update_time) { + return false; + } constexpr double keep_current_idx_time = 4.0; const bool has_passed_enough_time_from_increment = - (clock_->now() - *thread_safe_data_.get_last_path_update_time()).seconds() > - keep_current_idx_time; + (clock_->now() - ctx_data.last_path_update_time.value()).seconds() > keep_current_idx_time; if (!has_passed_enough_time_from_increment) { return false; } @@ -2433,8 +2534,8 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) add(createPathMarkerArray(pull_over_path.getCurrentPath(), "current_path", 0, 0.9, 0.5, 0.0)); // visualize each partial path - for (size_t i = 0; i < context_data.pull_over_path_opt.value().partial_paths().size(); ++i) { - const auto & partial_path = context_data.pull_over_path_opt.value().partial_paths().at(i); + for (size_t i = 0; i < pull_over_path.partial_paths().size(); ++i) { + const auto & partial_path = pull_over_path.partial_paths().at(i); add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } @@ -2567,48 +2668,4 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } } -void GoalPlannerModule::GoalPlannerData::initializeOccupancyGridMap( - const PlannerData & planner_data, const GoalPlannerParameters & parameters) -{ - OccupancyGridMapParam occupancy_grid_map_param{}; - const double margin = parameters.occupancy_grid_collision_check_margin; - occupancy_grid_map_param.vehicle_shape.length = - planner_data.parameters.vehicle_length + 2 * margin; - occupancy_grid_map_param.vehicle_shape.width = planner_data.parameters.vehicle_width + 2 * margin; - occupancy_grid_map_param.vehicle_shape.base2back = - planner_data.parameters.base_link2rear + margin; - occupancy_grid_map_param.theta_size = parameters.theta_size; - occupancy_grid_map_param.obstacle_threshold = parameters.obstacle_threshold; - occupancy_grid_map = std::make_shared(); - occupancy_grid_map->setParam(occupancy_grid_map_param); -} - -GoalPlannerModule::GoalPlannerData GoalPlannerModule::GoalPlannerData::clone() const -{ - GoalPlannerModule::GoalPlannerData gp_planner_data( - planner_data, parameters, last_previous_module_output); - gp_planner_data.update( - parameters, planner_data, current_status, previous_module_output, vehicle_footprint, - goal_candidates); - return gp_planner_data; -} - -void GoalPlannerModule::GoalPlannerData::update( - const GoalPlannerParameters & parameters_, const PlannerData & planner_data_, - const ModuleStatus & current_status_, const BehaviorModuleOutput & previous_module_output_, - const autoware::universe_utils::LinearRing2d & vehicle_footprint_, - const GoalCandidates & goal_candidates_) -{ - parameters = parameters_; - vehicle_footprint = vehicle_footprint_; - - planner_data = planner_data_; - planner_data.route_handler = std::make_shared(*(planner_data_.route_handler)); - current_status = current_status_; - last_previous_module_output = previous_module_output; - previous_module_output = previous_module_output_; - occupancy_grid_map->setMap(*(planner_data.occupancy_grid)); - goal_candidates = goal_candidates_; -} - } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 66192a00a99fa..b973cfd7ed549 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -214,12 +214,9 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p (transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontLeftIndex) + transformed_vehicle_footprint.at(vehicle_info_utils::VehicleInfo::FrontRightIndex)) / 2.0; - lanelet::ConstLanelet vehicle_front_closest_lanelet; - lanelet::utils::query::getClosestLanelet( - pull_over_lanes, search_pose, &vehicle_front_closest_lanelet); + const auto pull_over_lanelet = lanelet::utils::combineLaneletsShape(pull_over_lanes); const auto vehicle_front_pose_for_bound_opt = goal_planner_utils::calcClosestPose( - left_side_parking_ ? vehicle_front_closest_lanelet.leftBound() - : vehicle_front_closest_lanelet.rightBound(), + left_side_parking_ ? pull_over_lanelet.leftBound() : pull_over_lanelet.rightBound(), autoware::universe_utils::createPoint( vehicle_front_midpoint.x(), vehicle_front_midpoint.y(), search_pose.position.z)); if (!vehicle_front_pose_for_bound_opt) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index 6bbc34a495080..aeaf6836cdba3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -31,7 +31,7 @@ std::unique_ptr GoalPlannerModuleManager::createNewSceneMo { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( @@ -139,6 +139,7 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( const std::string ns = base_ns + "pull_over."; p.pull_over_minimum_request_length = node->declare_parameter(ns + "minimum_request_length"); + p.pull_over_prepare_length = node->declare_parameter(ns + "pull_over_prepare_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"); @@ -266,6 +267,15 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( p.rrt_star_parameters.margin = node->declare_parameter(ns + "margin"); } + // bezier parking + { + const std::string ns = base_ns + "pull_over.bezier_parking."; + p.bezier_parking.pull_over_angle_threshold = + node->declare_parameter(ns + "pull_over_angle_threshold"); + p.bezier_parking.after_shift_straight_distance = + node->declare_parameter(ns + "after_shift_straight_distance"); + } + // stop condition { p.maximum_deceleration_for_stop = diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp new file mode 100644 index 0000000000000..3347f2e4a6bdf --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/bezier_pull_over.cpp @@ -0,0 +1,428 @@ +// 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_goal_planner_module/pull_over_planner/bezier_pull_over.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 "autoware/behavior_path_planner_common/utils/utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +namespace autoware::behavior_path_planner +{ +BezierPullOver::BezierPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) +: PullOverPlannerBase(node, parameters), + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, + left_side_parking_(parameters.parking_policy == ParkingPolicy::LEFT_SIDE) +{ +} +std::optional BezierPullOver::plan( + [[maybe_unused]] const GoalCandidate & modified_goal_pose, [[maybe_unused]] const size_t id, + [[maybe_unused]] const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) +{ + const auto & route_handler = planner_data->route_handler; + const double min_jerk = parameters_.minimum_lateral_jerk; + const double max_jerk = parameters_.maximum_lateral_jerk; + const double backward_search_length = parameters_.backward_goal_search_length; + const double forward_search_length = parameters_.forward_goal_search_length; + const int shift_sampling_num = parameters_.shift_sampling_num; + [[maybe_unused]] const double jerk_resolution = + std::abs(max_jerk - min_jerk) / shift_sampling_num; + + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, + /*forward_only_in_route*/ false); + + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + + // find safe one from paths with different jerk + for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { + const auto pull_over_path = generateBezierPath( + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, + lateral_jerk); + if (!pull_over_path.empty()) { + return pull_over_path.at(0); + } + } + + return std::nullopt; +} + +std::vector BezierPullOver::plans( + [[maybe_unused]] const GoalCandidate & modified_goal_pose, + [[maybe_unused]] const size_t initial_id, + [[maybe_unused]] const std::shared_ptr planner_data, + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) +{ + const auto & route_handler = planner_data->route_handler; + const double min_jerk = parameters_.minimum_lateral_jerk; + const double max_jerk = parameters_.maximum_lateral_jerk; + const double backward_search_length = parameters_.backward_goal_search_length; + const double forward_search_length = parameters_.forward_goal_search_length; + const int shift_sampling_num = parameters_.shift_sampling_num; + [[maybe_unused]] const double jerk_resolution = + std::abs(max_jerk - min_jerk) / shift_sampling_num; + + const auto road_lanes = utils::getExtendedCurrentLanesFromPath( + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, + /*forward_only_in_route*/ false); + + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *route_handler, left_side_parking_, backward_search_length, forward_search_length); + if (road_lanes.empty() || pull_over_lanes.empty()) { + return {}; + } + + // find safe one from paths with different jerk + std::vector paths; + auto id = initial_id; + for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { + auto pull_over_paths = generateBezierPath( + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, + lateral_jerk); + id += pull_over_paths.size(); + std::copy( + std::make_move_iterator(pull_over_paths.begin()), + std::make_move_iterator(pull_over_paths.end()), std::back_inserter(paths)); + } + + return paths; +} + +std::vector BezierPullOver::generateBezierPath( + const GoalCandidate & goal_candidate, const size_t initial_id, + const std::shared_ptr planner_data, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, + const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const +{ + const double pull_over_velocity = parameters_.pull_over_velocity; + const double after_shift_straight_distance = + parameters_.bezier_parking.after_shift_straight_distance; + + const auto & goal_pose = goal_candidate.goal_pose; + + // shift end pose is longitudinal offset from goal pose to improve parking angle accuracy + const Pose shift_end_pose = + autoware::universe_utils::calcOffsetPose(goal_pose, -after_shift_straight_distance, 0, 0); + + // calculate lateral shift of previous module path terminal pose from road lane reference path + const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( + generateReferencePath(planner_data, road_lanes, shift_end_pose), + parameters_.center_line_path_interval); + const auto prev_module_path = utils::resamplePathWithSpline( + upstream_module_output.path, parameters_.center_line_path_interval); + const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; + + // process previous module path for path shifter input path + // case1) extend path if shift end pose is behind of previous module path terminal pose + // case2) crop path if shift end pose is ahead of previous module path terminal pose + const auto processed_prev_module_path = std::invoke([&]() -> std::optional { + const bool extend_previous_module_path = + lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose).length > + lanelet::utils::getArcCoordinates(road_lanes, prev_module_path_terminal_pose).length; + if (extend_previous_module_path) { // case1 + // NOTE: The previous module may insert a zero velocity at the end of the path, so remove it + // by setting remove_connected_zero_velocity=true. Inserting a velocity of 0 into the goal is + // the role of the goal planner, and the intermediate zero velocity after extension is + // unnecessary. + return goal_planner_utils::extendPath( + prev_module_path, road_lane_reference_path_to_shift_end, shift_end_pose, true); + } else { // case2 + return goal_planner_utils::cropPath(prev_module_path, shift_end_pose); + } + }); + if (!processed_prev_module_path || processed_prev_module_path->points.empty()) { + return {}; + } + + // calculate shift length + const Pose & shift_end_pose_prev_module_path = + processed_prev_module_path->points.back().point.pose; + const double shift_end_road_to_target_distance = + autoware::universe_utils::inverseTransformPoint( + shift_end_pose.position, shift_end_pose_prev_module_path) + .y; + + // calculate shift start pose on road lane + const double pull_over_distance = autoware::motion_utils::calc_longitudinal_dist_from_jerk( + shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); + const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( + processed_prev_module_path.value(), pull_over_distance, shift_end_road_to_target_distance); + const auto shift_start_pose = autoware::motion_utils::calcLongitudinalOffsetPose( + processed_prev_module_path->points, shift_end_pose_prev_module_path.position, + -before_shifted_pull_over_distance); + + std::vector> params; + const size_t n_sample_v_init = 3; + const size_t n_sample_v_final = 3; + const size_t n_sample_acc = 2; + for (unsigned i = 0; i <= n_sample_v_init; ++i) { + for (unsigned j = 0; j <= n_sample_v_final; j++) { + for (unsigned k = 0; k <= n_sample_acc; k++) { + const double v_init_coeff = i * (1.0 / n_sample_v_init); + const double v_final_coeff = j * 0.25 / (1.0 / n_sample_v_final); + const double acc_coeff = k * (10.0 / n_sample_acc); + params.emplace_back(v_init_coeff, v_final_coeff, acc_coeff); + } + } + } + + const auto drivable_lanes = + utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes); + const auto & dp = planner_data->drivable_area_expansion_parameters; + 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 combined_drivable = utils::combineDrivableLanes( + expanded_lanes, upstream_module_output.drivable_area_info.drivable_lanes); + const auto parking_lot_polygons = + lanelet::utils::query::getAllParkingLots(planner_data->route_handler->getLaneletMapPtr()); + + std::vector bezier_paths; + auto id = initial_id; + for (const auto & [v_init_coeff, v_final_coeff, acc_coeff] : params) { + // set path shifter and generate shifted path + PathShifter path_shifter{}; + path_shifter.setPath(processed_prev_module_path.value()); + ShiftLine shift_line{}; + shift_line.start = *shift_start_pose; + shift_line.end = shift_end_pose; + shift_line.end_shift_length = shift_end_road_to_target_distance; + path_shifter.addShiftLine(shift_line); + ShiftedPath shifted_path{}; + const bool offset_back = true; // offset front side from reference path + if (!path_shifter.generate(&shifted_path, offset_back)) { + continue; + } + const auto from_idx_opt = + autoware::motion_utils::findNearestIndex(shifted_path.path.points, *shift_start_pose); + const auto to_idx_opt = + autoware::motion_utils::findNearestIndex(shifted_path.path.points, shift_end_pose); + if (!from_idx_opt || !to_idx_opt) { + continue; + } + const auto from_idx = from_idx_opt.value(); + const auto to_idx = to_idx_opt.value(); + const auto span = static_cast( + std::max(static_cast(to_idx) - static_cast(from_idx) + 1, 0)); + const auto reference_curvature = + autoware::motion_utils::calcCurvature(processed_prev_module_path->points); + const auto & from_pose = shifted_path.path.points[from_idx].point.pose; + const auto & to_pose = shifted_path.path.points[to_idx].point.pose; + const autoware::sampler_common::State initial{ + {from_pose.position.x, from_pose.position.y}, + {0.0, 0.0}, + reference_curvature.at(from_idx), + tf2::getYaw(from_pose.orientation)}; + const autoware::sampler_common::State final{ + {to_pose.position.x, to_pose.position.y}, + {0.0, 0.0}, + reference_curvature.at(to_idx), + tf2::getYaw(to_pose.orientation)}; + // setting the initial velocity to higher gives straight forward path (the steering does not + // change) + const auto bezier_path = + bezier_sampler::sample(initial, final, v_init_coeff, v_final_coeff, acc_coeff); + const auto bezier_points = bezier_path.cartesianWithHeading(span); + for (unsigned i = 0; i + 1 < span; ++i) { + auto & p = shifted_path.path.points[from_idx + i]; + p.point.pose.position.x = bezier_points[i].x(); + p.point.pose.position.y = bezier_points[i].y(); + p.point.pose.orientation = + universe_utils::createQuaternionFromRPY(0.0, 0.0, bezier_points[i].z()); + } + + // interpolate between shift end pose to goal pose + for (const auto & toward_goal_straight_pose : + utils::interpolatePose(shift_end_pose, goal_pose, 0.5)) { + PathPointWithLaneId p = shifted_path.path.points.back(); + p.point.pose = toward_goal_straight_pose; + shifted_path.path.points.push_back(p); + } + + shifted_path.path.points = + autoware::motion_utils::removeOverlapPoints(shifted_path.path.points); + /* insertOrientation erases original goal pose, so not used + autoware::motion_utils::insertOrientation(shifted_path.path.points, true); + */ + + // combine road lanes and shoulder lanes to find closest lanelet id + const auto lanes = std::invoke([&]() -> lanelet::ConstLanelets { + auto lanes = road_lanes; + lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); + return lanes; // not copy the value (Return Value Optimization) + }); + + // set goal pose with velocity 0 + { + PathPointWithLaneId p{}; + p.point.longitudinal_velocity_mps = 0.0; + p.point.pose = goal_pose; + lanelet::Lanelet goal_lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanes, goal_pose, &goal_lanelet)) { + p.lane_ids = {goal_lanelet.id()}; + } else { + p.lane_ids = shifted_path.path.points.back().lane_ids; + } + shifted_path.path.points.push_back(p); + } + + // set lane_id and velocity to shifted_path + for (size_t i = path_shifter.getShiftLines().front().start_idx; + i < shifted_path.path.points.size() - 1; ++i) { + auto & point = shifted_path.path.points.at(i); + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); + lanelet::Lanelet lanelet{}; + if (lanelet::utils::query::getClosestLanelet(lanes, point.point.pose, &lanelet)) { + point.lane_ids = {lanelet.id()}; // overwrite lane_ids + } else { + point.lane_ids = shifted_path.path.points.at(i - 1).lane_ids; + } + } + + // set pull over path + auto pull_over_path_opt = PullOverPath::create( + getPlannerType(), id, {shifted_path.path}, path_shifter.getShiftLines().front().start, + goal_candidate, {std::make_pair(pull_over_velocity, 0)}); + + if (!pull_over_path_opt) { + continue; + } + auto & pull_over_path = pull_over_path_opt.value(); + + // check if the parking path will leave drivable area and lanes + const bool is_in_parking_lots = std::invoke([&]() -> bool { + const auto path_footprints = goal_planner_utils::createPathFootPrints( + pull_over_path.parking_path(), planner_data->parameters.base_link2front, + planner_data->parameters.base_link2rear, planner_data->parameters.vehicle_width); + const auto is_footprint_in_any_polygon = [&parking_lot_polygons](const auto & footprint) { + return std::any_of( + parking_lot_polygons.begin(), parking_lot_polygons.end(), + [&footprint](const auto & polygon) { + return lanelet::geometry::within( + footprint, lanelet::utils::to2D(polygon).basicPolygon()); + }); + }; + return std::all_of( + path_footprints.begin(), path_footprints.end(), + [&is_footprint_in_any_polygon](const auto & footprint) { + return is_footprint_in_any_polygon(footprint); + }); + }); + const bool is_in_lanes = std::invoke([&]() -> bool { + return !lane_departure_checker_.checkPathWillLeaveLane( + utils::transformToLanelets(combined_drivable), pull_over_path.parking_path()); + }); + if (!is_in_parking_lots && !is_in_lanes) { + continue; + } + bezier_paths.push_back(std::move(pull_over_path)); + id++; + } + + return bezier_paths; +} + +PathWithLaneId BezierPullOver::generateReferencePath( + const std::shared_ptr planner_data, const lanelet::ConstLanelets & road_lanes, + const Pose & end_pose) const +{ + const auto & route_handler = planner_data->route_handler; + const Pose & current_pose = planner_data->self_odometry->pose.pose; + const double backward_path_length = planner_data->parameters.backward_path_length; + const double pull_over_velocity = parameters_.pull_over_velocity; + const double deceleration_interval = parameters_.deceleration_interval; + + const auto current_road_arc_coords = lanelet::utils::getArcCoordinates(road_lanes, current_pose); + const double s_start = current_road_arc_coords.length - backward_path_length; + const double s_end = std::max( + lanelet::utils::getArcCoordinates(road_lanes, end_pose).length, + s_start + std::numeric_limits::epsilon()); + auto road_lane_reference_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); + + // decelerate velocity linearly to minimum pull over velocity + // (or keep original velocity if it is lower than pull over velocity) + for (auto & point : road_lane_reference_path.points) { + const auto arclength = lanelet::utils::getArcCoordinates(road_lanes, point.point.pose).length; + const double distance_to_pull_over_start = + std::clamp(s_end - arclength, 0.0, deceleration_interval); + const auto decelerated_velocity = static_cast( + distance_to_pull_over_start / deceleration_interval * + (point.point.longitudinal_velocity_mps - pull_over_velocity) + + pull_over_velocity); + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, decelerated_velocity); + } + return road_lane_reference_path; +} + +double BezierPullOver::calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr) +{ + // reverse path for checking from the end point + // note that the sign of curvature is also reversed + PathWithLaneId reversed_path{}; + std::reverse_copy( + path.points.begin(), path.points.end(), std::back_inserter(reversed_path.points)); + + double after_arc_length{0.0}; + + const auto curvature_and_segment_length = + autoware::motion_utils::calcCurvatureAndSegmentLength(reversed_path.points); + + for (size_t i = 0; i < curvature_and_segment_length.size(); ++i) { + const auto & [k, segment_length_pair] = curvature_and_segment_length[i]; + + // If it is the last point, add the lengths of the previous and next segments. + // For other points, only add the length of the previous segment. + const double segment_length = i == curvature_and_segment_length.size() - 1 + ? segment_length_pair.first + : segment_length_pair.first + segment_length_pair.second; + + // after shifted segment length + const double after_segment_length = + k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); + if (after_arc_length + after_segment_length > target_after_arc_length) { + const double offset = target_after_arc_length - after_arc_length; + after_arc_length += k > 0 ? offset * (1 + k * dr) : offset / (1 - k * dr); + return after_arc_length; + } + after_arc_length += after_segment_length; + } + + return after_arc_length; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp index b51d5df42c75b..6605d258a8d5e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp @@ -32,9 +32,7 @@ namespace autoware::behavior_path_planner using autoware::freespace_planning_algorithms::AstarSearch; using autoware::freespace_planning_algorithms::RRTStar; -FreespacePullOver::FreespacePullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +FreespacePullOver::FreespacePullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE}, @@ -45,7 +43,7 @@ FreespacePullOver::FreespacePullOver( } { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( - vehicle_info, parameters.vehicle_shape_margin); + vehicle_info_, parameters.vehicle_shape_margin); if (parameters.freespace_parking_algorithm == "astar") { planner_ = std::make_unique( parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters, @@ -60,12 +58,10 @@ FreespacePullOver::FreespacePullOver( std::optional FreespacePullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const Pose & current_pose = planner_data->self_odometry->pose.pose; - planner_->setMap(*planner_data->costmap); - // offset goal pose to make straight path near goal for improving parking precision // todo: support straight path when using back constexpr double straight_distance = 1.0; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp index f75358601d877..b7b4ad83de362 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/geometric_pull_over.cpp @@ -26,11 +26,15 @@ namespace autoware::behavior_path_planner { GeometricPullOver::GeometricPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker, const bool is_forward) + rclcpp::Node & node, const GoalPlannerParameters & parameters, const bool is_forward) : PullOverPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_{lane_departure_checker}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, is_forward_{is_forward}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { @@ -40,7 +44,7 @@ GeometricPullOver::GeometricPullOver( std::optional GeometricPullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - [[maybe_unused]] const BehaviorModuleOutput & previous_module_output) + [[maybe_unused]] const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; @@ -78,9 +82,6 @@ std::optional GeometricPullOver::plan( auto pull_over_path_opt = PullOverPath::create( getPlannerType(), id, planner_.getPaths(), planner_.getStartPose(), modified_goal_pose, planner_.getPairsTerminalVelocityAndAccel()); - if (!pull_over_path_opt) { - return {}; - } - return pull_over_path_opt.value(); + return pull_over_path_opt; } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp index 31d8afffbdabe..acb6ee1926a83 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/pull_over_planner_base.cpp @@ -13,7 +13,9 @@ // limitations under the License. #include +#include +#include #include #include @@ -78,11 +80,24 @@ std::optional PullOverPath::create( std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); std::tie(parking_path_curvatures, parking_path_max_curvature) = calculateCurvaturesAndMax(parking_path); + const double parking_path_curvature_total_derivative = [&]() { + const auto parking_path_curvature_base = autoware::motion_utils::calcSignedArcLengthPartialSum( + parking_path.points, 1, parking_path.points.size() - 1); + const auto size = std::min(parking_path_curvatures.size(), parking_path_curvature_base.size()); + double total_abs_kappa_diff = 0; + for (unsigned i = 0; i + 1 < size; ++i) { + total_abs_kappa_diff += std::fabs( + (parking_path_curvatures.at(i + 1) - parking_path_curvatures.at(i)) / + (parking_path_curvature_base.at(i + 1) - parking_path_curvature_base.at(i))); + } + return total_abs_kappa_diff; + }(); return PullOverPath( type, id, start_pose, modified_goal_pose, partial_paths, full_path, parking_path, full_path_curvatures, parking_path_curvatures, full_path_max_curvature, - parking_path_max_curvature, pairs_terminal_velocity_and_accel); + parking_path_max_curvature, parking_path_curvature_total_derivative, + pairs_terminal_velocity_and_accel); } PullOverPath::PullOverPath(const PullOverPath & other) @@ -108,7 +123,7 @@ PullOverPath::PullOverPath( const PathWithLaneId & full_path, const PathWithLaneId & parking_path, const std::vector & full_path_curvatures, const std::vector & parking_path_curvatures, const double full_path_max_curvature, - const double parking_path_max_curvature, + const double parking_path_max_curvature, const double parking_path_curvature_total_derivative, const std::vector> & pairs_terminal_velocity_and_accel) : type_(type), modified_goal_pose_(modified_goal_pose), @@ -121,6 +136,7 @@ PullOverPath::PullOverPath( parking_path_curvatures_(parking_path_curvatures), full_path_max_curvature_(full_path_max_curvature), parking_path_max_curvature_(parking_path_max_curvature), + parking_path_curvature_total_derivative_(parking_path_curvature_total_derivative), path_idx_(0), pairs_terminal_velocity_and_accel_(pairs_terminal_velocity_and_accel) { diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp index 501502125d7ee..c8e0788e39b6f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp @@ -30,18 +30,21 @@ namespace autoware::behavior_path_planner { -ShiftPullOver::ShiftPullOver( - rclcpp::Node & node, const GoalPlannerParameters & parameters, - const LaneDepartureChecker & lane_departure_checker) +ShiftPullOver::ShiftPullOver(rclcpp::Node & node, const GoalPlannerParameters & parameters) : PullOverPlannerBase{node, parameters}, - lane_departure_checker_{lane_departure_checker}, + lane_departure_checker_{[&]() { + auto lane_departure_checker_params = lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + return LaneDepartureChecker{lane_departure_checker_params, vehicle_info_}; + }()}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } std::optional ShiftPullOver::plan( const GoalCandidate & modified_goal_pose, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output) + const BehaviorModuleOutput & upstream_module_output) { const auto & route_handler = planner_data->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -52,7 +55,7 @@ std::optional ShiftPullOver::plan( const double jerk_resolution = std::abs(max_jerk - min_jerk) / shift_sampling_num; const auto road_lanes = utils::getExtendedCurrentLanesFromPath( - previous_module_output.path, planner_data, backward_search_length, forward_search_length, + upstream_module_output.path, planner_data, backward_search_length, forward_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( @@ -64,10 +67,10 @@ std::optional ShiftPullOver::plan( // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { const auto pull_over_path = generatePullOverPath( - modified_goal_pose, id, planner_data, previous_module_output, road_lanes, pull_over_lanes, + modified_goal_pose, id, planner_data, upstream_module_output, road_lanes, pull_over_lanes, lateral_jerk); if (!pull_over_path) continue; - return *pull_over_path; + return pull_over_path; } return {}; @@ -134,7 +137,7 @@ std::optional ShiftPullOver::cropPrevModulePath( std::optional ShiftPullOver::generatePullOverPath( const GoalCandidate & goal_candidate, const size_t id, const std::shared_ptr planner_data, - const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes, + const BehaviorModuleOutput & upstream_module_output, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; @@ -151,7 +154,7 @@ std::optional ShiftPullOver::generatePullOverPath( generateReferencePath(planner_data, road_lanes, shift_end_pose), parameters_.center_line_path_interval); const auto prev_module_path = utils::resamplePathWithSpline( - previous_module_output.path, parameters_.center_line_path_interval); + upstream_module_output.path, parameters_.center_line_path_interval); const auto prev_module_path_terminal_pose = prev_module_path.points.back().point.pose; // process previous module path for path shifter input path diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp new file mode 100644 index 0000000000000..c334fec7a1bd9 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp @@ -0,0 +1,67 @@ +// 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 + +#include + +namespace autoware::behavior_path_planner +{ + +void LaneParkingRequest::update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const BehaviorModuleOutput & upstream_module_output, + const std::optional & pull_over_path, const PathDecisionState & prev_data, + const bool trigger_thread_on_approach) +{ + planner_data_ = std::make_shared(planner_data); + planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); + current_status_ = current_status; + upstream_module_output_ = upstream_module_output; + pull_over_path_ = pull_over_path; + prev_data_ = prev_data; + trigger_thread_on_approach_ = trigger_thread_on_approach; +} + +void FreespaceParkingRequest::initializeOccupancyGridMap( + const PlannerData & planner_data, const GoalPlannerParameters & parameters) +{ + OccupancyGridMapParam occupancy_grid_map_param{}; + const double margin = parameters.occupancy_grid_collision_check_margin; + occupancy_grid_map_param.vehicle_shape.length = + planner_data.parameters.vehicle_length + 2 * margin; + occupancy_grid_map_param.vehicle_shape.width = planner_data.parameters.vehicle_width + 2 * margin; + occupancy_grid_map_param.vehicle_shape.base2back = + planner_data.parameters.base_link2rear + margin; + occupancy_grid_map_param.theta_size = parameters.theta_size; + occupancy_grid_map_param.obstacle_threshold = parameters.obstacle_threshold; + occupancy_grid_map_ = std::make_shared(); + occupancy_grid_map_->setParam(occupancy_grid_map_param); +} + +void FreespaceParkingRequest::update( + const PlannerData & planner_data, const ModuleStatus & current_status, + const std::optional & pull_over_path, + const std::optional & last_path_update_time, const bool is_stopped) +{ + planner_data_ = std::make_shared(planner_data); + planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); + current_status_ = current_status; + occupancy_grid_map_->setMap(*(planner_data_->occupancy_grid)); + pull_over_path_ = pull_over_path; + last_path_update_time_ = last_path_update_time; + is_stopped_ = is_stopped; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index a2a64fc700676..fb39a34c9ac91 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -831,4 +831,69 @@ std::optional calcClosestPose( return closest_pose; } +autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects( + const autoware_perception_msgs::msg::PredictedObjects & original_objects, + const route_handler::RouteHandler & route_handler, const GoalPlannerParameters & parameters, + const double vehicle_width) +{ + const bool left_side_parking = parameters.parking_policy == ParkingPolicy::LEFT_SIDE; + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + route_handler, left_side_parking, parameters.backward_goal_search_length, + parameters.forward_goal_search_length); + const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon( + pull_over_lanes, left_side_parking, parameters.detection_bound_offset, + parameters.margin_from_boundary + parameters.max_lateral_offset + vehicle_width); + + PredictedObjects dynamic_target_objects{}; + for (const auto & object : original_objects.objects) { + const auto object_polygon = universe_utils::toPolygon2d(object); + if ( + objects_extraction_polygon.has_value() && + boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) { + dynamic_target_objects.objects.push_back(object); + } + } + return dynamic_target_objects; +} + +bool is_goal_reachable_on_path( + const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking) +{ + const Pose goal_pose = route_handler.getOriginalGoalPose(); + const auto getNeighboringLane = + [&](const lanelet::ConstLanelet & lane) -> std::optional { + return left_side_parking ? route_handler.getLeftLanelet(lane, false, true) + : route_handler.getRightLanelet(lane, false, true); + }; + lanelet::ConstLanelets goal_check_lanes = current_lanes; + for (const auto & lane : current_lanes) { + auto neighboring_lane = getNeighboringLane(lane); + while (neighboring_lane) { + goal_check_lanes.push_back(neighboring_lane.value()); + neighboring_lane = getNeighboringLane(neighboring_lane.value()); + } + } + const bool goal_is_in_current_segment_lanes = std::any_of( + goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) { + return lanelet::utils::isInLanelet(goal_pose, lane); + }); + + // check that goal is in current neighbor shoulder lane + const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() { + for (const auto & lane : current_lanes) { + const auto shoulder_lane = left_side_parking ? route_handler.getLeftShoulderLanelet(lane) + : route_handler.getRightShoulderLanelet(lane); + if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) { + return true; + } + } + return false; + }); + + // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner, + // because goal arc coordinates cannot be calculated. + return goal_is_in_current_segment_lanes || goal_is_in_current_shoulder_lanes; +} + } // namespace autoware::behavior_path_planner::goal_planner_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp index de231b78041b4..472adb2d57944 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp @@ -92,7 +92,7 @@ TEST_F(TestUtilWithMap, getBusStopAreaPolygons) const auto shoulder_lanes = lanelet::utils::query::shoulderLanelets(lanes); const auto bus_stop_area_polygons = autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons(shoulder_lanes); - EXPECT_EQ(bus_stop_area_polygons.size(), 1); + EXPECT_EQ(bus_stop_area_polygons.size(), 2); } TEST_F(DISABLED_TestUtilWithMap, isWithinAreas) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CHANGELOG.rst index 42f6ddf841fec..92affbcfac9cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CHANGELOG.rst @@ -2,6 +2,219 @@ Changelog for package autoware_behavior_path_lane_change_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* docs(lane_change): update lane change documentation (`#9949 `_) + * update lane change requirements documentation + * remove unused function getNumToPreferredLane + * update candidate path generation documentation + * update prepare phase and lane changing phase documentation + * update longitudinal acceleration sampling documentation + * add prepare duration sampling documentation + * update candidate path validity and safety documentation + * fix formatting + * update image and fix formatting + * add overtaking turn lane documentation + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + * add LC global flowchart to documentation + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> + * reorganize lane change documentation + * fix section title + * add global flowchart description + * add warning + * apply pre-commit checks + * fix spelling + * edit some descriptions + --------- + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> +* docs(lane_change): object filtering description (`#9947 `_) + * docs(lane_change): object filtering description + * Move section up + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> + --------- + Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> +* refactor(behavior_path_planner): common test functions (`#9963 `_) + * feat: common test code in behavior_path_planner + * deal with other modules + * fix typo + * update + --------- +* refactor(lane_change): add missing safety check parameter (`#9928 `_) + * refactor(lane_change): parameterize incoming object yaw threshold + * Readme + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + * Add missing parameters + * missing dot + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> + * update readme + --------- + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(lane_change): ensure path generation doesn't exceed time limit (`#9908 `_) + * add time limit for lane change candidate path generation + * apply time limit for frenet method as well + * ensure param update value is valid + * fix param update initial value + * fix spelling + * fix param update initial values + --------- +* feat(lane_change_module): add update paramter function for non defined paramters (`#9887 `_) + * feat(lane_change_module): add new parameters for collision check and delay lane change functionality + * feat(lane_change_module): add validation for longitudinal and lateral acceleration sampling parameters + * feat(lane_change): update parameter handling and add lateral acceleration mapping + --------- +* feat(lane_change): using frenet planner to generate lane change path when ego near terminal (`#9767 `_) + * frenet planner + * minor refactoring + * adding parameter + * Add diff th param + * limit curvature for prepare segment + * minor refactoring + * print average curvature + * refactor + * filter the path directly + * fix some conflicts + * include curvature smoothing + * document + * fix image folder + * image size + * doxygen + * add debug for state + * use sign function instead + * rename argument + * readme + * fix failed test due to empty value + * add additional note + * fix conflict + --------- +* feat(lane_change): append candidate path index to metric debug table (`#9885 `_) + add candidate path index to metrics debug table +* docs(lane_change): fix broken link (`#9892 `_) +* docs(lane_change): explaining cancel and abort process (`#9845 `_) + * docs(lane_change): explaining cancel and abort process + * slight fix in formatting + * rephrase sentence + * rephrase and replace image for cancel + * Cancel explanations and limitations + * revise abort figure + * revise flow chart + * rephase sentence + * minor fix + * finish up + * offers change to reduces for negative connotation + * minor fix + * move limitation all the way down + * precommit + * equation mistake + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> + * rename subheading + --------- + Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> +* refactor(lane_change): refactor transit failure function (`#9835 `_) + * refactor(lane_change): refactor transit failure function + * fixed failed scenario + * remove is abort from debug + * set is abort state + * add comments for clarity + * include what you use. + --------- +* feat(lane_change): implement terminal lane change feature (`#9592 `_) + * implement function to compute terminal lane change path + * push terminal path to candidate paths if no other valid candidate path is found + * use terminal path in LC interface planWaitingApproval function + * set lane changing longitudinal accel to zero for terminal lc path + * rename function + * chore: rename codeowners file + * remove unused member variable prev_approved_path\_ + * refactor stop point insertion for terminal lc path + * add flag to enable/disable terminal path feature + * update README + * add parameter to configure stop point placement + * compute terminal path only when near terminal start + * add option to disable feature near goal + * set default flag value to false + * add documentation for terminal lane change path + * ensure actual prepare duration is always above minimum prepare duration threshold + * explicitly return std::nullopt + * Update planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp + Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> + * fix assignment + * fix spelling + * fix merge errors + --------- + Co-authored-by: tomoya.kimura + Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> +* feat(lane_change): add text display for candidate path sampling metrics (`#9810 `_) + * display candidate path sampling metrics on rviz + * rename struct + --------- +* feat(lane_change): revise current lane objects filtering (`#9785 `_) + * consider stopped front objects + * simplify computation of dist to front current lane object + * add flag to enable/disable keeping distance from front stopped vehicle + * fix object filtering test + --------- +* refactor(lane_change): replace sstream to fmt for marker's text (`#9775 `_) +* feat(lane_change): add info text to virtual wall (`#9783 `_) + * specify reason for lane change stop line + * add stop reason for incoming rear object + --------- +* fix(lane_change): add metrics to valid paths visualization (`#9737 `_) + * fix(lane_change): add metrics to valid paths visualization + * fix cpp-check error + --------- +* refactor(lane_change): separate path-related function to utils/path (`#9633 `_) + * refactor(lane_change): separate path-related function to utils/path + * remove old terminal lane change computation + * doxygen comments + * remove frenet planner header + * minor refactoring by throwing instead + * minor refactoring + * fix docstring and remove redundant argument + * get logger in header + * add docstring + * rename function is_colliding + * Fix failing test + * fix for failing scenario caused by prepare velocity + * fix error message + --------- +* fix(lane_change): fix prepare length too short at low speed (RT1-8909) (`#9735 `_) + fix prepare length too short at low speed (RT1-8909) +* refactor(lane_change): separate structs to different folders (`#9625 `_) +* fix(lane_change): remove overlapping preceding lanes (`#9526 `_) + * fix(lane_change): remove overlapping preceding lanes + * fix cpp check + * start searching disconnected lanes directly + * just remove starting from overlapped found + * return non reversed lanes + * fix precommit + --------- +* Contributors: Fumiya Watanabe, Kyoichi Sugahara, Mamoru Sobue, Takayuki Murooka, Zulfaqar Azmi, mkquda + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt index d1b6bfed868f0..0d56a28aaf450 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/utils/calculation.cpp src/utils/markers.cpp src/utils/utils.cpp + src/utils/path.cpp ) if(BUILD_TESTING) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index 86d8d1c0c1413..ba90713a221d9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -1,24 +1,141 @@ # 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`. - - The ego-vehicle isn't approaching a traffic light. (condition parameterized) - - The ego-vehicle isn't approaching a crosswalk. (condition parameterized) - - The ego-vehicle isn't approaching an intersection. (condition parameterized) -- 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 module is activated when lane change is needed (Ego is not on preferred lane), and activation requirements are satisfied. + +## Lane Change Requirements + +### Prerequisites + +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` + +### Activation Conditions + +The lane change module will activate under the following conditions : + +- The ego-vehicle is NOT on a `preferred_lane`. +- Distance to start of `target_lane` is less than `maximum_prepare_length` +- The ego-vehicle is NOT close to a regulatory element: + - Distance to next regulatory element is greater than `maximum_prepare_length`. + - Considers distance to traffic light. (If param `regulation.traffic_light` is enabled) + - Considers distance to crosswalk. (If param `regulation.crosswalk` is enabled) + - Considers distance to intersection. (If param `regulation.intersection` is enabled) + +!!! warning + + If ego vehicle is stuck, lane change will be enabled near crosswalk/intersection. Ego is considered stuck if it stops more than `stuck_detection.stop_time`. Ego is considered to be stopping if its velocity is smaller than `stuck_detection.velocity`. + +The following figure illustrates the logic for checking if lane change is required: + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE + +start + +if (Is data available AND lanes available) then (no) + #LightPink:False; + stop +else (yes) +endif + +:Get distance to target lane start; + +if (Is dist to target lane start < max prepare dist) then (no) + #LightPink:False; + stop +else (yes) +endif + +:Get distance to next regulatory element; + +if (Is dist to next regulatory element < max prepare dist) then (yes) + #LightPink:False; + stop +else (no) +end if + +#LightGreen:True; +stop + +@enduml +``` + +### Ready Conditions + +- Valid lane change path is found. +- Lane change path is safe; does not collide with other dynamic objects. +- Lane change candidate path is approved by an operator. + +## Implementation + +Lane change module uses a sampling based approach for generating a valid and safe lane changing trajectory. The process for generating the trajectory includes object filtering, metrics sampling, candidate paths generation, and lastly candidate paths evaluation. +Additionally the lane change module is responsible for turn signal activation when appropriate, and inserting a stop point when necessary. + +### Global Flowchart + +The following diagram, illustrates the overall flow of the lane change module implementation. + +```plantuml +@startuml + +start + +#LightBlue:Update lane change data; +#LightBlue:Filter detected objects; +if (Is lane change required?) then (yes) + #LightBlue:Activate turn signal; + #LightBlue:Generate metrics samples; + if (Is valid metrics available) then (yes) + While (Can generate new candidate path?) is (TRUE) + if (Candidate path is valid & safe?) then (yes) + #LightBlue:Generate drivable area; + #LightBlue:Execute lane change path; + while (Can module transit to success state?) is (False) + if (Can module trasit to failure state?) then (yes) + if (Can cancel or abort?) then (yes) + #LightBlue:Cancel/Abort; + #Orange:Restart process; + stop + else (no) + endif + else (no) + endif + endwhile (True) + #LightGreen:SUCCESS; + stop + else (no) + endif + endwhile (FALSE) + else (no) + endif +else (no) +endif +#LightBlue:Insert stop point; +#LightBlue:Execute previous approved path; +#Orange:Restart process; +stop + +@enduml +``` + +The lane change module first updates the necessary data for lane change such as lanes information and transient data. Then filters the detected objects to be used for safety evaluation (see [Object Filtering](#object-filtering)). + +If the lane change requirements are met, the turn signal is activated and the lane change module will proceed to generating a candidate path. Lane change candidate paths are generated by sampling different metrics and evaluating the validity of the corresponding generated trajectory. More details can be found in [Generating Lane Change Candidate Path](#generating-lane-change-candidate-path); + +When a valid candidate path is generated, a safety evaluation is conducted to check for any risk of collision or hindrance. The details of the safety evaluation can be found in [Safety Checks](#safety-checks). + +Once a valid and safe candidate path is found, the drivable area is generated and the path is executed. While executing the lane change maneuver, the safety of the approved path is continuously monitored to ensure there is no chance of collision or other hindrance. If the approved path remains safe and completion checks are met (see [Lane Change Completion Checks](#lane-change-completion-checks)) the module will transit to **SUCCESS** state. +In case the approved path is deemed to be no longer safe, the lane change module will attempt to abort the lane change maneuver (see [Aborting Lane Change](#aborting-lane-change)). When the lane change maneuver is aborted successfully the module will transit to **FAILURE** state, and the process is restarted. + +If the lane change module fails to find a valid and safe candidate path, the module will continue executing the previously approved path and insert a stop point along the path where appropriate, for more details refer to [Stopping Behavior](#stopping-behavior). + +### 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. @@ -43,39 +160,62 @@ endif :Calculate prepare phase metrics; -:Start loop through prepare phase metrics; - -repeat - :Get prepare segment; - - if (Is LC start point outside target lanes range) then (yes) - if (Is found a valid path) then (yes) - #Orange:Return first valid path; - stop - else (no) - #LightPink:Return prev approved path; - stop - endif - else (no) - endif - - :Calculate shift phase metrics; - - :Start loop through shift phase metrics; - repeat - :Get candidate path; - note left: Details shown in the next chart - if (Is valid candidate path?) then (yes) - :Store candidate path; - if (Is candidate path safe?) then (yes) - #LightGreen:Return candidate path; - stop +if(Is near terminal AND frenet enabled) then (yes) + group Frenet Candidate Paths #Lavender + :Calculate frenet candidates; + :Start loop through frenet candidates; + repeat + :Get candidate path; + if (Is valid candidate path?) then (yes) + if (Is candidate path safe?) then (yes) + #LightGreen:Return candidate path; + stop + else (no) + if (Is first valid path) then (yes) + :Store candidate path; + else (no) + endif + endif else (no) endif - else (no) - endif - repeat while (Finished looping shift phase metrics) is (FALSE) - repeat while (Is finished looping prepare phase metrics) is (FALSE) + repeat while (Finished looping frenet candidates) is (FALSE) + end group +else (no) + group Path Shifter Candidate Paths #LightCyan + :Start loop through prepare phase metrics; + repeat + :Get prepare segment; + + if (Is LC start point outside target lanes range) then (yes) + if (Is found a valid path) then (yes) + #Orange:Return first valid path; + stop + else (no) + #LightPink:Return prev approved path; + stop + endif + else (no) + endif + + :Calculate shift phase metrics; + + :Start loop through shift phase metrics; + repeat + :Get candidate path; + note left: Details shown in the next chart + if (Is valid candidate path?) then (yes) + :Store candidate path; + if (Is candidate path safe?) then (yes) + #LightGreen:Return candidate path; + stop + else (no) + endif + else (no) + endif + repeat while (Finished looping shift phase metrics) is (FALSE) + repeat while (Is finished looping prepare phase metrics) is (FALSE) + endgroup +endif if (Is found a valid path) then (yes) #Orange:Return first valid path; @@ -89,7 +229,7 @@ stop @enduml ``` -While the following chart demonstrates the process of generating a valid candidate path. +The following chart demonstrates the process of generating a valid candidate path with path shifter method. ```plantuml @startuml @@ -126,37 +266,76 @@ endif @enduml ``` -### Preparation phase +#### Prepare 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. +The prepare phase is the first section of the lane change candidate path and the corresponding prepare segment consists of a subsection of the current reference path along the current lane. The length of the prepare phase trajectory is computed as follows. ```C++ -lane_change_prepare_distance = current_speed * lane_change_prepare_duration + 0.5 * deceleration * lane_change_prepare_duration^2 +prepare_distance = current_speed * prepare_duration + 0.5 * lon_acceleration * 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`. +The prepare phase trajectory is valid if: -### Lane-changing phase +- The length of the prepare phase trajectory is greater than the distance to start of target lane +- The length of the prepare phase trajectory is less than the distance to terminal start point -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. +#### Lane-changing Phase + +The lane-changing phase consists of the shifted path that moves ego from current lane to the target lane. Total duration of lane-changing phase is computed from the `shift_length`, `lateral_jerk` and `lateral_acceleration`. + +In principle, positive longitudinal acceleration is considered during lane-changing phase, and is computed as follows. + +```C++ +lane_changing_acceleration = std::clamp((max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, + 0.0, prepare_longitudinal_acc); +``` + +Where `max_path_velocity` is the current path speed limit. + +!!! warning + + If the longitudinal acceleration of prepare phase is negative (slowing down), AND ego is near terminal, then the lane-changing longitudinal acceleration will also be negative and its value is decided by the parameter `lane_changing_decel_factor`. + +The lane changing distance is then computed as follows. ```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 +lane_changing_distance = initial_lane_changing_velocity * lane_changing_duration + 0.5 * lon_acceleration * lane_changing_duration^2 ``` 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) +#### Sampling Multiple Candidate Paths + +In order to find a valid and safe lane change path it might be necessary to generate multiple candidate path samples. The lane change module does this by sampling one or more of: `prepare_duration`, `longitudinal_acceleration`, and `lateral_acceleration`. + +##### Prepare Duration Sampling + +In principle, a fixed prepare duration is assumed when generating lane change candidate path. The default prepare duration value is determined from the min and max values set in the lane change parameters, as well as the duration of turn signal activation. +For example, when the lane change module first activates and turn signal is activated then prepare duration will be `max_prepare_duration`, as time passes and a path is still not approved, the prepare duration will decrease gradually down to `min_prepare_duration`. The formula is as follows. + +```C++ +prepare_duration = std::max(max_prepare_duration - turn_signal_duration, min_prepare_duration); +``` + +!!! note + + When the current ego velocity is lower than the `min_lane_change_velocity`, the `min_prepare_duration` is adjusted to ensure sufficient time for reaching `min_lane_change_velocity` assuming `max_longitudinal_acceleration`. + +!!! warning -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 value of the prepare duration impacts lane change cancelling behavior. A shorter prepare duration results in a smaller window in which lane change maneuver can be cancelled. See [Evaluating Ego Vehicle's Position to Prevent Abrupt Maneuvers](#evaluating-ego-vehicles-position-to-prevent-abrupt-maneuvers) for more details. -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`. +When ego vehicles is close to the terminal start, we need to sample multiple prepare duration values to find a valid and safe path. In this case prepare duration values are sampled starting from `max_prepare_duration` down to `0.0` at a fixed time interval of `0.5 s`. + +##### Longitudinal Acceleration Sampling + +In principle, maximum longitudinal acceleration is assumed for generating lane change candidate path. +However in certain situations, we need to sample multiple longitudinal acceleration values to find a valid and safe candidate path. +The lower and upper bounds of the longitudinal acceleration sampled are determined from the values specified in the lane change parameters and common planner parameters, as follows ```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) +minimum_longitudinal_acceleration = 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. @@ -167,9 +346,7 @@ The `longitudinal_acceleration_resolution` is determine by the following 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. +The chart below illustrates the conditions under which longitudinal acceleration values are sampled. ```plantuml @startuml @@ -233,7 +410,7 @@ if (min_acc > max_acc) then (yes) :Return empty list; stop elseif (max_acc - min_acc < epsilon) then (yes) - :Return {0.0}; + :Return {min_acc}; stop else (no) :Calculate resolution; @@ -243,24 +420,27 @@ endif repeat if (sampled_values.back() < -epsilon AND next_value > epsilon) then (yes) :Insert 0.0 into sampled_values; + else (no) endif :Add sampled_acc to sampled_values; repeat while (sampled_acc < max_acc + epsilon) is (TRUE) +:Reverse sampled_values; :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`. +The following figure illustrates when `longitudinal_acceleration_sampling_num = 4`. Assuming that `maximum_acceleration = 1.0` and `minimum_acceleration = 1.0` then `a0 == 1.0`, `a1 == 0.5`, `a2 == 0.0`, `a3 == -0.5` and `a4 == -1.0`. `a0` is the expected lane change trajectory when sampling is not required. -![path_samples](./images/lane_change-candidate_path_samples.png) +![path_samples](./images/lane_change-lon_accel_sampling.png) -Which path will be chosen will depend on validity and collision check. +Which path will be chosen depends on validity and safety checks. -#### Multiple candidate path samples (lateral acceleration) +##### Lateral Acceleration Sampling -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. +In addition to sampling longitudinal acceleration, we also sample lane change paths by varying the lateral acceleration. +Lateral acceleration affects the lane changing duration, a lower value results in a longer trajectory, while a higher value results in a shorter trajectory. This allows the lane change module to explore shorter trajectories through higher 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 @@ -279,16 +459,76 @@ Within this range, we sample the lateral acceleration for the ego vehicle. Simil lateral_acceleration_resolution = (maximum_lateral_acceleration - minimum_lateral_acceleration) / lateral_acceleration_sampling_num ``` -#### Candidate Path's validity check +#### Terminal Lane Change Path + +Depending on the space configuration around the Ego vehicle, it is possible that a valid LC path cannot be generated. If that happens, then Ego will get stuck at `terminal_start` and not be able to proceed. Therefore we introduced the terminal LC path feature; when ego gets near to the terminal point (dist to `terminal_start` is less than the maximum lane change length) a terminal lane changing path will be computed starting from the terminal start point on the current lane and connects to the target lane. The terminal path only needs to be computed once in each execution of LC module. If no valid candidate paths are found in the path generation process, then the terminal path will be used as a fallback candidate path, the safety of the terminal path is not ensured and therefore it can only be force approved. The following images illustrate the expected behavior without and with the terminal path feature respectively: + +![no terminal path](./images/lane_change-no_terminal_path.png) + +![terminal path](./images/lane_change-terminal_path.png) + +Additionally if terminal path feature is enabled and path is computed, stop point placement can be configured to be at the edge of the current lane instead of at the `terminal_start` position, as indicated by the dashed red line in the image above. + +#### Generating Path Using Frenet Planner + +!!! warning -A candidate path is considered valid if it meets the following criteria: + Generating path using Frenet planner applies only when ego is near terminal start -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. The distance from the ego vehicle's current position to the next regulatory element is adequate to perform a single lane change. -5. The lane change can be completed after passing a parked vehicle. -6. The lane change is deemed safe to execute. +If the ego vehicle is far from the terminal, the lane change module defaults to using the [path shifter](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design/). This ensures that the lane change is completed while the target lane remains a neighbor of the current lane. However, this approach may result in high curvature paths near the terminal, potentially causing long vehicles to deviate from the lane. + +To address this, the lane change module provides an option to choose between the path shifter and the [Frenet planner](https://autowarefoundation.github.io/autoware.universe/main/planning/sampling_based_planner/autoware_frenet_planner/). The Frenet planner allows for some flexibility in the lane change endpoint, extending the lane changing end point slightly beyond the current lane's neighbors. + +The following table provides comparisons between the planners + +
+ + + + + + + + + + + + + + + + + +
With Path ShifterWith Frenet Planner
Path shifter result at straight laneletsFrenet planner result at straight lanelets
Path shifter result at branching laneletsFrenet planner result at branching lanelets
Path shifter result at curved laneletsFrenet planner result at curved lanelets
+
+ +!!! note + + The planner can be enabled or disabled using the `frenet.enable` flag. + +!!! note + + Since only a segment of the target lane is used as input to generate the lane change path, the end pose of the lane change segment may not smoothly connect to the target lane centerline. To address this, increase the value of `frenet.th_curvature_smoothing` to improve the smoothness. + +!!! note + + The yaw difference threshold (`frenet.th_yaw_diff`) limits the maximum curvature difference between the end of the prepare segment and the lane change segment. This threshold might prevent the generation of a lane change path when the lane curvature is high. In such cases, you can increase the frenet.th_yaw_diff value. However, note that if the prepare path was initially shifted by other modules, the resultant steering may not be continuous. + +#### Candidate Path Validity + +It is a prerequisite, that both prepare length and lane-changing length are valid, such that: + +1. The prepare segment length is greater than the distance from ego to target lane start. +2. The prepare segment length is smaller than the distance from ego to terminal start. +3. The lane-changing distance is smaller than the remaining distance after prepare segment to terminal end. +4. The lane-changing distance is smaller than the remaining distance after prepare segment to the next regulatory element. + +If so, a candidate path is considered valid if: + +1. The lane changing start point (end of prepare segment) is valid; it is within the target lane neighbor's polygon. +2. The distance from ego to the end of the current lanes is sufficient to perform a single lane change. +3. The distance from ego to the goal along the current lanes is adequate to complete multiple lane changes. +4. The distance from ego to the end of the target lanes is adequate for completing multiple lane changes. The following flow chart illustrates the validity check. @@ -304,306 +544,342 @@ if (Check if start point is valid by check if it is covered by neighbour lanes p 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 +: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 (target lanes is empty) then (yes) +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 - 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 + stop + else (no) + endif +else (no) +endif -if (ego is not stuck but parked vehicle exists in target lane) then (yes) +if (target lanes is empty) then (yes) #LightPink:Reject path; stop else (no) endif - -if (is safe to perform lane change) then (yes) - #Cyan:Return candidate path list; +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) - #LightPink:Reject path; endif +#LightGreen:Valid Candidate Path; stop @enduml ``` -#### Delay Lane Change Check +!!! warning -In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle. -To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected. + A valid path does NOT mean that the path is safe, however it will be available as a candidate path and can be force approved by operator. A path needs to be both valid AND safe to be automatically approved. -1. The distance from object to terminal end is sufficient to perform lane change -2. The distance to object is less than the lane changing length -3. The distance from object to next object is sufficient to perform lane change +### Lane Change Completion Checks -If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked. +To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria. -The following flow chart illustrates the delay lane change check. +For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets. + +If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue. + +The process of determining lane change completion is shown in the following diagram. ```plantuml @startuml skinparam defaultTextAlignment center -skinparam backgroundColor #White +skinparam backgroundColor #WHITE + +title Lane change completion judge start -if (Is target objects, candidate path, OR current lane path empty?) then (yes) - #LightPink:Return false; - stop -else (no) + +:Calculate distance from current ego pose to lane change end pose; + +if (Is ego velocity < 1.0?) then (YES) + :Set finish_judge_buffer to 0.0; +else (NO) + :Set finish_judge_buffer to lane_change_finish_judge_buffer; endif -:Start checking objects from closest to furthest; -repeat - if (Is distance from object to terminal sufficient) then (yes) - else (no) - #LightPink:Return false; +if (ego has passed the end_pose and ego is finish_judge_buffer meters away from end_pose?) then (YES) + if (Current ego pose is in target lanes' polygon?) then (YES) + :Lane change is completed; stop + else (NO) +:Lane change is NOT completed; +stop endif +else (NO) +endif - if (Is distance to object less than lane changing length) then (yes) - else (no) - if (Is only check parked vehicles and vehicle is not parked) then (yes) - else (no) - if(Is last object OR distance to next object is sufficient) then (yes) - #LightGreen: Return true; - stop - else (no) - endif - endif +if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (YES) + :Lane change is NOT completed; + stop +else (NO) + :Calculate distance to the target lanes' centerline; + if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (YES) + :Lane change is completed; + stop + else (NO) + :Lane change is NOT completed; + stop endif - repeat while (Is finished checking all objects) is (FALSE) - -#LightPink: Return false; -stop +endif @enduml ``` -The following figures demonstrate different situations under which will or will not be triggered: +### Safety Checks -1. Delay lane change will be triggered as there is sufficient distance ahead. - ![delay lane change 1](./images/delay_lane_change_1.drawio.svg) -2. Delay lane change will NOT be triggered as there is no sufficient distance ahead - ![delay lane change 2](./images/delay_lane_change_2.drawio.svg) -3. Delay lane change will be triggered by fist NPC as there is sufficient distance ahead. - ![delay lane change 3](./images/delay_lane_change_3.drawio.svg) -4. Delay lane change will be triggered by second NPC as there is sufficient distance ahead - ![delay lane change 4](./images/delay_lane_change_4.drawio.svg) -5. Delay lane change will NOT be triggered as there is no sufficient distance ahead. - ![delay lane change 5](./images/delay_lane_change_5.drawio.svg) +A candidate path needs to be both valid and safe for it to be executed. After generating a candidate path and validating it, the path will be checked against surrounding objects to ensure its safety. However the impacts of an object depends on its categorization, therefore it is necessary to filter the predicted objects before performing the safety checks. -#### Candidate Path's Safety check +#### Object filtering -See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) +In order to perform safety checks on the sampled candidate paths, it is needed to categorize the predicted objects based on their current pose and behavior at the time. These categories help determine how each object impacts the lane change process and guide the safety evaluation. -#### Objects selection and classification +The predicted objects are divided into four main categories: -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`. +- **Target Lane Leading**: Objects that overlap with the target lane and are in front of the ego vehicle. This category is further divided into three subcategories: + - Moving: Objects with a velocity above a certain threshold. + - Stopped: Stationary objects within the target lane. + - Stopped at Bound: Objects outside the target lane but close to its boundaries. +- **Target Lane Trailing**: Objects that overlap with the target lane or any lanes preceding the target lane. Only moving vehicles are included in this category. +- **Current Lane**: Objects in front of the ego vehicle in the ego vehicle's current lane. +- **Others**: Any objects not classified into the above categories. ![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). +Furthermore, for **Target Lane Leading** and **Current Lane** objects, only those positioned within the lane boundary or before the goal position are considered. Objects exceeding the end of the lane or the goal position are classified as **Others**. -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. +Once objects are filtered into their respective categories, they are sorted by distance closest to the ego vehicle to farthest. -
- - - - - -
-
-
Without Lane Expansion
- Without lane expansion -
- 		-
-
With Lane Expansion
- With lane expansion -
-
-
- -##### Object filtering +The following diagram illustrates the filtering process, ```plantuml @startuml skinparam defaultTextAlignment center skinparam backgroundColor #WHITE -title NormalLaneChange::filterObjects Method Execution Flow +title Filter Objects main flowchart start -group "Filter Objects by Class" { -while (has not finished iterating through predicted 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 +:Filter predicted objects by class; +note left: Remove objects whose classes are\nnot specified in the **target_object** parameter. -if (predicted object list is empty?) then (TRUE) - :Return empty result; - stop -else (FALSE) -endif +:Filter oncoming predicted objects; +note left: Compare ego's current pose and object's current pose,\nand filter out any object whose yaw difference exceeds\n**collision_check.th_incoming_object_yaw**; -group "Filter Oncoming Objects" #PowderBlue { -while (has not finished iterating through predicted object list?) is (TRUE) -if (object's yaw with reference to ego's 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 +:Transform predicted objects to extended predicted objects; -if (predicted object list is empty?) then (TRUE) - :Return empty result; +group "Filter target lane objects" { +if (Object's lateral distance\nfrom the centerline\nis more than\nhalf of ego's width?) then (TRUE) + if (Object's current pose\nis before the goal or\nthe end of the lane) then (TRUE) + if (Object overlaps target lane?) then (TRUE) + #LightGreen:To SUBPROCESS "Separate object based on its behavior in target lane"; + if(can separate object in SUBPROCESS) then (TRUE) + stop + endif + else (FALSE) + if (Object is moving\nAND\nis a vehicle class\nAND\npath overlaps target lane?) then (TRUE) + #LightGreen:To SUBPROCESS "Separate object based on its behavior in target lane"; + if(can separate object in SUBPROCESS) then (TRUE) + stop + endif + endif + endif + endif + #LightPink:From SUBPROCESS "Separate object based on its behavior in target lane"; + if (Object is in expanded target lane\nAND\nis stopped\nAND\nin front of ego?) then (TRUE) + :Add object to **filtered_object.leading_objects.stopped_at_bound** list; + stop + endif + else (FALSE) + if (Object overlaps preceding target lanes?) then (TRUE) + :Add object to **filtered_object.trailing_objects** list; + stop + endif +endif +} +if (Object is in front of ego\nAND\nis before terminal\nAND\noverlaps current lane?) then (TRUE) + :Add object to **filtered_object.current_lane** list; stop else (FALSE) + :Add object to **filtered_object.others** list; endif -group "Filter Objects By Lanelets" #LightGreen { +stop -while (has not finished iterating through predicted object list) is (TRUE) - :Calculate lateral distance diff; - if (Object in target lane polygon?) then (TRUE) - if (lateral distance diff > half of ego's width?) then (TRUE) - if (Object's physical position is before terminal point?) then (TRUE) - :Add to target_lane_objects; - else (FALSE) - endif - else (FALSE) - endif +group Target Lane Objects Subprocess #LightYellow +start +if (Object is behind ego?) then (TRUE) + if (Object is moving?) then (TRUE) + :Add object to **filtered_object.trailing_objects** list; + #LightGreen:Can separate object; + stop else (FALSE) + #LightPink:Cant separate proceed with other checks; + stop endif - - if (Object overlaps with backward target lanes?) then (TRUE) - :Add to target_lane_objects; +else (FALSE) + if (Object is moving?) then (TRUE) + :Add object to **filtered_object.leading_objects.moving** list; else (FALSE) - if (Object in current lane polygon?) then (TRUE) - :Add to current_lane_objects; - else (FALSE) - :Add to other_lane_objects; - endif + :Add object to **filtered_object.leading_objects.stopped** list; endif -end while + #LightGreen:Can separate object; + stop +endif -:Return target lanes object, current lanes object and other lanes object; -end group +@enduml +``` -:Generate path from current lanes; +!!! note -if (path empty?) then (TRUE) - :Return empty result; - stop -else (FALSE) -endif + As shown in the flowchart, oncoming objects are also filtered out. The filtering process considers the difference between the current orientation of the ego vehicle and that of the object. However, depending on the map's geometry, a certain threshold may need to be allowed. This threshold can be configured using the parameter collision_check.th_incoming_object_yaw. -group "Filter Target Lanes' objects" #LightCyan { +!!! note -while (has not finished iterating through target lanes' object list) is (TRUE) - if(velocity is within threshold?) then (TRUE) - :Keep current object; - else (FALSE) - if(object is ahead of ego?) then (TRUE) - :keep current object; - else (FALSE) - #LightPink:remove current object; - endif - endif -endwhile -end group + The **Target Lane Leading's Stopped at Boundary** objects are detected using the expanded area of the target lane beyond its original boundaries. The parameters `lane_expansion.left_offset` and `lane_expansion.right_offset` can be configured to adjust the expanded width. -group "Filter Current Lanes' objects" #LightYellow { +
+ + + + + +
+
+
Without Lane Expansion
+ Without lane expansion +
+ 		+
+
With Lane Expansion
+ With lane expansion +
+
+
-while (has not finished iterating through current lanes' object list) is (TRUE) - if(velocity is within threshold?) then (TRUE) - 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 +#### Candidate Path Safety + +A candidate path is considered safe if: + +1. There are no overtaking objects when the ego vehicle exits the turn-direction lane. (see [Overtaking Object Check](#overtaking-object-check)) +2. There is no parked vehicle along the target lane ahead of ego (see [Delay Lane Change Check](#delay-lane-change-check)) +3. The path does NOT cause ego footprint to exceed the target lane opposite boundary +4. The path passes the collision check (See [Collision Check](#collision-check)) + +#### Overtaking Object Check + +When ego is exiting an intersection on a turning lane, there is a possibility that a rear vehicle will attempt to overtake the ego vehicle. Which can be dangerous if ego is also trying to perform a lane change. Therefore lane change module will adopt a more conservative behavior in such situation. + +If the ego vehicle is currently within an intersection on a turning lane, as shown in the figure below, the generated candidate paths will be marked as unsafe. + +![within intersection turn lane](./images/lane_change-intersection_turn_lane_1.png) + +Additionally, if the ego vehicle has just exited the turn lane of an intersection and its distance from the intersection is within the `backward_length_from_intersection`, as shown in the figure below, the generated candidate paths will also be marked as unsafe. -group "Filter Other Lanes' objects" #Lavender { +![after intersection turn lane](./images/lane_change-intersection_turn_lane_2.png) -while (has not finished iterating through other lanes' object list) is (TRUE) - if(velocity is within threshold?) then (TRUE) - if(object is ahead of ego?) then (TRUE) - :keep current object; - else (FALSE) - #LightPink:remove current object; +#### Delay Lane Change Check + +In certain situations, when there are stopped vehicles along the target lane ahead of Ego vehicle, to avoid getting stuck, it is desired to perform the lane change maneuver after the stopped vehicle. +To do so, all static objects ahead of ego along the target lane are checked in order from closest to furthest, if any object satisfies the following conditions, lane change will be delayed and candidate path will be rejected. + +1. The distance from object to terminal end is sufficient to perform lane change +2. The distance to object is less than the lane changing length +3. The distance from object to next object is sufficient to perform lane change + +If the parameter `check_only_parked_vehicle` is set to `true`, the check will only consider objects which are determined as parked. +More details on parked vehicle detection can be found in [documentation for avoidance module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md). + +The following flow chart illustrates the delay lane change check. + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam backgroundColor #White + +start +if (Is target objects, candidate path, OR current lane path empty?) then (yes) + #LightPink:Return false; + stop +else (no) +endif + +:Start checking objects from closest to furthest; +repeat + if (Is distance from object to terminal sufficient) then (yes) + else (no) + #LightPink:Return false; + stop + endif + + if (Is distance to object less than lane changing length) then (yes) + else (no) + if (Is only check parked vehicles and vehicle is not parked) then (yes) + else (no) + if(Is last object OR distance to next object is sufficient) then (yes) + #LightGreen: Return true; + stop + else (no) + endif endif - else (FALSE) - #LightPink:remove current object; - endif -endwhile -end group + endif + repeat while (Is finished checking all objects) is (FALSE) -:Transform the objects into extended predicted object and return them as lane_change_target_objects; +#LightPink: Return false; stop @enduml ``` -##### Collision check in prepare phase +The following figures demonstrate different situations under which delay action will or won't be triggered. In each figure the target lane vehicles are assumed to be stopped. The target lane vehicle responsible for triggering the delay action is marked with blue color. -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. +- Delay lane change will be triggered as there is sufficient distance ahead. -![enable collision check at prepare phase](./images/lane_change-enable_collision_check_at_prepare_phase.png) +![delay lane change 1](./images/delay_lane_change_1.drawio.svg) -#### If the lane is blocked and multiple lane changes +- Delay lane change will NOT be triggered as there is no sufficient distance ahead. -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. +![delay lane change 2](./images/delay_lane_change_2.drawio.svg) -```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 -``` +- Delay lane change will be triggered by fist NPC as there is sufficient distance ahead. -The following figure illustrates when the lane is blocked in multiple lane changes cases. +![delay lane change 3](./images/delay_lane_change_3.drawio.svg) -![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) +- Delay lane change will be triggered by second NPC as there is sufficient distance ahead. + +![delay lane change 4](./images/delay_lane_change_4.drawio.svg) + +- Delay lane change will NOT be triggered as there is no sufficient distance ahead. + +![delay lane change 5](./images/delay_lane_change_5.drawio.svg) + +#### Collision Check + +To ensure the safety of the lane change candidate path an RSS check is performed against the surrounding predicted objects. +More details on the collision check implementation can be found in [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) + +##### Collision Check In Prepare Phase + +The collision check can be applied to the lane changing section only or to the entire candidate path by enabling the flag `enable_collision_check_at_prepare_phase`. Enabling this flag ensures that the ego vehicle secures enough inter-vehicle distance ahead of target lane rear vehicle before attempting a lane change. 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) + +!!! note + + When ego vehicles is stuck, i.e it is stopped, and there is an obstacle in front or is at end of current lane. Then the safety check for lane change is relaxed compared to normal times. -### Stopping behavior +### Stopping Behavior The stopping behavior of the ego vehicle is determined based on various factors, such as the number of lane changes required, the presence of obstacles, and the position of blocking objects in relation to the lane change plan. The objective is to choose a suitable stopping point that allows for a safe and effective lane change while adapting to different traffic scenarios. @@ -703,66 +979,77 @@ If the target lane for the lane change is far away and not next to the current l ![stop_not_at_terminal](./images/lane_change-stop_not_at_terminal.drawio.svg) -### Lane Change When Stuck +#### When target lane is blocked and multiple lane changes -The ego vehicle is considered stuck if it is stopped and meets any of the following conditions: +When ego vehicle needs to perform multiple lane changes to reach the `preferred_lane`, and the `target_lane` is blocked, for example, due to incoming vehicles, the ego vehicle must stop at a sufficient distance from the lane end and wait for the `target_lane` to clear. The minimum stopping distance can be computed from shift length and minimum lane changing velocity. -- There is an obstacle in front of the current lane -- The ego vehicle is at the end of the current lane +```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 +``` -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. +The following figure illustrates when the lane is blocked in multiple lane changes cases. -### Lane change regulations +![multiple-lane-changes](./images/lane_change-when_cannot_change_lanes.png) + +### Aborting Lane Change + +Once the lane change path is approved, there are several situations where we may need to abort the maneuver. The abort process is triggered when any of the following conditions is met + +1. The ego vehicle is near a traffic light, crosswalk, or intersection, and it is possible to complete the lane change after the ego vehicle passes these areas. +2. The target object list is updated, requiring us to [delay lane change](#delay-lane-change-check) +3. The lane change is forcefully canceled via [RTC](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/features/cooperation/). +4. The path has become unsafe. (see [Checking Approved Path Safety](#checking-approved-path-safety)) + +Furthermore, if the path has become unsafe, there are three possible outcomes for the maneuver: + +1. **CANCEL**: The approved path has become unsafe while ego is still in prepare phase. Lane change path is canceled, and the ego vehicle resumes its previous maneuver. +2. **ABORT**: The approved path has become unsafe while ego is in lane changing phase. Lane change module generates a return path to bring the ego vehicle back to its current lane. +3. **CRUISE** or **STOP**: If aborting is not feasible, the ego vehicle continues with the lane change. [Another module](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_obstacle_cruise_planner/) should decide whether the ego vehicle should cruise or stop in this scenario. + +**CANCEL** can be enabled by setting the `cancel.enable_on_prepare_phase` flag to `true`, and **ABORT** can be enabled by setting the `cancel.enable_on_lane_changing_phase` flag to true. + +!!! warning -If you want to regulate lane change on crosswalks, intersections, or traffic lights, the lane change module is disabled near any of them. -To regulate lane change on crosswalks, intersections, or traffic lights, set `regulation.crosswalk`, `regulation.intersection` or `regulation.traffic_light` 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. + Enabling **CANCEL** is a prerequisite for enabling **ABORT**. -### Aborting lane change +!!! warning -The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. + When **CANCEL** is disabled, all maneuvers will default to either **CRUISE** or **STOP**. -The following depicts the flow of the abort lane change check. +The chart shows the high level flow of the lane change abort process. ```plantuml @startuml -skinparam monochrome true skinparam defaultTextAlignment center +skinparam backgroundColor #WHITE -title Abort Lane Change +title High-Level Flow of Lane Change Abort Process 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**) + if (Safe to change lane) then (SAFE) + :Approve safe path; + while(Lane change maneuver is completed?) is (**NO**) + if (Is cancel/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; + if (Ego is preparing to change lane) then (**YES**) + #LightPink:CANCEL; break else (**NO**) - if (Will the overhang to target lane be less than threshold?) then (**YES**) - :Perform abort maneuver; + if (Overhang from current lanes is less than threshold?) then (**YES**) + #Cyan:ABORT; break else (NO) - :Stop or Cruise depending on the situation; endif endif else (**NO**) endif + #Yellow:CRUISE/STOP; endif - endif - :Stop and wait; - endwhile - else (**UNSAFE**) + endwhile (**YES**) + else (UNSAFE) endif else (**NO**) endif @@ -772,107 +1059,138 @@ detach @enduml ``` -During a lane change, a safety check is made in consideration of the deceleration of the ego vehicle, and a safety check is made for `cancel.deceleration_sampling_num` deceleration patterns, and the lane change will be canceled if the abort condition is satisfied for all deceleration patterns. +#### Preventing Oscillating Paths When Unsafe -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. +Lane change paths can oscillate when conditions switch between safe and unsafe. To address this, a hysteresis count check is added before executing an abort maneuver. When the path is unsafe, the `unsafe_hysteresis_count_` increases. If it exceeds the `unsafe_hysteresis_threshold`, an abort condition check is triggered. This logic stabilizes the path approval process and prevents abrupt changes caused by temporary unsafe conditions. ```plantuml @startuml skinparam defaultTextAlignment center skinparam backgroundColor #WHITE -title Abort Lane Change +title Hysteresis count flow for oscillation prevention +while (lane changing completed?) is (FALSE) 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) + if (unsafe_hysteresis_count_ is more than\n unsafe_hysteresis_threshold?) then (FALSE) else (TRUE) #LightPink:Check abort condition; - stop + -[hidden]-> + detach endif endif -:Continue lane changing; +endwhile (TRUE) +stop @enduml ``` -#### Cancel +#### Evaluating Ego Vehicle's Position to Prevent Abrupt Maneuvers + +To avoid abrupt maneuvers during **CANCEL** or **ABORT**, the lane change module ensures the ego vehicle can safely return to the original lane. This is done through geometric checks that verify whether the ego vehicle remains within the lane boundaries. + +The edges of the ego vehicle’s footprint are compared against the boundary of the current lane to determine if they exceed the overhang tolerance, `cancel.overhang_tolerance`. If the distance from any edge of the footprint to the boundary exceeds this threshold, the vehicle is considered to be diverging. + +The footprints checked against the lane boundary include: + +1. Current Footprint: Based on the ego vehicle's current position. +2. Future Footprint: Based on the ego vehicle's estimated position after traveling a distance, calculated as $𝑑_{est}=𝑣_{ego} \cdot \Delta_{𝑡}$, where + + - $v_{ego}$ is ego vehicle's current velocity + - $\Delta_{t}$ is parameterized time constant value, `cancel.delta_time`. + + as depicted in the following diagram + + ![can_return](./images/check_able_to_return.png) + +!!! note + + The ego vehicle is considered capable of safely returning to the current lane only if **BOTH** the current and future footprint checks are `true`. -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. +#### Checking Approved Path Safety -The function can be enabled by setting `enable_on_prepare_phase` to `true`. +The lane change module samples accelerations along the path and recalculates velocity to perform safety checks. The motivation for this feature is explained in the [Limitation](#limitation) section. -The following image illustrates the cancel process. +The computation of sampled accelerations is as follows: + +Let + +$$ +\text{resolution} = \frac{a_{\text{min}} - a_{\text{LC}}}{N} +$$ + +The sequence of sampled accelerations is then given as + +$$ +\text{acc} = a_{\text{LC}} + k \cdot \text{resolution}, \quad k = [0, N] +$$ + +where + +- $a_{\text{min}}$, is the minimum of the parameterized [global acceleration constant](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/common/common.param.yaml) `normal.min_acc` or the [parameterized constant](#essential-lane-change-parameters) `trajectory.min_longitudinal_acceleration`. +- $a_{\text{LC}}$ is the acceleration used to generate the approved path. +- $N$ is the parameterized constant `cancel.deceleration_sampling` + +If none of the sampled accelerations pass the safety check, the lane change path will be canceled, subject to the [hysteresis check](#preventing-oscillating-paths-when-unsafe). + +#### Cancel + +Cancelling lane change is possible as long as the ego vehicle is in the prepare phase and has not started deviating from the current lane center line. +When lane change is canceled, the approved path is reset. After the reset, the ego vehicle will return to following the original reference path (the last approved path before the lane change started), as illustrated in the following image: ![cancel](./images/lane_change-cancel.png) -#### Abort +The following parameters can be configured to tune the behavior of the cancel process: -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. +1. [Safety constraints](#safety-constraints-to-cancel-lane-change-path) for cancel. +2. [Safety constraints](#safety-constraints-specifically-for-stopped-or-parked-vehicles) for parked vehicle. -![abort](./images/lane_change-abort.png) +!!! note -#### Stop/Cruise + To ensure feasible behavior, all safety constraint values must be equal to or less than their corresponding parameters in the [execution](#safety-constraints-during-lane-change-path-is-computed) settings. -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. + - The closer the values, the more conservative the lane change behavior will be. This means it will be easier to cancel the lane change but harder for the ego vehicle to complete a lane change. + - The larger the difference, the more aggressive the lane change behavior will be. This makes it harder to cancel the lane change but easier for the ego vehicle to change lanes. -![stop](./images/lane_change-cant_cancel_no_abort.png) +#### Abort -## Lane change completion checks +During the prepare phase, the ego vehicle follows the previously approved path. However, once the ego vehicle begins the lane change, its heading starts to diverge from this path. Resetting to the previously approved path in this situation would cause abrupt steering, as the controller would attempt to rapidly realign the vehicle with the reference trajectory. -To determine if the ego vehicle has successfully changed lanes, one of two criteria must be met: either the longitudinal or the lateral criteria. +Instead, the lane change module generates an abort path. This return path is specifically designed to guide the ego vehicle back to the current lane, avoiding any sudden maneuvers. The following image provides an illustration of the abort process. -For the longitudinal criteria, the ego vehicle must pass the lane-changing end pose and be within the `finish_judge_buffer` distance from it. The module then checks if the ego vehicle is in the target lane. If true, the module returns success. This check ensures that the planner manager updates the root lanelet correctly based on the ego vehicle's current pose. Without this check, if the ego vehicle is changing lanes while avoiding an obstacle and its current pose is in the original lane, the planner manager might set the root lanelet as the original lane. This would force the ego vehicle to perform the lane change again. With the target lane check, the ego vehicle is confirmed to be in the target lane, and the planner manager can correctly update the root lanelets. +![abort](./images/lane_change-abort.png) -If the longitudinal criteria are not met, the module evaluates the lateral criteria. For the lateral criteria, the ego vehicle must be within `finish_judge_lateral_threshold` distance from the target lane's centerline, and the angle deviation must be within `finish_judge_lateral_angle_deviation` degrees. The angle deviation check ensures there is no sudden steering. If the angle deviation is set too high, the ego vehicle's orientation could deviate significantly from the centerline, causing the trajectory follower to aggressively correct the steering to return to the centerline. Keeping the angle deviation value as small as possible avoids this issue. +The abort path is generated by shifting the approved lane change path using the path shifter. This ensures the continuity in lateral velocity, and prevents abrupt changes in the vehicle’s movement. The abort start shift and abort end shift are computed as follows: -The process of determining lane change completion is shown in the following diagram. +1. Start Shift: $d_{start}^{abort} = v_{ego} \cdot \Delta_{t}$ +2. End Shift: $d_{end}^{abort} = v_{ego} \cdot ( \Delta_{t} + t_{end} )$ -```plantuml -@startuml -skinparam defaultTextAlignment center -skinparam backgroundColor #WHITE +- $v_{ego}$ is ego vehicle's current velocity +- $\Delta_{t}$ is parameterized time constant value, `cancel.delta_time`. +- $t_{end}$ is the parameterized time constant value, `cancel.duration`. -title Lane change completion judge +as depicted in the following diagram -start +![abort_computation](./images/lane_change-abort_computation.png) -:Calculate distance from current ego pose to lane change end pose; +!!! note -if (Is ego velocity < 1.0?) then (YES) - :Set finish_judge_buffer to 0.0; -else (NO) - :Set finish_judge_buffer to lane_change_finish_judge_buffer; -endif + When executing the abort process, comfort is not a primary concern. However, due to safety considerations, limited real-world testing has been conducted to tune or validate this parameter. Currently, the maximum lateral jerk is set to an arbitrary value. To avoid generating a path with excessive lateral jerk, this value can be configured using `cancel.max_lateral_jerk`. -if (ego has passed the end_pose and ego is finish_judge_buffer meters away from end_pose?) then (YES) - if (Current ego pose is in target lanes' polygon?) then (YES) - :Lane change is completed; - stop - else (NO) -:Lane change is NOT completed; -stop - endif -else (NO) -endif +!!! note -if (ego's yaw deviation to centerline exceeds finish_judge_lateral_angle_deviation?) then (YES) - :Lane change is NOT completed; - stop -else (NO) - :Calculate distance to the target lanes' centerline; - if (abs(distance to the target lanes' centerline) is less than finish_judge_lateral_threshold?) then (YES) - :Lane change is completed; - stop - else (NO) - :Lane change is NOT completed; - stop - endif -endif + Lane change module returns `ModuleStatus::FAILURE` once abort is completed. -@enduml -``` +#### Stop/Cruise + +Once canceling or aborting the lane change is no longer an option, the ego vehicle will proceed with the lane change. This can happen in the following situations: + +- The ego vehicle is performing a lane change near a terminal or dead-end, making it impossible to return to the original lane. In such cases, completing the lane change is necessary. +- If safety parameters are tuned too aggressively, it becomes harder to cancel or abort the lane change. This reduces tolerance for unexpected behaviors from surrounding vehicles, such as a trailing vehicle in the target lane suddenly accelerating or a leading vehicle suddenly decelerating. Aggressive settings leave less room for error during the maneuver. + +![stop](./images/lane_change-cant_cancel_no_abort.png) ## Parameters @@ -882,10 +1200,12 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | Name | Unit | Type | Description | Default value | | :------------------------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------------------- | ------------------ | +| `time_limit` | [ms] | double | Time limit for lane change candidate path generation | 50.0 | | `backward_lane_length` | [m] | double | The backward length to check incoming objects in lane change target lane. | 200.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 | | `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | +| `enable_stopped_vehicle_buffer` | [-] | bool | If true, will keep enough distance from current lane front stopped object to perform lane change when possible | true | | `trajectory.max_prepare_duration` | [s] | double | The maximum preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `trajectory.min_prepare_duration` | [s] | double | The minimum preparation time for the ego vehicle to be ready to perform lane change. | 2.0 | | `trajectory.lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | @@ -895,6 +1215,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `trajectory.max_longitudinal_acc` | [m/s2] | double | maximum longitudinal acceleration for lane change | 1.0 | | `trajectory.min_longitudinal_acc` | [m/s2] | double | maximum longitudinal deceleration for lane change | -1.0 | | `trajectory.lane_changing_decel_factor` | [-] | double | longitudinal deceleration factor during lane changing phase | 0.5 | +| `trajectory.th_prepare_curvature` | [-] | double | If the maximum curvature of the prepare segment exceeds the threshold, the prepare segment is invalid. | 0.03 | | `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 | | `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/s2] | double | Min lateral acceleration values corresponding to velocity (look up table) | [0.4, 0.4, 0.4] | @@ -934,6 +1255,28 @@ The following parameters are used to judge lane change completion. | `delay_lane_change.min_road_shoulder_width` | [m] | double | Width considered as road shoulder if lane doesn't have road shoulder when checking for parked vehicle | 0.5 | | `delay_lane_change.th_parked_vehicle_shift_ratio` | [-] | double | Stopped vehicles beyond this distance ratio from center line will be considered as parked | 0.6 | +### Terminal Lane Change Path + +The following parameters are used to configure terminal lane change path feature. + +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | ---- | ---- | ------------------------------------------------------------------------- | ------------- | +| `terminal_path.enable` | [-] | bool | Flag to enable/disable terminal path feature | true | +| `terminal_path.disable_near_goal` | [-] | bool | Flag to disable terminal path feature if ego is near goal | true | +| `terminal_path.stop_at_boundary` | [-] | bool | If true, ego will stop at current lane boundary instead of middle of lane | false | + +### Generating Lane Changing Path using Frenet Planner + +!!! warning + + Only applicable when ego is near terminal start + +| Name | Unit | Type | Description | Default value | +| :------------------------------ | ----- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| `frenet.enable` | [-] | bool | Flag to enable/disable frenet planner when ego is near terminal start. | true | +| `frenet.th_yaw_diff` | [deg] | double | If the yaw diff between of the prepare segment's end and lane changing segment's start exceed the threshold , the lane changing segment is invalid. | 10.0 | +| `frenet.th_curvature_smoothing` | [-] | double | Filters and appends target path points with curvature below the threshold to candidate path. | 0.1 | + ### Collision checks #### Target Objects @@ -967,7 +1310,8 @@ The following parameters are used to judge lane change completion. | `collision_check.check_other_lanes` | [-] | boolean | If true, the lane change module includes objects in other lanes when performing collision assessment. | false | | `collision_check.use_all_predicted_paths` | [-] | boolean | If false, use only the predicted path that has the maximum confidence. | true | | `collision_check.prediction_time_resolution` | [s] | double | Time resolution for object's path interpolation and collision check. | 0.5 | -| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between ego and object when executing rss-based collision checking | 3.1416 | +| `collision_check.yaw_diff_threshold` | [rad] | double | Maximum yaw difference between predicted ego pose and predicted object pose when executing rss-based collision checking | 3.1416 | +| `collision_check.th_incoming_object_yaw` | [rad] | double | Maximum yaw difference between current ego pose and current object pose. Objects with a yaw difference exceeding this value are excluded from the safety check. | 2.3562 | #### safety constraints during lane change path is computed @@ -980,6 +1324,7 @@ The following parameters are used to judge lane change completion. | `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.execution.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_check.execution.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | #### safety constraints specifically for stopped or parked vehicles @@ -992,6 +1337,7 @@ The following parameters are used to judge lane change completion. | `safety_check.parked.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.parked.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.parked.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_check.parked.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints to cancel lane change path @@ -1004,6 +1350,7 @@ The following parameters are used to judge lane change completion. | `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_check.cancel.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | ##### safety constraints used during lane change path is computed when ego is stuck @@ -1016,6 +1363,7 @@ The following parameters are used to judge lane change completion. | `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 | +| `safety_check.stuck.extended_polygon_policy` | [-] | string | Policy used to determine the polygon shape for the safety check. Available options are: `rectangle` or `along-path`. | `rectangle` | (\*1) the value must be negative. @@ -1074,3 +1422,10 @@ Available information 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. + +## Limitation + +1. When a lane change is canceled, the lane change module returns `ModuleStatus::FAILURE`. As the module is removed from the approved module stack (see [Failure modules](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design/#failure-modules)), a new instance of the lane change module is initiated. Due to this, any information stored prior to the reset is lost. For example, the `lane_change_prepare_duration` in the `TransientData` is reset to its maximum value. +2. The lane change module has no knowledge of any velocity modifications introduced to the path after it is approved. This is because other modules may add deceleration points after subscribing to the behavior path planner output, and the final velocity is managed by the [velocity smoother](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_velocity_smoother/). Since this limitation affects **CANCEL**, the lane change module mitigates it by [sampling accelerations along the approved lane change path](#checking-approved-path-safety). These sampled accelerations are used during safety checks to estimate the velocity that might occur if the ego vehicle decelerates. +3. Ideally, the abort path should account for whether its execution would affect trailing vehicles in the current lane. However, the lane change module does not evaluate such interactions or assess whether the abort path is safe. As a result, **the abort path is not guaranteed to be safe**. To minimize the risk of unsafe situations, the abort maneuver is only permitted if the ego vehicle has not yet diverged from the current lane. +4. Due to limited resources, the abort path logic is not fully optimized. The generated path may overshoot, causing the return trajectory to slightly shift toward the opposite lane. This can be dangerous, especially if the opposite lane has traffic moving in the opposite direction. Furthermore, the logic does not account for different vehicle types, which can lead to varying effects. For instance, the behavior might differ significantly between a bus and a small passenger car. diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index bf892b3058a16..71fac1246598f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -1,14 +1,22 @@ /**: ros__parameters: lane_change: + time_limit: 50.0 # [ms] backward_lane_length: 200.0 backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] backward_length_from_intersection: 5.0 # [m] + enable_stopped_vehicle_buffer: true # turn signal min_length_for_turn_signal_activation: 10.0 # [m] + # terminal path + terminal_path: + enable: true + disable_near_goal: true + stop_at_boundary: false + # trajectory generation trajectory: max_prepare_duration: 4.0 @@ -22,6 +30,7 @@ lon_acc_sampling_num: 5 lat_acc_sampling_num: 3 lane_changing_decel_factor: 0.5 + th_prepare_curvature: 0.03 # [/] # delay lane change delay_lane_change: @@ -30,6 +39,12 @@ min_road_shoulder_width: 0.5 # [m] th_parked_vehicle_shift_ratio: 0.6 + # trajectory generation near terminal using frenet planner + frenet: + enable: true + th_yaw_diff: 10.0 # [deg] + th_curvature_smoothing: 0.1 # [/] + # safety check safety_check: allow_loose_check_for_cancel: true @@ -43,6 +58,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" parked: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -51,6 +67,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" cancel: expected_front_deceleration: -1.0 expected_rear_deceleration: -2.0 @@ -59,6 +76,7 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 2.5 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" stuck: expected_front_deceleration: -1.0 expected_rear_deceleration: -1.0 @@ -67,6 +85,7 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.0 + extended_polygon_policy: "rectangle" # lane expansion for object filtering lane_expansion: @@ -108,7 +127,8 @@ intersection: true turns: true prediction_time_resolution: 0.5 - yaw_diff_threshold: 3.1416 + th_incoming_object_yaw: 2.3562 # [rad] + yaw_diff_threshold: 3.1416 # [rad] check_current_lanes: false check_other_lanes: false use_all_predicted_paths: false diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png new file mode 100644 index 0000000000000..1f01b6df5464a Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/check_able_to_return.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png index 0913912b6a332..41e2bddbdafba 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png new file mode 100644 index 0000000000000..47d4ba81b3354 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-abort_computation.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png index 10b7beb6ad7d0..91e44e609bf25 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cancel.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png deleted file mode 100644 index 8578549bb1f5b..0000000000000 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-candidate_path_samples.png and /dev/null differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png index c3780540f8d00..f142b42c8a644 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-cant_cancel_no_abort.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-intersection_turn_lane_1.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-intersection_turn_lane_1.png new file mode 100644 index 0000000000000..d22ddc815c4e7 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-intersection_turn_lane_1.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-intersection_turn_lane_2.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-intersection_turn_lane_2.png new file mode 100644 index 0000000000000..dc1b6bef33fe5 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-intersection_turn_lane_2.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lon_accel_sampling.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lon_accel_sampling.png new file mode 100644 index 0000000000000..3f4c08b6a244c Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-lon_accel_sampling.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png new file mode 100644 index 0000000000000..2c7b76e174997 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-no_terminal_path.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png new file mode 100644 index 0000000000000..83e9d377e8f34 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/lane_change-terminal_path.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png new file mode 100644 index 0000000000000..51f3a2fc46500 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png new file mode 100644 index 0000000000000..4af931348d6ca Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_branched_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png new file mode 100644 index 0000000000000..f834a64867e6c Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png new file mode 100644 index 0000000000000..22ba8d2d7c47d Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_curved_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png new file mode 100644 index 0000000000000..9a36642b2d545 Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_frenet.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png new file mode 100644 index 0000000000000..7a4e3a453304b Binary files /dev/null and b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/images/terminal_straight_path_shifter.png differ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp similarity index 89% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index 741b5afb50e08..a1ed5517edf25 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -11,13 +11,14 @@ // WITHOUT 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__BASE_CLASS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__BASE_CLASS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__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/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/debug.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.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/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -38,6 +39,7 @@ class TestNormalLaneChange; namespace autoware::behavior_path_planner { +using autoware::behavior_path_planner::PoseWithDetailOpt; using autoware::route_handler::Direction; using autoware::universe_utils::StopWatch; using geometry_msgs::msg::Point; @@ -130,7 +132,7 @@ class LaneChangeBase virtual void insert_stop_point( [[maybe_unused]] const lanelet::ConstLanelets & lanelets, - [[maybe_unused]] PathWithLaneId & path) + [[maybe_unused]] PathWithLaneId & path, [[maybe_unused]] const bool is_waiting_approval = false) { } @@ -224,7 +226,7 @@ class LaneChangeBase return direction_; } - std::optional getStopPose() const { return lane_change_stop_pose_; } + PoseWithDetailOpt getStopPose() const { return lane_change_stop_pose_; } void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } @@ -233,11 +235,6 @@ class LaneChangeBase virtual bool is_near_regulatory_element() const = 0; protected: - virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; - - virtual bool get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const = 0; - virtual bool isValidPath(const PathWithLaneId & path) const = 0; virtual bool isAbleToStopSafely() const = 0; @@ -276,20 +273,19 @@ class LaneChangeBase } } - LaneChangeStatus status_{}; + LaneChangeStatus status_; PathShifter path_shifter_{}; LaneChangeStates current_lane_change_state_{}; - std::shared_ptr lane_change_parameters_{}; - std::shared_ptr abort_path_{}; - std::shared_ptr planner_data_{}; - lane_change::CommonDataPtr common_data_ptr_{}; + std::shared_ptr lane_change_parameters_; + std::shared_ptr abort_path_; + std::shared_ptr planner_data_; + lane_change::CommonDataPtr common_data_ptr_; FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; - std::optional lane_change_stop_pose_{std::nullopt}; - - PathWithLaneId prev_approved_path_{}; + PoseWithDetailOpt lane_change_stop_pose_{std::nullopt}; + mutable std::optional terminal_lane_change_path_{std::nullopt}; int unsafe_hysteresis_count_{0}; bool is_abort_path_approved_{false}; @@ -311,4 +307,4 @@ class LaneChangeBase friend class ::TestNormalLaneChange; }; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__BASE_CLASS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp index 07920f83fd980..82b042d1135d2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/interface.hpp @@ -15,10 +15,10 @@ #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/base_class.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_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/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" @@ -35,6 +35,7 @@ #include #include #include +#include namespace autoware::behavior_path_planner { @@ -52,6 +53,7 @@ class LaneChangeInterface : public SceneModuleInterface const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && module_type); LaneChangeInterface(const LaneChangeInterface &) = delete; @@ -120,6 +122,8 @@ class LaneChangeInterface : public SceneModuleInterface } } + std::pair check_transit_failure(); + void updateDebugMarker() const; void updateSteeringFactorPtr(const BehaviorModuleOutput & output); @@ -129,13 +133,13 @@ class LaneChangeInterface : public SceneModuleInterface 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}; + + lane_change::InterfaceDebug interface_debug_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp index 2a1862de6a27e..124579735e5d8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/manager.hpp @@ -15,7 +15,7 @@ #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_lane_change_module/structs/data.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "autoware/route_handler/route_handler.hpp" diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 7202a1c6a9108..fec43d021bf8b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -14,10 +14,11 @@ #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 "autoware/behavior_path_lane_change_module/base_class.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include +#include #include #include @@ -70,9 +71,12 @@ class NormalLaneChange : public LaneChangeBase void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; - void insert_stop_point(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; + void insert_stop_point( + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, + const bool is_waiting_approval = false) override; - void insert_stop_point_on_current_lanes(PathWithLaneId & path); + void insert_stop_point_on_current_lanes( + PathWithLaneId & path, const bool is_waiting_approval = false); PathWithLaneId getReferencePath() const override; @@ -116,8 +120,6 @@ class NormalLaneChange : public LaneChangeBase protected: lanelet::ConstLanelets get_lane_change_lanes(const lanelet::ConstLanelets & current_lanes) const; - int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; - TurnSignalInfo get_terminal_turn_signal_info() const final; lane_change::TargetObjects get_target_objects( @@ -128,47 +130,41 @@ class NormalLaneChange : public LaneChangeBase void filterOncomingObjects(PredictedObjects & objects) const; - bool get_prepare_segment( - PathWithLaneId & prepare_segment, 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; - std::vector get_prepare_metrics() const; std::vector get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metrics, - const double shift_length, const double dist_to_reg_element) const; + const double shift_length, const double dist_to_reg_element, + lane_change::MetricsDebug & debug_metrics) const; bool get_lane_change_paths(LaneChangePaths & candidate_paths) const; - LaneChangePath get_candidate_path( - const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, - const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double shift_length) const; + bool get_path_using_frenet( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; + + bool get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const; bool check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const; - std::optional calcTerminalLaneChangePath( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + std::optional compute_terminal_lane_change_path() const; bool isValidPath(const PathWithLaneId & path) const override; PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, + const std::vector> & ego_predicted_paths, const lane_change::TargetObjects & collision_check_objects, const utils::path_safety_checker::RSSparams & rss_params, - const size_t deceleration_sampling_num, CollisionCheckDebugMap & debug_data) const; - - bool has_collision_with_decel_patterns( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, - const size_t deceleration_sampling_num, const RSSparams & rss_param, - const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const; + CollisionCheckDebugMap & debug_data) const; - bool is_collided( + bool is_colliding( const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, const bool check_prepare_phase, @@ -178,27 +174,10 @@ class NormalLaneChange : public LaneChangeBase bool is_ego_stuck() const; - /** - * @brief Checks if the given pose is a valid starting point for a lane change. - * - * This function determines whether the given pose (position) of the vehicle is within - * the area of either the target neighbor lane or the target lane itself. It uses geometric - * checks to see if the start point of the lane change is covered by the polygons representing - * these lanes. - * - * @param common_data_ptr Shared pointer to a CommonData structure, which should include: - * - Non-null `lanes_polygon_ptr` that contains the polygon data for lanes. - * @param pose The current pose of the vehicle - * - * @return `true` if the pose is within either the target neighbor lane or the target lane; - * `false` otherwise. - */ - bool is_valid_start_point( - const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const; - bool check_prepare_phase() const; - void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); + void set_stop_pose( + const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason = ""); void updateStopTime(); @@ -213,7 +192,6 @@ class NormalLaneChange : public LaneChangeBase std::vector path_after_intersection_; double stop_time_{0.0}; - static constexpr double floating_err_th{1e-3}; }; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp similarity index 88% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.hpp index c121ab8512cce..c844b6b218d10 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/data.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 AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/parameters.hpp" +#include "autoware/behavior_path_lane_change_module/structs/parameters.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" @@ -48,6 +48,7 @@ enum class States { Cancel, Abort, Stop, + Warning, }; struct PhaseInfo @@ -72,6 +73,7 @@ struct PhaseMetrics double actual_lon_accel{0.0}; double lat_accel{0.0}; + PhaseMetrics() = default; PhaseMetrics( const double _duration, const double _length, const double _velocity, const double _sampled_lon_accel, const double _actual_lon_accel, const double _lat_accel) @@ -103,10 +105,10 @@ struct Info PhaseInfo duration{0.0, 0.0}; PhaseInfo length{0.0, 0.0}; - Pose lane_changing_start{}; - Pose lane_changing_end{}; + Pose lane_changing_start; + Pose lane_changing_end; - ShiftLine shift_line{}; + ShiftLine shift_line; double lateral_acceleration{0.0}; double terminal_lane_changing_velocity{0.0}; @@ -127,6 +129,22 @@ struct Info terminal_lane_changing_velocity = _lc_metrics.velocity; shift_line = _shift_line; } + + void set_prepare(const PhaseMetrics & _prep_metrics) + { + longitudinal_acceleration.prepare = _prep_metrics.actual_lon_accel; + velocity.prepare = _prep_metrics.velocity; + duration.prepare = _prep_metrics.duration; + length.prepare = _prep_metrics.length; + } + + void set_lane_changing(const PhaseMetrics & _lc_metrics) + { + longitudinal_acceleration.lane_changing = _lc_metrics.actual_lon_accel; + velocity.lane_changing = _lc_metrics.velocity; + duration.lane_changing = _lc_metrics.duration; + length.lane_changing = _lc_metrics.length; + } }; struct TargetLaneLeadingObjects @@ -159,6 +177,8 @@ struct TargetObjects : leading(std::move(leading)), trailing(std::move(trailing)) { } + + [[nodiscard]] bool empty() const { return leading.empty() && trailing.empty(); } }; enum class ModuleType { @@ -216,6 +236,8 @@ struct TransientData double target_lane_length{std::numeric_limits::min()}; + double dist_to_target_end{std::numeric_limits::max()}; + lanelet::ArcCoordinates current_lanes_ego_arc; // arc coordinates of ego pose along current lanes lanelet::ArcCoordinates target_lanes_ego_arc; // arc coordinates of ego pose along target lanes @@ -247,6 +269,7 @@ struct CommonData LanesPolygonPtr lanes_polygon_ptr; TransientData transient_data; PathWithLaneId current_lanes_path; + PathWithLaneId target_lanes_path; ModuleType lc_type; Direction direction; @@ -294,4 +317,4 @@ using FilteredLanesObjects = lane_change::FilteredLanesObjects; using LateralAccelerationMap = lane_change::LateralAccelerationMap; } // namespace autoware::behavior_path_planner -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DATA_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp similarity index 67% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp index a28b8523a75c7..eb07d2f0d3704 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/debug.hpp @@ -11,11 +11,11 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_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_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include @@ -25,10 +25,34 @@ #include #include +#include namespace autoware::behavior_path_planner::lane_change { using utils::path_safety_checker::CollisionCheckDebugMap; + +struct MetricsDebug +{ + LaneChangePhaseMetrics prep_metric; + std::vector> lc_metrics; + double max_prepare_length; + double max_lane_changing_length; +}; + +struct FrenetStateDebug +{ + LaneChangePhaseMetrics prep_metric; + frenet_planner::SamplingParameter sampling_parameter; + double max_lane_changing_length; + + FrenetStateDebug( + LaneChangePhaseMetrics prep_metric, frenet_planner::SamplingParameter sampling_param, + const double max_len) + : prep_metric(prep_metric), sampling_parameter(sampling_param), max_lane_changing_length(max_len) + { + } +}; + struct Debug { std::string module_type; @@ -41,13 +65,14 @@ struct Debug lanelet::ConstLanelets current_lanes; lanelet::ConstLanelets target_lanes; lanelet::ConstLanelets target_backward_lanes; + std::vector lane_change_metrics; + std::vector frenet_states; double collision_check_object_debug_lifetime{0.0}; double distance_to_end_of_current_lane{std::numeric_limits::max()}; double distance_to_lane_change_finished{std::numeric_limits::max()}; double distance_to_abort_finished{std::numeric_limits::max()}; - bool is_able_to_return_to_current_lane{false}; + bool is_able_to_return_to_current_lane{true}; bool is_stuck{false}; - bool is_abort{false}; void reset() { @@ -64,16 +89,22 @@ struct Debug current_lanes.clear(); target_lanes.clear(); target_backward_lanes.clear(); + lane_change_metrics.clear(); collision_check_object_debug_lifetime = 0.0; distance_to_end_of_current_lane = std::numeric_limits::max(); distance_to_lane_change_finished = std::numeric_limits::max(); distance_to_abort_finished = std::numeric_limits::max(); - is_able_to_return_to_current_lane = false; + is_able_to_return_to_current_lane = true; is_stuck = false; - is_abort = false; } }; + +struct InterfaceDebug +{ + std::string_view failing_reason; + LaneChangeStates lc_state; +}; } // namespace autoware::behavior_path_planner::lane_change -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp similarity index 88% rename from planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp rename to planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp index a76742307e061..358f60f3193d5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ -#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ +#ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" @@ -94,6 +94,7 @@ struct CollisionCheckParameters bool check_current_lane{true}; bool check_other_lanes{true}; bool use_all_predicted_paths{false}; + double th_incoming_object_yaw{2.3562}; double th_yaw_diff{3.1416}; double prediction_time_resolution{0.5}; }; @@ -113,6 +114,13 @@ struct SafetyParameters CollisionCheckParameters collision_check{}; }; +struct FrenetPlannerParameters +{ + bool enable{true}; + double th_yaw_diff_deg{10.0}; + double th_curvature_smoothing{0.1}; +}; + struct TrajectoryParameters { double max_prepare_duration{4.0}; @@ -124,6 +132,7 @@ struct TrajectoryParameters double th_lane_changing_length_diff{0.5}; double min_lane_changing_velocity{5.6}; double lane_changing_decel_factor{0.5}; + double th_prepare_curvature{0.03}; int lon_acc_sampling_num{10}; int lat_acc_sampling_num{10}; LateralAccelerationMap lat_acc_map{}; @@ -137,18 +146,29 @@ struct DelayParameters double th_parked_vehicle_shift_ratio{0.6}; }; +struct TerminalPathParameters +{ + bool enable{false}; + bool disable_near_goal{false}; + bool stop_at_boundary{false}; +}; + struct Parameters { TrajectoryParameters trajectory{}; SafetyParameters safety{}; CancelParameters cancel{}; DelayParameters delay{}; + TerminalPathParameters terminal_path{}; + FrenetPlannerParameters frenet{}; // lane change parameters + double time_limit{50.0}; double backward_lane_length{200.0}; double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; double backward_length_from_intersection{5.0}; + bool enable_stopped_vehicle_buffer{false}; // parked vehicle double object_check_min_road_shoulder_width{0.5}; @@ -177,4 +197,4 @@ struct Parameters } // namespace autoware::behavior_path_planner::lane_change -#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PARAMETERS_HPP_ +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp new file mode 100644 index 0000000000000..0fa2c6cc8dfbc --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/structs/path.hpp @@ -0,0 +1,85 @@ +// 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__STRUCTS__PATH_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PATH_HPP_ + +#include "autoware/behavior_path_lane_change_module/structs/data.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 + +namespace autoware::behavior_path_planner::lane_change +{ +enum class PathType { ConstantJerk = 0, FrenetPlanner }; + +using autoware::behavior_path_planner::TurnSignalInfo; +using tier4_planning_msgs::msg::PathWithLaneId; +struct TrajectoryGroup +{ + PathWithLaneId prepare; + PathWithLaneId target_lane_ref_path; + std::vector target_lane_ref_path_dist; + LaneChangePhaseMetrics prepare_metric; + frenet_planner::Trajectory lane_changing; + frenet_planner::FrenetState initial_state; + double max_lane_changing_length{0.0}; + + TrajectoryGroup() = default; + TrajectoryGroup( + PathWithLaneId prepare, PathWithLaneId target_lane_ref_path, + std::vector target_lane_ref_path_dist, LaneChangePhaseMetrics prepare_metric, + frenet_planner::Trajectory lane_changing, frenet_planner::FrenetState initial_state, + const double max_lane_changing_length) + : prepare(std::move(prepare)), + target_lane_ref_path(std::move(target_lane_ref_path)), + target_lane_ref_path_dist(std::move(target_lane_ref_path_dist)), + prepare_metric(prepare_metric), + lane_changing(std::move(lane_changing)), + initial_state(initial_state), + max_lane_changing_length(max_lane_changing_length) + { + } +}; + +struct Path +{ + Info info; + PathWithLaneId path; + ShiftedPath shifted_path; + TrajectoryGroup frenet_path; + PathType type = PathType::ConstantJerk; +}; + +struct Status +{ + Path lane_change_path; + bool is_safe{false}; + bool is_valid_path{false}; +}; +} // namespace autoware::behavior_path_planner::lane_change + +namespace autoware::behavior_path_planner +{ +using LaneChangePath = lane_change::Path; +using LaneChangePaths = std::vector; +using LaneChangeStatus = lane_change::Status; +} // namespace autoware::behavior_path_planner +#endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__STRUCTS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 29047c90590b3..29a9f258545a2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include @@ -67,6 +67,9 @@ double calc_dist_to_last_fit_width( const lanelet::ConstLanelets & lanelets, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); +double calc_dist_to_last_fit_width( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin = 0.1); + /** * @brief Calculates the maximum preparation longitudinal distance for lane change. * diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp index c95b388a2e4a4..a9ea35cb9b83e 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/markers.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ #define AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_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/structs/debug.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -32,19 +32,23 @@ namespace marker_utils::lane_change_markers using autoware::behavior_path_planner::FilteredLanesObjects; using autoware::behavior_path_planner::LaneChangePath; using autoware::behavior_path_planner::lane_change::Debug; +using autoware::behavior_path_planner::lane_change::InterfaceDebug; using autoware::behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( - const std::vector & lanes, std::string && ns); + const std::vector & lane_change_paths, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); MarkerArray showFilteredObjects( const FilteredLanesObjects & filtered_objects, const std::string & ns); MarkerArray createExecutionArea(const geometry_msgs::msg::Polygon & execution_area); -MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); +MarkerArray showExecutionInfo( + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose); MarkerArray createDebugMarkerArray( - const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose); + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose); } // namespace marker_utils::lane_change_markers #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 5623f03a22eb3..42586e7b1df92 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 TIER IV, Inc. +// 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. @@ -11,43 +11,152 @@ // WITHOUT 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 "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.hpp" -#include +#include #include -namespace autoware::behavior_path_planner::lane_change -{ -using autoware::behavior_path_planner::TurnSignalInfo; -using tier4_planning_msgs::msg::PathWithLaneId; -struct Path +namespace autoware::behavior_path_planner::utils::lane_change { - PathWithLaneId path{}; - ShiftedPath shifted_path{}; - Info info{}; -}; +using behavior_path_planner::LaneChangePath; +using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::TrajectoryGroup; -struct Status -{ - Path lane_change_path{}; - bool is_safe{false}; - bool is_valid_path{false}; - double start_distance{0.0}; -}; +/** + * @brief Generates a prepare segment for a lane change maneuver. + * + * This function generates the "prepare segment" of the path by trimming it to the specified length, + * adjusting longitudinal velocity for acceleration or deceleration, and ensuring the starting point + * meets necessary constraints for a lane change. + * + * @param common_data_ptr Shared pointer to CommonData containing current and target lane + * information, vehicle parameters, and ego state. + * @param prev_module_path The input path from the previous module, which will be used + * as the base for the prepare segment. + * @param prep_metric Preparation metrics containing desired length and velocity for the segment. + * @param prepare_segment Output parameter where the resulting prepare segment path will be stored. + * + * @throws std::logic_error If the lane change start point is behind the target lanelet or + * if lane information is invalid. + * + * @return true if the prepare segment is successfully generated, false otherwise. + */ +bool get_prepare_segment( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const double prep_length, PathWithLaneId & prepare_segment); -} // namespace autoware::behavior_path_planner::lane_change +/** + * @brief Generates the candidate path for a lane change maneuver. + * + * This function calculates the candidate lane change path based on the provided preparation + * and lane change metrics. It resamples the target lane reference path, determines the start + * and end poses for the lane change, and constructs the shift line required for the maneuver. + * The function ensures that the resulting path satisfies length and distance constraints. + * + * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient data. + * @param prep_metric Metrics for the preparation phase, including path length and velocity. + * @param lc_metric Metrics for the lane change phase, including path length and velocity. + * @param prep_segment The path segment prepared before initiating the lane change. + * @param sorted_lane_ids A vector of sorted lane IDs used for updating lane information in the + * path. + * @param lc_start_pose The pose where the lane change begins. + * @param shift_length The lateral distance to shift during the lane change maneuver. + * + * @throws std::logic_error If the target lane reference path is empty, candidate path generation + * fails, or the resulting path length exceeds terminal constraints. + * + * @return LaneChangePath The constructed candidate lane change path. + */ +LaneChangePath get_candidate_path( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric, + const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment, + const std::vector> & sorted_lane_ids, const double shift_length); + +/** + * @brief Constructs a candidate path for a lane change maneuver. + * + * This function generates a candidate lane change path by shifting the target lane's reference path + * and combining it with the prepare segment. It verifies the path's validity by checking the yaw + * difference, adjusting longitudinal velocity, and updating lane IDs based on the provided lane + * sorting information. + * + * @param lane_change_info Information about the lane change, including shift line and target + * velocity. + * @param prepare_segment The path segment leading up to the lane change. + * @param target_lane_reference_path The reference path of the target lane. + * @param sorted_lane_ids A vector of sorted lane IDs used to update the candidate path's lane IDs. + * + * @return std::optional The constructed candidate path if valid, or std::nullopt + * if the path fails any constraints. + */ +LaneChangePath construct_candidate_path( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids); + +/** + * @brief Generates candidate trajectories in the Frenet frame for a lane change maneuver. + * + * This function computes a set of candidate trajectories for the ego vehicle in the Frenet frame, + * based on the provided metrics, target lane reference path, and preparation segments. It ensures + * that the generated trajectories adhere to specified constraints, such as lane boundaries and + * velocity limits, while optimizing for smoothness and curvature. + * + * @param common_data_ptr Shared pointer to CommonData containing route, lane, and transient + * information. + * @param prev_module_path The previous module's path used as a base for preparation segments. + * @param prep_metric Metrics for the preparation phase, including path length and velocity. + * + * @return std::vector A vector of trajectory groups representing + * valid lane change candidates, each containing the prepare segment, target reference path, and + * Frenet trajectory. + */ +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & prep_metrics); + +/** + * @brief Constructs a lane change path candidate based on a Frenet trajectory group. + * + * This function generates a candidate lane change path by converting a Frenet trajectory group + * into a shifted path and combining it with a prepare segment. It validates the path's feasibility + * by ensuring yaw differences are within allowable thresholds and calculates lane change + * information, such as acceleration, velocity, and duration. + * + * @param trajectory_group A Frenet trajectory group containing the prepared path and Frenet + * trajectory data. + * @param common_data_ptr Shared pointer to CommonData providing parameters and constraints for lane + * changes. + * @param sorted_lane_ids A vector of sorted lane IDs used to update the lane IDs in the path. + * + * @return std::optional The constructed candidate lane change path if valid, or + * std::nullopt if the path is not feasible. + * + * @throws std::logic_error If the yaw difference exceeds the threshold, or other critical errors + * occur. + */ +std::optional get_candidate_path( + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids); + +/** + * @brief Appends the target lane reference path to the candidate lane change path. + * + * This function extends the lane change candidate path by appending points from the + * target lane reference path beyond the lane change end position. The appending process + * is constrained by a curvature threshold to ensure smooth transitions and avoid sharp turns. + * + * @param frenet_candidate The candidate lane change path to which the target reference path is + * appended. This includes the lane change path and associated Frenet trajectory data. + * @param th_curvature_smoothing A threshold for the allowable curvature during the extension + * process. Points with curvature exceeding this threshold are excluded. + */ +void append_target_ref_to_candidate(LaneChangePath & frenet_candidate, const double th_curvature); +} // namespace autoware::behavior_path_planner::utils::lane_change -namespace autoware::behavior_path_planner -{ -using LaneChangePath = lane_change::Path; -using LaneChangePaths = std::vector; -using LaneChangeStatus = lane_change::Status; -} // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index 5dcd132f16377..422c392cac462 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -15,8 +15,8 @@ #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_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/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" @@ -24,6 +24,8 @@ #include #include +#include +#include #include #include @@ -52,26 +54,45 @@ using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; +using behavior_path_planner::lane_change::LCParamPtr; using behavior_path_planner::lane_change::ModuleType; using behavior_path_planner::lane_change::PathSafetyStatus; using behavior_path_planner::lane_change::TargetLaneLeadingObjects; +using behavior_path_planner::lane_change::TrajectoryGroup; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using tier4_planning_msgs::msg::PathWithLaneId; -bool is_mandatory_lane_change(const ModuleType lc_type); +rclcpp::Logger get_logger(); -double calcLaneChangeResampleInterval( - const double lane_changing_length, const double lane_changing_velocity); +bool is_mandatory_lane_change(const ModuleType lc_type); -void setPrepareVelocity( +void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids); +/** + * @brief Replaces the current lane IDs with a sorted set of IDs based on a predefined mapping. + * + * This function checks if the current lane IDs match the previously processed lane IDs. + * If they do, it returns the previously sorted IDs for efficiency. Otherwise, it matches + * the current lane IDs to the appropriate sorted IDs from the provided mapping and updates + * the cached values. + * + * @param current_lane_ids The current lane IDs to be replaced or verified. + * @param sorted_lane_ids A vector of sorted lane ID groups, each representing a predefined + * order of IDs for specific conditions. + * @param prev_lane_ids Reference to the previously processed lane IDs for caching purposes. + * @param prev_sorted_lane_ids Reference to the previously sorted lane IDs for caching purposes. + * + * @return std::vector The sorted lane IDs if a match is found, or the original + * `current_lane_ids` if no match exists. + */ +std::vector replace_with_sorted_ids( + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids); std::vector> get_sorted_lane_ids(const CommonDataPtr & common_data_ptr); @@ -79,27 +100,10 @@ lanelet::ConstLanelets get_target_neighbor_lanes( 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); - bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin = 0.1); -std::optional construct_candidate_path( - const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, - const std::vector> & sorted_lane_ids); - -ShiftLine get_lane_changing_shift_line( - const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, - const PathWithLaneId & reference_path, const double shift_length); - -PathWithLaneId get_reference_path_from_target_Lane( - const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, - const double lane_changing_length, const double resample_interval); - std::vector generateDrivableLanes( const std::vector & original_drivable_lanes, const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & lane_change_lanes); @@ -141,8 +145,8 @@ bool isParkedObject( * If the parameter delay_lc_param.check_only_parked_vehicle is set to True, only objects * which pass isParkedObject() check will be considered. * - * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, parameters, - * and transient data. + * @param common_data_ptr Shared pointer to CommonData that holds necessary lanes info, + * parameters, and transient data. * @param lane_change_path Candidate lane change path to apply checks on. * @param target_objects Relevant objects to consider for delay LC checks (assumed to only include * target lane leading static objects). @@ -202,8 +206,8 @@ rclcpp::Logger getLogger(const std::string & type); * 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 common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions - * and pose information. + * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's + * dimensions and pose information. * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ @@ -233,15 +237,15 @@ bool is_within_intersection( /** * @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. + * 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. + * @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 is_within_turn_direction_lanes( const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); @@ -276,8 +280,8 @@ double get_distance_to_next_regulatory_element( * * @param common_data_ptr Pointer to the common data structure containing parameters for lane * change. - * @param filtered_objects A collection of objects filtered by lanes, including those in the current - * lane. + * @param filtered_objects A collection of objects filtered by lanes, including those in the + * current lane. * @param dist_to_target_lane_start The distance to the start of the target lane from the ego * vehicle. * @param path The current path of the ego vehicle, containing path points and lane information. @@ -297,8 +301,8 @@ double get_min_dist_to_current_lanes_obj( * * @param common_data_ptr Pointer to the common data structure containing parameters for the lane * change. - * @param filtered_objects A collection of objects filtered by lanes, including those in the target - * lane. + * @param filtered_objects A collection of objects filtered by lanes, including those in the + * target lane. * @param stop_arc_length The arc length at which the ego vehicle is expected to stop. * @param path The current path of the ego vehicle, containing path points and lane information. * @return true if there is an object in the target lane that influences the stop point decision; @@ -365,14 +369,15 @@ bool has_overtaking_turn_lane_object( * * @param common_data_ptr Shared pointer to CommonData containing information about current lanes, * vehicle dimensions, lane polygons, and behavior parameters. - * @param object An extended predicted object representing a potential obstacle in the environment. + * @param object An extended predicted object representing a potential obstacle in the + * environment. * @param dist_ego_to_current_lanes_center Distance from the ego vehicle to the center of the * current lanes. * @param ahead_of_ego Boolean flag indicating if the object is ahead of the ego vehicle. - * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point of - * the lane. - * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, or - * outside boundaries). + * @param before_terminal Boolean flag indicating if the ego vehicle is before the terminal point + * of the lane. + * @param leading_objects Reference to a structure for storing leading objects (stopped, moving, + * or outside boundaries). * @param trailing_objects Reference to a collection for storing trailing objects. * * @return true if the object is classified as either leading or trailing, false otherwise. @@ -383,6 +388,22 @@ bool filter_target_lane_objects( const bool before_terminal, TargetLaneLeadingObjects & leading_objects, ExtendedPredictedObjects & trailing_objects); +/** + * @brief Retrieves the preceding lanes for the target lanes while removing overlapping and + * disconnected lanes. + * + * This function identifies all lanes that precede the target lanes based on the ego vehicle's + * current position and a specified backward search length. The resulting preceding lanes are + * filtered to remove lanes that overlap with the current lanes or are not connected to the route. + * + * @param common_data_ptr Shared pointer to commonly used data in lane change module, which contains + * route handler information, lane details, ego vehicle pose, and behavior parameters. + * + * @return A vector of preceding lanelet groups, with each group containing only the connected and + * non-overlapping preceding lanes. + */ +std::vector get_preceding_lanes(const CommonDataPtr & common_data_ptr); + /** * @brief Determines if the object's predicted path overlaps with the given lane polygon. * @@ -399,5 +420,57 @@ bool filter_target_lane_objects( */ bool object_path_overlaps_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon); + +/** + * @brief Converts a lane change path into multiple predicted paths with varying acceleration + * profiles. + * + * This function generates a set of predicted paths for the ego vehicle during a lane change, + * using different acceleration values within the specified range. It accounts for deceleration + * sampling if the global minimum acceleration differs from the lane-changing acceleration. + * + * @param common_data_ptr Shared pointer to CommonData containing parameters and state information. + * @param lane_change_path The lane change path used to generate predicted paths. + * @param deceleration_sampling_num Number of samples for deceleration profiles to generate paths. + * + * @return std::vector> A collection of predicted paths, where + * each path is represented as a series of poses with associated velocity. + */ +std::vector> convert_to_predicted_paths( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const size_t deceleration_sampling_num); + +/** + * @brief Validates whether a given pose is a valid starting point for a lane change. + * + * This function checks if the specified pose lies within the polygons representing + * the target lane or its neighboring areas. This ensures that the starting point is + * appropriately positioned for initiating a lane change, even if previous paths were adjusted. + * + * @param common_data_ptr Shared pointer to CommonData containing lane polygon information. + * @param pose The pose to validate as a potential lane change starting point. + * + * @return true if the pose is within the target or target neighbor polygons, false otherwise. + */ +bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose); + +/** + * @brief Converts a lane change frenet candidate into a predicted path for the ego vehicle. + * + * This function generates a predicted path based on the provided Frenet candidate, + * simulating the vehicle's trajectory during the preparation and lane-changing phases. + * It interpolates poses and velocities over the duration of the prediction, considering + * the ego vehicle's initial conditions and the candidate's trajectory data. + * + * @param common_data_ptr Shared pointer to CommonData containing parameters and ego vehicle state. + * @param frenet_candidate A Frenet trajectory group representing the lane change candidate. + * @param deceleration_sampling_num Unused parameter for deceleration sampling count. + * + * @return std::vector The predicted path as a series of stamped poses + * with associated velocities over the prediction time. + */ +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num); } // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml index dd4dbe63e41d4..40fd6fc4c572d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_lane_change_module - 0.40.0 + 0.41.0 The autoware_behavior_path_lane_change_module package Fumiya Watanabe @@ -23,9 +23,11 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_frenet_planner autoware_motion_utils autoware_rtc_interface autoware_universe_utils + fmt pluginlib range-v3 rclcpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index d65e51997eb32..4519b20d4644d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -40,16 +40,14 @@ LaneChangeInterface::LaneChangeInterface( const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface, std::unique_ptr && module_type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{std::move(parameters)}, - module_type_{std::move(module_type)}, - prev_approved_path_{std::make_unique()} + module_type_{std::move(module_type)} { module_type_->setTimeKeeper(getTimeKeeper()); logger_ = utils::lane_change::getLogger(module_type_->getModuleTypeStr()); - steering_factor_interface_.init(PlanningBehavior::LANE_CHANGE); - velocity_factor_interface_.init(PlanningBehavior::LANE_CHANGE); } void LaneChangeInterface::processOnExit() @@ -109,11 +107,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); path_reference_ = std::make_shared(output.reference_path); - *prev_approved_path_ = getPreviousModuleOutput().path; - const auto stop_pose_opt = module_type_->getStopPose(); - stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value())) - : PoseWithDetailOpt(); + 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) { @@ -143,25 +138,22 @@ BehaviorModuleOutput LaneChangeInterface::plan() const auto force_activated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); - const bool safe = force_activated ? false : true; updateRTCStatus( - path.start_distance_to_path_change, path.finish_distance_to_path_change, safe, + path.start_distance_to_path_change, path.finish_distance_to_path_change, !force_activated, State::RUNNING); } } - setVelocityFactor(output.path); + set_longitudinal_planning_factor(output.path); return output; } BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { - BehaviorModuleOutput out = getPreviousModuleOutput(); + BehaviorModuleOutput out = module_type_->getTerminalLaneChangePath(); - *prev_approved_path_ = out.path; - - module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path); + module_type_->insert_stop_point(module_type_->get_current_lanes(), out.path, true); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); @@ -171,9 +163,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() } path_reference_ = std::make_shared(out.reference_path); - const auto stop_pose_opt = module_type_->getStopPose(); - stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value())) - : PoseWithDetailOpt(); + stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); @@ -188,7 +178,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() isExecutionReady(), State::WAITING_FOR_EXECUTION); is_abort_path_approved_ = false; - setVelocityFactor(out.path); + set_longitudinal_planning_factor(out.path); return out; } @@ -228,9 +218,7 @@ bool LaneChangeInterface::canTransitSuccessState() if (module_type_->specialExpiredCheck() && isWaitingApproval()) { log_debug_throttled("Run specialExpiredCheck."); - if (isWaitingApproval()) { - return true; - } + return true; } if (module_type_->hasFinishedLaneChange()) { @@ -245,13 +233,6 @@ bool LaneChangeInterface::canTransitSuccessState() bool LaneChangeInterface::canTransitFailureState() { - auto log_debug_throttled = [&](std::string_view message) -> void { - RCLCPP_DEBUG(getLogger(), "%s", message.data()); - }; - - updateDebugMarker(); - log_debug_throttled(__func__); - const auto force_activated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); }); @@ -261,121 +242,106 @@ bool LaneChangeInterface::canTransitFailureState() return false; } - if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) { - log_debug_throttled("Abort process has on going."); - return false; - } + const auto [state, reason] = check_transit_failure(); - if (isWaitingApproval()) { - if (module_type_->is_near_regulatory_element()) { - log_debug_throttled("Ego is close to regulatory element. Cancel lane change"); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; - } - log_debug_throttled("Can't transit to failure state. Module is WAITING_FOR_APPROVAL"); - return false; - } + interface_debug_.failing_reason = reason; + interface_debug_.lc_state = state; - if (!module_type_->isValidPath()) { - log_debug_throttled("Transit to failure state due not to find valid path"); + updateDebugMarker(); + + if (state == LaneChangeStates::Cancel) { updateRTCStatus( std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, State::FAILED); + module_type_->toCancelState(); return true; } - if (module_type_->isAbortState() && module_type_->hasFinishedAbort()) { - log_debug_throttled("Abort process has completed."); - updateRTCStatus( - std::numeric_limits::lowest(), std::numeric_limits::lowest(), true, - State::FAILED); - return true; + if (state == LaneChangeStates::Abort) { + module_type_->toAbortState(); + return false; } - if (module_type_->is_near_terminal()) { - log_debug_throttled("Unsafe, but ego is approaching terminal. Continue lane change"); + // Note: Ideally, if it is unsafe, but for some reason, we can't abort or cancel, then we should + // stop. Note: This feature is not working properly for now. + const auto [is_safe, unsafe_trailing_obj] = post_process_safety_status_; + if (!is_safe && module_type_->isRequiredStop(unsafe_trailing_obj)) { + module_type_->toStopState(); + return false; + } + + module_type_->toNormalState(); + return false; +} - if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { - log_debug_throttled("Module require stopping"); +std::pair LaneChangeInterface::check_transit_failure() +{ + if (module_type_->isAbortState()) { + if (module_type_->hasFinishedAbort()) { + return {LaneChangeStates::Cancel, "Aborted"}; } - return false; + return {LaneChangeStates::Abort, "Aborting"}; } - 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 (isWaitingApproval()) { + if (module_type_->is_near_regulatory_element()) { + return {LaneChangeStates::Cancel, "CloseToRegElement"}; } + return {LaneChangeStates::Normal, "WaitingForApproval"}; + } + if (!module_type_->isValidPath()) { + return {LaneChangeStates::Cancel, "InvalidPath"}; + } + + const auto is_preparing = module_type_->isEgoOnPreparePhase(); + const auto can_return_to_current = module_type_->isAbleToReturnCurrentLane(); + + // regardless of safe and unsafe, we want to cancel lane change. + if (is_preparing) { const auto force_deactivated = std::any_of( rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) { return rtc.second->isForceDeactivated(uuid_map_.at(rtc.first)); }); - if (force_deactivated && module_type_->isAbleToReturnCurrentLane()) { - log_debug_throttled("Cancel lane change due to force deactivation"); - 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 (force_deactivated && can_return_to_current) { + return {LaneChangeStates::Cancel, "ForceDeactivation"}; } } 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; + return {LaneChangeStates::Normal, "SafeToLaneChange"}; } - if (module_type_->isRequiredStop(post_process_safety_status_.is_trailing_object)) { - log_debug_throttled("Module require stopping"); + if (!module_type_->isCancelEnabled()) { + return {LaneChangeStates::Warning, "CancelDisabled"}; } - if (!module_type_->isCancelEnabled()) { - log_debug_throttled( - "Lane change path is unsafe but cancel was not enabled. Continue lane change."); - return false; + // We also check if the ego can return to the current lane, as prepare segment might be out of the + // lane, for example, during an evasive maneuver around a static object. + if (is_preparing && can_return_to_current) { + return {LaneChangeStates::Cancel, "SafeToCancel"}; + } + + if (module_type_->is_near_terminal()) { + return {LaneChangeStates::Warning, "TooNearTerminal"}; } if (!module_type_->isAbortEnabled()) { - log_debug_throttled( - "Lane change path is unsafe but abort was not enabled. Continue lane change."); - return false; + return {LaneChangeStates::Warning, "AbortDisabled"}; } - if (!module_type_->isAbleToReturnCurrentLane()) { - log_debug_throttled("It's is not possible to return to original lane. Continue lane change."); - return false; + // To prevent the lane module from having to check rear objects in the current lane, we limit the + // abort maneuver to cases where the ego vehicle is still in the current lane. + if (!can_return_to_current) { + return {LaneChangeStates::Warning, "TooLateToAbort"}; } 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; + return {LaneChangeStates::Warning, "AbortPathNotFound"}; } - log_debug_throttled("Lane change path is unsafe. Abort lane change."); - module_type_->toAbortState(); - return false; + return {LaneChangeStates::Abort, "SafeToAbort"}; } void LaneChangeInterface::updateDebugMarker() const @@ -385,7 +351,8 @@ void LaneChangeInterface::updateDebugMarker() const return; } using marker_utils::lane_change_markers::createDebugMarkerArray; - debug_marker_ = createDebugMarkerArray(module_type_->getDebugData(), module_type_->getEgoPose()); + debug_marker_ = createDebugMarkerArray( + interface_debug_, module_type_->getDebugData(), module_type_->getEgoPose()); } MarkerArray LaneChangeInterface::getModuleVirtualWall() @@ -416,15 +383,6 @@ MarkerArray LaneChangeInterface::getModuleVirtualWall() void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); - 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(); @@ -433,24 +391,34 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o const auto finish_distance = autoware::motion_utils::calcSignedArcLength( output.path.points, current_position, status.lane_change_path.info.shift_line.end.position); - steering_factor_interface_.set( - {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); + const auto planning_factor_direction = std::invoke([&]() { + if (module_type_->getDirection() == Direction::LEFT) { + return PlanningFactor::SHIFT_LEFT; + } + if (module_type_->getDirection() == Direction::RIGHT) { + return PlanningFactor::SHIFT_RIGHT; + } + return PlanningFactor::UNKNOWN; + }); + + planning_factor_interface_->add( + start_distance, finish_distance, status.lane_change_path.info.shift_line.start, + status.lane_change_path.info.shift_line.end, planning_factor_direction, SafetyFactorArray{}); } void LaneChangeInterface::updateSteeringFactorPtr( const CandidateOutput & output, const LaneChangePath & selected_path) const { - const uint16_t steering_factor_direction = std::invoke([&output]() { + const uint16_t planning_factor_direction = std::invoke([&output]() { if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; } - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; }); - steering_factor_interface_.set( - {selected_path.info.shift_line.start, selected_path.info.shift_line.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + output.start_distance_to_path_change, output.finish_distance_to_path_change, + selected_path.info.shift_line.start, selected_path.info.shift_line.end, + planning_factor_direction, SafetyFactorArray{}); } } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index 1bb41069140e7..589338d7b3ace 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -63,6 +63,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("trajectory.min_lane_changing_velocity")); p.trajectory.lane_changing_decel_factor = getOrDeclareParameter(*node, parameter("trajectory.lane_changing_decel_factor")); + p.trajectory.th_prepare_curvature = + getOrDeclareParameter(*node, parameter("trajectory.th_prepare_curvature")); p.trajectory.lon_acc_sampling_num = getOrDeclareParameter(*node, parameter("trajectory.lon_acc_sampling_num")); p.trajectory.lat_acc_sampling_num = @@ -150,6 +152,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("collision_check.prediction_time_resolution")); p.safety.collision_check.th_yaw_diff = getOrDeclareParameter(*node, parameter("collision_check.yaw_diff_threshold")); + p.safety.collision_check.th_incoming_object_yaw = + getOrDeclareParameter(*node, parameter("collision_check.th_incoming_object_yaw")); // rss check auto set_rss_params = [&](auto & params, const std::string & prefix) { @@ -167,6 +171,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s *node, parameter(prefix + ".rear_vehicle_safety_time_margin")); params.lateral_distance_max_threshold = getOrDeclareParameter(*node, parameter(prefix + ".lateral_distance_max_threshold")); + params.extended_polygon_policy = + getOrDeclareParameter(*node, parameter(prefix + ".extended_polygon_policy")); }; set_rss_params(p.safety.rss_params, "safety_check.execution"); set_rss_params(p.safety.rss_params_for_parked, "safety_check.parked"); @@ -188,6 +194,7 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s } // lane change parameters + p.time_limit = getOrDeclareParameter(*node, parameter("time_limit")); p.backward_lane_length = getOrDeclareParameter(*node, parameter("backward_lane_length")); p.backward_length_buffer_for_end_of_lane = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); @@ -195,6 +202,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); p.backward_length_from_intersection = getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); + p.enable_stopped_vehicle_buffer = + getOrDeclareParameter(*node, parameter("enable_stopped_vehicle_buffer")); if (p.backward_length_buffer_for_end_of_lane < 1.0) { RCLCPP_WARN_STREAM( @@ -211,6 +220,12 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s p.delay.th_parked_vehicle_shift_ratio = getOrDeclareParameter( *node, parameter("delay_lane_change.th_parked_vehicle_shift_ratio")); + // trajectory generation near terminal using frenet planner + p.frenet.enable = getOrDeclareParameter(*node, parameter("frenet.enable")); + p.frenet.th_yaw_diff_deg = getOrDeclareParameter(*node, parameter("frenet.th_yaw_diff")); + p.frenet.th_curvature_smoothing = + getOrDeclareParameter(*node, parameter("frenet.th_curvature_smoothing")); + // lane change cancel p.cancel.enable_on_prepare_phase = getOrDeclareParameter(*node, parameter("cancel.enable_on_prepare_phase")); @@ -240,6 +255,13 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s // debug marker p.publish_debug_marker = getOrDeclareParameter(*node, parameter("publish_debug_marker")); + // terminal lane change path + p.terminal_path.enable = getOrDeclareParameter(*node, parameter("terminal_path.enable")); + p.terminal_path.disable_near_goal = + getOrDeclareParameter(*node, parameter("terminal_path.disable_near_goal")); + p.terminal_path.stop_at_boundary = + getOrDeclareParameter(*node, parameter("terminal_path.stop_at_boundary")); + // validation of safety check parameters // if loose check is not enabled, lane change module will keep on chattering and canceling, and // false positive situation might occur @@ -284,7 +306,7 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } @@ -296,6 +318,17 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortime_limit; + updateParam(parameters, ns + "time_limit", time_limit); + if (time_limit >= 10.0) { + p->time_limit = time_limit; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'time_limit' is not updated because the value (%.3f ms) is not valid, " + "keep current value (%.3f ms)", + time_limit, p->time_limit); + } updateParam(parameters, ns + "backward_lane_length", p->backward_lane_length); updateParam( parameters, ns + "backward_length_buffer_for_end_of_lane", @@ -305,10 +338,14 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectorbackward_length_buffer_for_blocking_object); updateParam( parameters, ns + "lane_change_finish_judge_buffer", p->lane_change_finish_judge_buffer); - + updateParam( + parameters, ns + "enable_stopped_vehicle_buffer", p->enable_stopped_vehicle_buffer); updateParam( parameters, ns + "finish_judge_lateral_threshold", p->th_finish_judge_lateral_diff); updateParam(parameters, ns + "publish_debug_marker", p->publish_debug_marker); + updateParam( + parameters, ns + "min_length_for_turn_signal_activation", + p->min_length_for_turn_signal_activation); } { @@ -319,24 +356,37 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.min_prepare_duration); updateParam(parameters, ns + "lateral_jerk", p->trajectory.lateral_jerk); updateParam( - parameters, ns + ".min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); - // longitudinal acceleration + parameters, ns + "min_lane_changing_velocity", p->trajectory.min_lane_changing_velocity); updateParam( parameters, ns + "min_longitudinal_acc", p->trajectory.min_longitudinal_acc); updateParam( parameters, ns + "max_longitudinal_acc", p->trajectory.max_longitudinal_acc); updateParam( parameters, ns + "lane_changing_decel_factor", p->trajectory.lane_changing_decel_factor); - int longitudinal_acc_sampling_num = 0; + updateParam( + parameters, ns + "th_prepare_curvature", p->trajectory.th_prepare_curvature); + int longitudinal_acc_sampling_num = p->trajectory.lon_acc_sampling_num; updateParam(parameters, ns + "lon_acc_sampling_num", longitudinal_acc_sampling_num); if (longitudinal_acc_sampling_num > 0) { p->trajectory.lon_acc_sampling_num = longitudinal_acc_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lon_acc_sampling_num' is not updated because the value (%d) is not " + "positive", + longitudinal_acc_sampling_num); } - int lateral_acc_sampling_num = 0; + int lateral_acc_sampling_num = p->trajectory.lat_acc_sampling_num; updateParam(parameters, ns + "lat_acc_sampling_num", lateral_acc_sampling_num); if (lateral_acc_sampling_num > 0) { p->trajectory.lat_acc_sampling_num = lateral_acc_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Parameter 'lat_acc_sampling_num' is not updated because the value (%d) is not " + "positive", + lateral_acc_sampling_num); } updateParam( @@ -345,12 +395,84 @@ void LaneChangeModuleManager::updateModuleParams(const std::vectortrajectory.th_lane_changing_length_diff); } + { + const std::string ns = "lane_change.frenet."; + updateParam(parameters, ns + "enable", p->frenet.enable); + updateParam(parameters, ns + "th_yaw_diff", p->frenet.th_yaw_diff_deg); + updateParam( + parameters, ns + "th_curvature_smoothing", p->frenet.th_curvature_smoothing); + } + { const std::string ns = "lane_change.safety_check.lane_expansion."; updateParam(parameters, ns + "left_offset", p->safety.lane_expansion_left_offset); updateParam(parameters, ns + "right_offset", p->safety.lane_expansion_right_offset); } + { + const std::string ns = "lane_change.lateral_acceleration."; + std::vector velocity = p->trajectory.lat_acc_map.base_vel; + std::vector min_values = p->trajectory.lat_acc_map.base_min_acc; + std::vector max_values = p->trajectory.lat_acc_map.base_max_acc; + + updateParam>(parameters, ns + "velocity", velocity); + updateParam>(parameters, ns + "min_values", min_values); + updateParam>(parameters, ns + "max_values", max_values); + if ( + velocity.size() >= 2 && velocity.size() == min_values.size() && + velocity.size() == max_values.size()) { + LateralAccelerationMap lat_acc_map; + for (size_t i = 0; i < velocity.size(); ++i) { + lat_acc_map.add(velocity.at(i), min_values.at(i), max_values.at(i)); + } + p->trajectory.lat_acc_map = lat_acc_map; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Mismatched size for lateral acceleration. Expected size: %lu, but velocity: %lu, " + "min_values: %lu, max_values: %lu", + std::max(2ul, velocity.size()), velocity.size(), min_values.size(), max_values.size()); + } + } + + { + const std::string ns = "lane_change.collision_check."; + updateParam( + parameters, ns + "enable_for_prepare_phase.general_lanes", + p->safety.collision_check.enable_for_prepare_phase_in_general_lanes); + updateParam( + parameters, ns + "enable_for_prepare_phase.intersection", + p->safety.collision_check.enable_for_prepare_phase_in_intersection); + updateParam( + parameters, ns + "enable_for_prepare_phase.turns", + p->safety.collision_check.enable_for_prepare_phase_in_turns); + updateParam( + parameters, ns + "check_current_lanes", p->safety.collision_check.check_current_lane); + updateParam( + parameters, ns + "check_other_lanes", p->safety.collision_check.check_other_lanes); + updateParam( + parameters, ns + "use_all_predicted_paths", + p->safety.collision_check.use_all_predicted_paths); + updateParam( + parameters, ns + "prediction_time_resolution", + p->safety.collision_check.prediction_time_resolution); + updateParam( + parameters, ns + "yaw_diff_threshold", p->safety.collision_check.th_yaw_diff); + + auto th_incoming_object_yaw = p->safety.collision_check.th_incoming_object_yaw; + updateParam(parameters, ns + "th_incoming_object_yaw", th_incoming_object_yaw); + if (th_incoming_object_yaw >= M_PI_2) { + p->safety.collision_check.th_incoming_object_yaw = th_incoming_object_yaw; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, + "The value of th_incoming_object_yaw (%.3f rad) is less than the minimum possible value " + "(%.3f " + "rad).", + th_incoming_object_yaw, M_PI_2); + } + } + { const std::string ns = "lane_change.target_object."; updateParam(parameters, ns + "car", p->safety.target_object_types.check_car); @@ -378,23 +500,96 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector(parameters, ns + "stop_time", p->th_stop_time); } + auto update_rss_params = [¶meters, this](const std::string & prefix, auto & params) { + using autoware::universe_utils::updateParam; + updateParam( + parameters, prefix + "longitudinal_distance_min_threshold", + params.longitudinal_distance_min_threshold); + updateParam( + parameters, prefix + "longitudinal_velocity_delta_time", + params.longitudinal_velocity_delta_time); + updateParam( + parameters, prefix + "expected_front_deceleration", params.front_vehicle_deceleration); + updateParam( + parameters, prefix + "expected_rear_deceleration", params.rear_vehicle_deceleration); + updateParam( + parameters, prefix + "rear_vehicle_reaction_time", params.rear_vehicle_reaction_time); + updateParam( + parameters, prefix + "rear_vehicle_safety_time_margin", + params.rear_vehicle_safety_time_margin); + updateParam( + parameters, prefix + "lateral_distance_max_threshold", params.lateral_distance_max_threshold); + + auto extended_polygon_policy = params.extended_polygon_policy; + updateParam( + parameters, prefix + "extended_polygon_policy", extended_polygon_policy); + if (extended_polygon_policy == "rectangle" || extended_polygon_policy == "along_path") { + params.extended_polygon_policy = extended_polygon_policy; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Policy %s not supported or there's typo. Make sure you choose either 'rectangle' or " + "'along_path'", + extended_polygon_policy.c_str()); + } + }; + + update_rss_params("lane_change.safety_check.execution.", p->safety.rss_params); + update_rss_params("lane_change.safety_check.parked.", p->safety.rss_params_for_parked); + update_rss_params("lane_change.safety_check.cancel.", p->safety.rss_params_for_abort); + update_rss_params("lane_change.safety_check.stuck.", p->safety.rss_params_for_stuck); + + { + const std::string ns = "lane_change.delay_lane_change."; + updateParam(parameters, ns + "enable", p->delay.enable); + updateParam( + parameters, ns + "check_only_parked_vehicle", p->delay.check_only_parked_vehicle); + updateParam( + parameters, ns + "min_road_shoulder_width", p->delay.min_road_shoulder_width); + updateParam( + parameters, ns + "th_parked_vehicle_shift_ratio", p->delay.th_parked_vehicle_shift_ratio); + } + + { + const std::string ns = "lane_change.terminal_path."; + updateParam(parameters, ns + "enable", p->terminal_path.enable); + updateParam(parameters, ns + "disable_near_goal", p->terminal_path.disable_near_goal); + updateParam(parameters, ns + "stop_at_boundary", p->terminal_path.stop_at_boundary); + } + { const std::string ns = "lane_change.cancel."; - bool enable_on_prepare_phase = true; + bool enable_on_prepare_phase = p->cancel.enable_on_prepare_phase; 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."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change cancel function is disabled."); p->cancel.enable_on_prepare_phase = enable_on_prepare_phase; } - bool enable_on_lane_changing_phase = true; + bool enable_on_lane_changing_phase = p->cancel.enable_on_lane_changing_phase; 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."); + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "WARNING! Lane Change abort function is disabled."); p->cancel.enable_on_lane_changing_phase = enable_on_lane_changing_phase; } + int deceleration_sampling_num = p->cancel.deceleration_sampling_num; + updateParam(parameters, ns + "deceleration_sampling_num", deceleration_sampling_num); + if (deceleration_sampling_num > 0) { + p->cancel.deceleration_sampling_num = deceleration_sampling_num; + } else { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, + "Parameter 'deceleration_sampling_num' is not updated because the value (%d) is not " + "positive", + deceleration_sampling_num); + } + 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); @@ -402,6 +597,7 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector( parameters, ns + "unsafe_hysteresis_threshold", p->cancel.th_unsafe_hysteresis); } + std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); }); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 2770831dbc485..a641f08820aec 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -15,6 +15,7 @@ #include "autoware/behavior_path_lane_change_module/scene.hpp" #include "autoware/behavior_path_lane_change_module/utils/calculation.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/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -27,21 +28,28 @@ #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 @@ -102,6 +110,9 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->current_lanes_path = route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->target_lanes_path = + route_handler_ptr->getCenterLinePath(target_lanes, 0.0, std::numeric_limits::max()); + common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); @@ -112,9 +123,8 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(target_lanes.back()); - common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( - *route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), - common_data_ptr_->lc_param_ptr->backward_lane_length); + common_data_ptr_->lanes_ptr->preceding_target = + utils::lane_change::get_preceding_lanes(common_data_ptr_); lane_change_debug_.current_lanes = common_data_ptr_->lanes_ptr->current; lane_change_debug_.target_lanes = common_data_ptr_->lanes_ptr->target; @@ -169,6 +179,9 @@ void NormalLaneChange::update_transient_data(const bool is_approved) transient_data.dist_to_terminal_start = transient_data.dist_to_terminal_end - transient_data.current_dist_buffer.min; + transient_data.dist_to_target_end = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target, common_data_ptr_->get_ego_pose()); + transient_data.max_prepare_length = calculation::calc_maximum_prepare_length(common_data_ptr_); transient_data.target_lane_length = @@ -228,8 +241,6 @@ void NormalLaneChange::updateLaneChangeStatus() // Update status status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - - status_.start_distance = common_data_ptr_->transient_data.target_lanes_ego_arc.length; status_.lane_change_path.path.header = getRouteHeader(); } @@ -246,6 +257,10 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool found_safe_path = get_lane_change_paths(valid_paths); // if no safe path is found and ego is stuck, try to find a path with a small margin + if (valid_paths.empty() && terminal_lane_change_path_) { + valid_paths.push_back(terminal_lane_change_path_.value()); + } + lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { @@ -382,17 +397,18 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const { - auto output = prev_module_output_; + if ( + !lane_change_parameters_->terminal_path.enable || + !common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + return 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 is_near_goal = lane_change_parameters_->terminal_path.disable_near_goal && + common_data_ptr_->lanes_ptr->target_lane_in_goal_section && + common_data_ptr_->transient_data.dist_to_target_end < + common_data_ptr_->transient_data.lane_changing_length.max; + if (is_near_goal) { + return prev_module_output_; } const auto & current_lanes = get_current_lanes(); @@ -401,23 +417,19 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const return prev_module_output_; } - const auto terminal_path = - calcTerminalLaneChangePath(current_lanes, get_lane_change_lanes(current_lanes)); - if (!terminal_path) { + const auto terminal_lc_path = compute_terminal_lane_change_path(); + + if (!terminal_lc_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(); + auto output = prev_module_output_; - extendOutputDrivableArea(output); + output.path = *terminal_lc_path; + output.turn_signal_info = get_current_turn_signal_info(); - 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); + extendOutputDrivableArea(output); return output; } @@ -450,7 +462,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() 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)); - set_stop_pose(stop_dist + current_dist, output.path); + set_stop_pose(stop_dist + current_dist, output.path, "incoming rear object"); } else { insert_stop_point(get_target_lanes(), output.path); } @@ -492,7 +504,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c } void NormalLaneChange::insert_stop_point( - const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) + const lanelet::ConstLanelets & lanelets, PathWithLaneId & path, const bool is_waiting_approval) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); if (lanelets.empty()) { @@ -513,59 +525,62 @@ void NormalLaneChange::insert_stop_point( if (!is_current_lane) { const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) - common_data_ptr_->transient_data.next_dist_buffer.min; - set_stop_pose(arc_length_to_stop_pose, path); + set_stop_pose(arc_length_to_stop_pose, path, "next lc terminal"); return; } - insert_stop_point_on_current_lanes(path); + insert_stop_point_on_current_lanes(path, is_waiting_approval); } -void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) +void NormalLaneChange::insert_stop_point_on_current_lanes( + PathWithLaneId & path, const bool is_waiting_approval) { const auto & path_front_pose = path.points.front().point.pose; - const auto & center_line = common_data_ptr_->current_lanes_path.points; - const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) { - return motion_utils::calcSignedArcLength( - center_line, path_front_pose.position, target.position); - }; + const auto & ego_pose = common_data_ptr_->get_ego_pose(); + const auto dist_from_path_front = + motion_utils::calcSignedArcLength(path.points, path_front_pose.position, ego_pose.position); const auto & transient_data = common_data_ptr_->transient_data; const auto & lanes_ptr = common_data_ptr_->lanes_ptr; const auto & lc_param_ptr = common_data_ptr_->lc_param_ptr; - const auto dist_to_terminal = std::invoke([&]() -> double { - const auto target_pose = (lanes_ptr->current_lane_in_goal_section) - ? common_data_ptr_->route_handler_ptr->getGoalPose() - : center_line.back().point.pose; - return get_arc_length_along_lanelet(target_pose); - }); - - const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; - const auto min_dist_buffer = transient_data.current_dist_buffer.min; const auto dist_to_terminal_start = - dist_to_terminal - min_dist_buffer - calculation::calc_stopping_distance(lc_param_ptr); + transient_data.dist_to_terminal_start - calculation::calc_stopping_distance(lc_param_ptr); - const auto distance_to_last_fit_width = std::invoke([&]() -> double { + const auto & bpp_param_ptr = common_data_ptr_->bpp_param_ptr; + const auto dist_to_terminal_stop = std::invoke([&]() -> double { const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current; - if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { - return utils::lane_change::calculation::calc_dist_to_last_fit_width( - lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); + if (!utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), *bpp_param_ptr)) { + return dist_from_path_front + dist_to_terminal_start; + } + + if ( + terminal_lane_change_path_ && is_waiting_approval && + lc_param_ptr->terminal_path.stop_at_boundary) { + return calculation::calc_dist_to_last_fit_width(common_data_ptr_, path); } - return std::numeric_limits::max(); - }); - const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + const auto dist_to_last_fit_width = calculation::calc_dist_to_last_fit_width( + lanes_ptr->current, path.points.front().point.pose, *bpp_param_ptr); - if (filtered_objects_.current_lane.empty()) { - set_stop_pose(dist_to_terminal_stop, path); + return std::min(dist_from_path_front + dist_to_terminal_start, dist_to_last_fit_width); + }); + + const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path"; + if ( + filtered_objects_.current_lane.empty() || + !lane_change_parameters_->enable_stopped_vehicle_buffer) { + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } + const auto & center_line = common_data_ptr_->current_lanes_path.points; const auto dist_to_target_lane_start = std::invoke([&]() -> double { const auto & front_lane = lanes_ptr->target_neighbor.front(); const auto target_front = utils::to_geom_msg_pose(front_lane.centerline2d().front(), front_lane); - return get_arc_length_along_lanelet(target_front); + return motion_utils::calcSignedArcLength( + center_line, path_front_pose.position, target_front.position); }); const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj( @@ -583,12 +598,12 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin; if (stop_arc_length_behind_obj < dist_to_target_lane_start) { - set_stop_pose(dist_to_target_lane_start, path); + set_stop_pose(dist_to_target_lane_start, path, "maintain distance to front object"); return; } if (stop_arc_length_behind_obj > dist_to_terminal_stop) { - set_stop_pose(dist_to_terminal_stop, path); + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } @@ -604,11 +619,11 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path); if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { - set_stop_pose(dist_to_terminal_stop, path); + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } - set_stop_pose(stop_arc_length_behind_obj, path); + set_stop_pose(stop_arc_length_behind_obj, path, "maintain distance to front object"); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -682,7 +697,7 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; - status_ = {}; + status_ = LaneChangeStatus(); unsafe_hysteresis_count_ = 0; lane_change_debug_.reset(); @@ -884,7 +899,6 @@ bool NormalLaneChange::isAbleToStopSafely() const bool NormalLaneChange::hasFinishedAbort() const { if (!abort_path_) { - lane_change_debug_.is_abort = true; return true; } @@ -893,7 +907,6 @@ bool NormalLaneChange::hasFinishedAbort() const 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; } @@ -908,56 +921,7 @@ bool NormalLaneChange::isAbortState() const 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)); -} - -bool NormalLaneChange::get_prepare_segment( - PathWithLaneId & prepare_segment, const double prepare_length) const -{ - const auto & current_lanes = common_data_ptr_->lanes_ptr->current; - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - const auto backward_path_length = common_data_ptr_->bpp_param_ptr->backward_path_length; - - if (current_lanes.empty() || target_lanes.empty()) { - return false; - } - - prepare_segment = prev_module_output_.path; - const size_t current_seg_idx = - autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prepare_segment.points, getEgoPose(), 3.0, 1.0); - utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - - if (prepare_segment.points.empty()) return false; - - const auto & lc_start_pose = prepare_segment.points.back().point.pose; - - // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is - // larger than distance to target lane start - if (!is_valid_start_point(common_data_ptr_, lc_start_pose)) return false; - - // lane changing start is at the end of prepare segment - const auto target_length_from_lane_change_start_pose = - utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_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 > std::numeric_limits::epsilon()) { - throw std::logic_error("lane change start is behind target lanelet!"); - } - - return true; + return abort_path_ != nullptr; } lane_change::TargetObjects NormalLaneChange::get_target_objects( @@ -1026,8 +990,6 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const filtered_objects.target_lane_trailing.reserve(reserve_size); filtered_objects.others.reserve(reserve_size); - const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - for (const auto & object : objects.objects) { auto ext_object = utils::lane_change::transform(object, *lane_change_parameters_); const auto & ext_obj_pose = ext_object.initial_pose; @@ -1046,12 +1008,8 @@ FilteredLanesObjects NormalLaneChange::filter_objects() const continue; } - // TODO(Azu): We have to think about how to remove is_within_vel_th without breaking AW behavior - const auto is_moving = velocity_filter( - ext_object.initial_twist, stopped_obj_vel_th, std::numeric_limits::max()); - if ( - ahead_of_ego && is_moving && is_before_terminal && + ahead_of_ego && is_before_terminal && !boost::geometry::disjoint(ext_object.initial_polygon, lanes_polygon.current)) { // check only the objects that are in front of the ego vehicle filtered_objects.current_lane.push_back(ext_object); @@ -1086,7 +1044,9 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const 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); + return !utils::path_safety_checker::isTargetObjectOncoming( + current_pose, object_pose, + common_data_ptr_->lc_param_ptr->safety.collision_check.th_incoming_object_yaw); }; // Perception noise could make stationary objects seem opposite the ego vehicle; check the @@ -1108,41 +1068,6 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const }); } -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()); - }); - - 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; -} - std::vector NormalLaneChange::get_prepare_metrics() const { const auto & current_lanes = common_data_ptr_->lanes_ptr->current; @@ -1160,7 +1085,8 @@ std::vector NormalLaneChange::get_prepare_metrics() cons std::vector NormalLaneChange::get_lane_changing_metrics( const PathWithLaneId & prep_segment, const LaneChangePhaseMetrics & prep_metric, - const double shift_length, const double dist_to_reg_element) const + const double shift_length, const double dist_to_reg_element, + lane_change::MetricsDebug & debug_metrics) const { const auto & transient_data = common_data_ptr_->transient_data; const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( @@ -1177,6 +1103,7 @@ std::vector NormalLaneChange::get_lane_changing_metrics( return max_length; }); + debug_metrics.max_lane_changing_length = max_lane_changing_length; const auto max_path_velocity = prep_segment.points.back().point.longitudinal_velocity_mps; return calculation::calc_shift_phase_metrics( common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, @@ -1186,6 +1113,7 @@ std::vector NormalLaneChange::get_lane_changing_metrics( bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const { lane_change_debug_.collision_check_objects.clear(); + lane_change_debug_.lane_change_metrics.clear(); if (!common_data_ptr_->is_lanes_available()) { RCLCPP_WARN(logger_, "lanes are not available. Not expected."); @@ -1198,16 +1126,97 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) } const auto & current_lanes = get_current_lanes(); - const auto & target_lanes = get_target_lanes(); - const auto current_velocity = getEgoVelocity(); - const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); const auto target_objects = get_target_objects(filtered_objects_, current_lanes); const auto prepare_phase_metrics = get_prepare_metrics(); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); + if ( + common_data_ptr_->lc_param_ptr->frenet.enable && + common_data_ptr_->transient_data.is_ego_near_current_terminal_start) { + return get_path_using_frenet( + prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths); + } + + return get_path_using_path_shifter( + prepare_phase_metrics, target_objects, sorted_lane_ids, candidate_paths); +} + +bool NormalLaneChange::get_path_using_frenet( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const +{ + stop_watch_.tic(__func__); + constexpr auto found_safe_path = true; + const auto frenet_candidates = utils::lane_change::generate_frenet_candidates( + common_data_ptr_, prev_module_output_.path, prepare_metrics); + RCLCPP_DEBUG( + logger_, "Generated %lu candidate paths in %2.2f[us]", frenet_candidates.size(), + stop_watch_.toc(__func__)); + + candidate_paths.reserve(frenet_candidates.size()); + lane_change_debug_.frenet_states.clear(); + lane_change_debug_.frenet_states.reserve(frenet_candidates.size()); + for (const auto & frenet_candidate : frenet_candidates) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + break; + } + + lane_change_debug_.frenet_states.emplace_back( + frenet_candidate.prepare_metric, frenet_candidate.lane_changing.sampling_parameter, + frenet_candidate.max_lane_changing_length); + + std::optional candidate_path_opt; + try { + candidate_path_opt = + utils::lane_change::get_candidate_path(frenet_candidate, common_data_ptr_, sorted_lane_ids); + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } + + if (!candidate_path_opt) { + continue; + } + + try { + if (check_candidate_path_safety(*candidate_path_opt, target_objects)) { + RCLCPP_DEBUG( + logger_, "Found safe path after %lu candidate(s). Total time: %2.2f[us]", + frenet_candidates.size(), stop_watch_.toc("__func__")); + utils::lane_change::append_target_ref_to_candidate( + *candidate_path_opt, common_data_ptr_->lc_param_ptr->frenet.th_curvature_smoothing); + candidate_paths.push_back(*candidate_path_opt); + return found_safe_path; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "%s", e.what()); + } + + // appending all paths affect performance + if (candidate_paths.empty()) { + candidate_paths.push_back(*candidate_path_opt); + } + } + + RCLCPP_DEBUG( + logger_, "No safe path after %lu candidate(s). Total time: %2.2f[us]", frenet_candidates.size(), + stop_watch_.toc(__func__)); + return !found_safe_path; +} + +bool NormalLaneChange::get_path_using_path_shifter( + const std::vector & prepare_metrics, + const lane_change::TargetObjects & target_objects, + const std::vector> & sorted_lane_ids, + LaneChangePaths & candidate_paths) const +{ + stop_watch_.tic(__func__); + const auto & target_lanes = get_target_lanes(); candidate_paths.reserve( - prepare_phase_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); + prepare_metrics.size() * lane_change_parameters_->trajectory.lat_acc_sampling_num); const bool only_tl = getStopTime() >= lane_change_parameters_->th_stop_time; const auto dist_to_next_regulatory_element = @@ -1226,7 +1235,7 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) return lc_diff > lane_change_parameters_->trajectory.th_lane_changing_length_diff; }; - for (const auto & prep_metric : prepare_phase_metrics) { + for (const auto & prep_metric : prepare_metrics) { const auto debug_print = [&](const std::string & s) { RCLCPP_DEBUG( logger_, "%s | prep_time: %.5f | lon_acc: %.5f | prep_len: %.5f", s.c_str(), @@ -1240,7 +1249,8 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) PathWithLaneId prepare_segment; try { - if (!get_prepare_segment(prepare_segment, prep_metric.length)) { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, prep_metric.length, prepare_segment)) { debug_print("Reject: failed to get valid prepare segment!"); continue; } @@ -1256,12 +1266,26 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + lane_change_debug_.lane_change_metrics.emplace_back(); + auto & debug_metrics = lane_change_debug_.lane_change_metrics.back(); + debug_metrics.prep_metric = prep_metric; + debug_metrics.max_prepare_length = common_data_ptr_->transient_data.dist_to_terminal_start; const auto lane_changing_metrics = get_lane_changing_metrics( - prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element); + prepare_segment, prep_metric, shift_length, dist_to_next_regulatory_element, debug_metrics); - utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prep_metric.velocity); + // set_prepare_velocity must only be called after computing lane change metrics, as lane change + // metrics rely on the prepare segment's original velocity as max_path_velocity. + utils::lane_change::set_prepare_velocity( + prepare_segment, common_data_ptr_->get_ego_speed(), prep_metric.velocity); for (const auto & lc_metric : lane_changing_metrics) { + if (stop_watch_.toc(__func__) >= lane_change_parameters_->time_limit) { + RCLCPP_DEBUG(logger_, "Time limit reached and no safe path was found."); + return false; + } + + debug_metrics.lc_metrics.emplace_back(lc_metric, -1); + const auto debug_print_lat = [&](const std::string & s) { RCLCPP_DEBUG( logger_, "%s | lc_time: %.5f | lon_acc: %.5f | lat_acc: %.5f | lc_len: %.5f", s.c_str(), @@ -1275,15 +1299,15 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) LaneChangePath candidate_path; try { - candidate_path = get_candidate_path( - prep_metric, lc_metric, prepare_segment, sorted_lane_ids, lane_changing_start_pose, - shift_length); + candidate_path = utils::lane_change::get_candidate_path( + common_data_ptr_, prep_metric, lc_metric, prepare_segment, sorted_lane_ids, shift_length); } catch (const std::exception & e) { debug_print_lat(std::string("Reject: ") + e.what()); continue; } candidate_paths.push_back(candidate_path); + debug_metrics.lc_metrics.back().second = static_cast(candidate_paths.size()) - 1; try { if (check_candidate_path_safety(candidate_path, target_objects)) { @@ -1303,52 +1327,6 @@ bool NormalLaneChange::get_lane_change_paths(LaneChangePaths & candidate_paths) return false; } -LaneChangePath NormalLaneChange::get_candidate_path( - const LaneChangePhaseMetrics & prep_metrics, const LaneChangePhaseMetrics & lc_metrics, - const PathWithLaneId & prep_segment, const std::vector> & sorted_lane_ids, - const Pose & lc_start_pose, const double shift_length) const -{ - const auto & route_handler = *getRouteHandler(); - const auto & target_lanes = common_data_ptr_->lanes_ptr->target; - - const auto resample_interval = - utils::lane_change::calcLaneChangeResampleInterval(lc_metrics.length, prep_metrics.velocity); - const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( - common_data_ptr_, lc_start_pose, lc_metrics.length, resample_interval); - - if (target_lane_reference_path.points.empty()) { - throw std::logic_error("target_lane_reference_path is empty!"); - } - - const auto lc_end_pose = std::invoke([&]() { - const auto dist_to_lc_start = - lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; - const auto dist_to_lc_end = dist_to_lc_start + lc_metrics.length; - return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); - }); - - const auto shift_line = utils::lane_change::get_lane_changing_shift_line( - lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); - - LaneChangeInfo lane_change_info{prep_metrics, lc_metrics, lc_start_pose, lc_end_pose, shift_line}; - - const auto candidate_path = utils::lane_change::construct_candidate_path( - common_data_ptr_, lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); - - if (!candidate_path) { - throw std::logic_error("failed to generate candidate path!"); - } - - if ( - candidate_path.value().info.length.sum() + - common_data_ptr_->transient_data.next_dist_buffer.min > - common_data_ptr_->transient_data.dist_to_terminal_end) { - throw std::logic_error("invalid candidate path length!"); - } - - return *candidate_path; -} - bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { @@ -1378,145 +1356,113 @@ bool NormalLaneChange::check_candidate_path_safety( throw std::logic_error("Path footprint exceeds target lane boundary. Skip lane change."); } + if ((target_objects.empty()) || candidate_path.path.points.empty()) { + RCLCPP_DEBUG(logger_, "There is nothing to check."); + return true; + } + constexpr size_t decel_sampling_num = 1; + const auto ego_predicted_paths = utils::lane_change::convert_to_predicted_paths( + common_data_ptr_, candidate_path, decel_sampling_num); + const auto safety_check_with_normal_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params, - decel_sampling_num, lane_change_debug_.collision_check_objects); + candidate_path, ego_predicted_paths, target_objects, + common_data_ptr_->lc_param_ptr->safety.rss_params, lane_change_debug_.collision_check_objects); if (!safety_check_with_normal_rss.is_safe && is_stuck) { const auto safety_check_with_stuck_rss = isLaneChangePathSafe( - candidate_path, target_objects, common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, - decel_sampling_num, lane_change_debug_.collision_check_objects); + candidate_path, ego_predicted_paths, target_objects, + common_data_ptr_->lc_param_ptr->safety.rss_params_for_stuck, + lane_change_debug_.collision_check_objects); return safety_check_with_stuck_rss.is_safe; } return safety_check_with_normal_rss.is_safe; } -std::optional NormalLaneChange::calcTerminalLaneChangePath( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +std::optional NormalLaneChange::compute_terminal_lane_change_path() const { - const auto is_empty = [&](const auto & data, const auto & s) { - if (!data.empty()) return false; - RCLCPP_WARN(logger_, "%s is empty. Not expected.", s); - return true; - }; - - const auto target_lane_neighbors_polygon_2d = - common_data_ptr_->lanes_polygon_ptr->target_neighbor; - if ( - is_empty(current_lanes, "current_lanes") || is_empty(target_lanes, "target_lanes") || - is_empty(target_lane_neighbors_polygon_2d, "target_lane_neighbors_polygon_2d")) { - return {}; - } - - const auto & route_handler = *getRouteHandler(); - - const auto minimum_lane_changing_velocity = - lane_change_parameters_->trajectory.min_lane_changing_velocity; - - const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - - const auto current_min_dist_buffer = common_data_ptr_->transient_data.current_dist_buffer.min; - const auto next_min_dist_buffer = common_data_ptr_->transient_data.next_dist_buffer.min; - - const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); - - // 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 = autoware::motion_utils::calcLongitudinalOffsetPose( - prev_module_output_.path.points, current_lane_terminal_point, - -(current_min_dist_buffer + next_min_dist_buffer + distance_to_terminal_from_goal)); + const auto & transient_data = common_data_ptr_->transient_data; + const auto dist_to_terminal_start = transient_data.dist_to_terminal_start; + const auto min_lc_velocity = lane_change_parameters_->trajectory.min_lane_changing_velocity; + const auto current_velocity = getEgoVelocity(); - if (!lane_changing_start_pose) { - RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); - return {}; + PathWithLaneId prepare_segment; + try { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr_, prev_module_output_.path, dist_to_terminal_start, prepare_segment)) { + RCLCPP_DEBUG(logger_, "failed to get valid prepare segment for terminal LC path"); + return std::nullopt; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG(logger_, "failed to get terminal LC path: %s", e.what()); + return std::nullopt; } - 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 {}; + // t = 2 * d / (v1 + v2) + const auto duration_to_lc_start = + 2.0 * dist_to_terminal_start / (current_velocity + min_lc_velocity); + const auto lon_accel = std::invoke([&]() -> double { + if (duration_to_lc_start < calculation::eps) { + return 0.0; + } + return std::clamp( + (min_lc_velocity - current_velocity) / duration_to_lc_start, + lane_change_parameters_->trajectory.min_longitudinal_acc, + lane_change_parameters_->trajectory.max_longitudinal_acc); + }); + const auto vel_on_prep = current_velocity + lon_accel * duration_to_lc_start; + const LaneChangePhaseMetrics prep_metric( + duration_to_lc_start, dist_to_terminal_start, vel_on_prep, lon_accel, lon_accel, 0.0); + + if (terminal_lane_change_path_) { + terminal_lane_change_path_->info.set_prepare(prep_metric); + if (prepare_segment.points.empty()) { + terminal_lane_change_path_->path = terminal_lane_change_path_->shifted_path.path; + return terminal_lane_change_path_->path; + } + prepare_segment.points.pop_back(); + terminal_lane_change_path_->path = + utils::combinePath(prepare_segment, terminal_lane_change_path_->shifted_path.path); + return terminal_lane_change_path_->path; } - 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_->trajectory.lat_acc_map.find(minimum_lane_changing_velocity); - - const auto lane_changing_time = autoware::motion_utils::calc_shift_time_from_jerk( - shift_length, lane_change_parameters_->trajectory.lateral_jerk, max_lateral_acc); - - const auto target_lane_length = common_data_ptr_->transient_data.target_lane_length; - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose.value(), target_lane_length, current_min_dist_buffer, - minimum_lane_changing_velocity, next_min_dist_buffer); + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto & target_lanes = common_data_ptr_->lanes_ptr->target; + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); - return {}; - } + const auto dist_lc_start_to_end_of_lanes = calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr_, common_data_ptr_->lanes_ptr->target_neighbor, + prepare_segment.points.back().point.pose); - 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, current_min_dist_buffer}; - 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; + const auto max_lane_changing_length = std::invoke([&]() { + double max_length = transient_data.dist_to_terminal_end - prep_metric.length; + return std::min( + max_length, dist_lc_start_to_end_of_lanes - transient_data.next_dist_buffer.min); + }); - if (!is_valid_start_point(common_data_ptr_, lane_changing_start_pose.value())) { - RCLCPP_DEBUG( - logger_, - "Reject: lane changing points are not inside of the target preferred lanes or its " - "neighbors"); - return {}; - } + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + constexpr double lane_changing_lon_accel{0.0}; + const auto lane_changing_metrics = calculation::calc_shift_phase_metrics( + common_data_ptr_, shift_length, prep_metric.velocity, max_path_velocity, + lane_changing_lon_accel, max_lane_changing_length); - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - current_min_dist_buffer, minimum_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::get_reference_path_from_target_Lane( - common_data_ptr_, lane_changing_start_pose.value(), current_min_dist_buffer, resample_interval); + const auto sorted_lane_ids = utils::lane_change::get_sorted_lane_ids(common_data_ptr_); - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); - return {}; + LaneChangePath candidate_path; + for (const auto & lc_metric : lane_changing_metrics) { + try { + candidate_path = utils::lane_change::get_candidate_path( + common_data_ptr_, prep_metric, lc_metric, prepare_segment, sorted_lane_ids, shift_length); + } catch (const std::exception & e) { + continue; + } + terminal_lane_change_path_ = candidate_path; + return candidate_path.path; } - lane_change_info.shift_line = utils::lane_change::get_lane_changing_shift_line( - 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 = autoware::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 = - static_cast(minimum_lane_changing_velocity); - - const auto terminal_lane_change_path = utils::lane_change::construct_candidate_path( - common_data_ptr_, lane_change_info, reference_segment, target_lane_reference_path, - sorted_lane_ids); - - return terminal_lane_change_path; + return std::nullopt; } PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const @@ -1546,9 +1492,13 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return {false, false}; } + const auto decel_sampling_num = + static_cast(lane_change_parameters_->cancel.deceleration_sampling_num); + const auto ego_predicted_paths = + utils::lane_change::convert_to_predicted_paths(common_data_ptr_, path, decel_sampling_num); const auto safety_status = isLaneChangePathSafe( - path, target_objects, lane_change_parameters_->safety.rss_params_for_abort, - static_cast(lane_change_parameters_->cancel.deceleration_sampling_num), debug_data); + path, ego_predicted_paths, target_objects, lane_change_parameters_->safety.rss_params_for_abort, + debug_data); { // only for debug purpose lane_change_debug_.collision_check_objects.clear(); @@ -1627,14 +1577,8 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_trailing_object) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - if ( - common_data_ptr_->transient_data.is_ego_near_current_terminal_start && isAbleToStopSafely() && - is_trailing_object) { - current_lane_change_state_ = LaneChangeStates::Stop; - return true; - } - current_lane_change_state_ = LaneChangeStates::Normal; - return false; + return common_data_ptr_->transient_data.is_ego_near_current_terminal_start && + isAbleToStopSafely() && is_trailing_object; } bool NormalLaneChange::calcAbortPath() @@ -1780,103 +1724,57 @@ bool NormalLaneChange::calcAbortPath() PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const LaneChangePath & lane_change_path, + const std::vector> & ego_predicted_paths, const lane_change::TargetObjects & collision_check_objects, - const utils::path_safety_checker::RSSparams & rss_params, const size_t deceleration_sampling_num, + const utils::path_safety_checker::RSSparams & rss_params, CollisionCheckDebugMap & debug_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); constexpr auto is_safe = true; constexpr auto is_object_behind_ego = true; - if (collision_check_objects.leading.empty() && collision_check_objects.trailing.empty()) { - RCLCPP_DEBUG(logger_, "There is nothing to check."); - return {is_safe, !is_object_behind_ego}; - } - const auto is_check_prepare_phase = check_prepare_phase(); - const auto all_decel_pattern_has_collision = - [&](const utils::path_safety_checker::ExtendedPredictedObjects & objects) -> bool { - return has_collision_with_decel_patterns( - lane_change_path, objects, deceleration_sampling_num, rss_params, is_check_prepare_phase, - debug_data); + const auto all_paths_collide = [&](const auto & objects) { + const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; + return ranges::all_of(ego_predicted_paths, [&](const auto & ego_predicted_path) { + const auto check_for_collision = [&](const auto & object) { + const auto selected_rss_param = (object.initial_twist.linear.x <= stopped_obj_vel_th) + ? lane_change_parameters_->safety.rss_params_for_parked + : rss_params; + return is_colliding( + lane_change_path, object, ego_predicted_path, selected_rss_param, is_check_prepare_phase, + debug_data); + }; + return ranges::any_of(objects, check_for_collision); + }); }; - if (all_decel_pattern_has_collision(collision_check_objects.trailing)) { + if (all_paths_collide(collision_check_objects.trailing)) { return {!is_safe, is_object_behind_ego}; } - if (all_decel_pattern_has_collision(collision_check_objects.leading)) { + if (all_paths_collide(collision_check_objects.leading)) { return {!is_safe, !is_object_behind_ego}; } return {is_safe, !is_object_behind_ego}; } -bool NormalLaneChange::has_collision_with_decel_patterns( - const LaneChangePath & lane_change_path, const ExtendedPredictedObjects & objects, - const size_t deceleration_sampling_num, const RSSparams & rss_param, - const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const -{ - if (objects.empty()) { - return false; - } - - const auto & path = lane_change_path.path; - - if (path.points.empty()) { - return false; - } - - const auto bpp_param = *common_data_ptr_->bpp_param_ptr; - const auto global_min_acc = bpp_param.min_acc; - const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; - - const auto min_acc = std::min(lane_changing_acc, global_min_acc); - const auto sampling_num = - std::abs(min_acc - lane_changing_acc) > floating_err_th ? deceleration_sampling_num : 1; - const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); - - std::vector acceleration_values(sampling_num); - std::iota(acceleration_values.begin(), acceleration_values.end(), 0); - - std::transform( - acceleration_values.begin(), acceleration_values.end(), acceleration_values.begin(), - [&](double n) { return lane_changing_acc + n * acc_resolution; }); - - const auto stopped_obj_vel_th = lane_change_parameters_->safety.th_stopped_object_velocity; - const auto all_collided = std::all_of( - acceleration_values.begin(), acceleration_values.end(), [&](const auto acceleration) { - const auto ego_predicted_path = utils::lane_change::convert_to_predicted_path( - common_data_ptr_, lane_change_path, acceleration); - - return std::any_of(objects.begin(), objects.end(), [&](const auto & obj) { - const auto selected_rss_param = (obj.initial_twist.linear.x <= stopped_obj_vel_th) - ? lane_change_parameters_->safety.rss_params_for_parked - : rss_param; - return is_collided( - lane_change_path, obj, ego_predicted_path, selected_rss_param, check_prepare_phase, - debug_data); - }); - }); - - return all_collided; -} - -bool NormalLaneChange::is_collided( +bool NormalLaneChange::is_colliding( const LaneChangePath & lane_change_path, const ExtendedPredictedObject & obj, const std::vector & ego_predicted_path, const RSSparams & selected_rss_param, const bool check_prepare_phase, CollisionCheckDebugMap & debug_data) const { - constexpr auto is_collided{true}; + constexpr auto is_colliding{true}; if (lane_change_path.path.points.empty()) { - return !is_collided; + return !is_colliding; } if (ego_predicted_path.empty()) { - return !is_collided; + return !is_colliding; } const auto & lanes_polygon_ptr = common_data_ptr_->lanes_polygon_ptr; @@ -1884,12 +1782,11 @@ bool NormalLaneChange::is_collided( const auto & expanded_target_polygon = lanes_polygon_ptr->target; if (current_polygon.empty() || expanded_target_polygon.empty()) { - return !is_collided; + return !is_colliding; } constexpr auto is_safe{true}; auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); - constexpr auto collision_check_yaw_diff_threshold{M_PI}; constexpr auto hysteresis_factor{1.0}; const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->safety.collision_check.use_all_predicted_paths); @@ -1914,7 +1811,8 @@ bool NormalLaneChange::is_collided( const auto collided_polygons = utils::path_safety_checker::get_collided_polygons( lane_change_path.path, ego_predicted_path, obj, predicted_obj_path, bpp_param.vehicle_info, selected_rss_param, hysteresis_factor, safety_check_max_vel, - collision_check_yaw_diff_threshold, current_debug_data.second); + common_data_ptr_->lc_param_ptr->safety.collision_check.th_yaw_diff, + current_debug_data.second); if (collided_polygons.empty()) { utils::path_safety_checker::updateCollisionCheckDebugMap( @@ -1935,10 +1833,10 @@ bool NormalLaneChange::is_collided( utils::path_safety_checker::updateCollisionCheckDebugMap( debug_data, current_debug_data, !is_safe); - return is_collided; + return is_colliding; } utils::path_safety_checker::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); - return !is_collided; + return !is_colliding; } double NormalLaneChange::get_max_velocity_for_safety_check() const @@ -2005,22 +1903,11 @@ bool NormalLaneChange::is_ego_stuck() const return has_object_blocking; } -bool NormalLaneChange::is_valid_start_point( - const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) const -{ - const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); - - const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; - const auto & target_lane_poly = common_data_ptr_->lanes_polygon_ptr->target; - - return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || - boost::geometry::covered_by(lc_start_point, target_lane_poly); -} - -void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) +void NormalLaneChange::set_stop_pose( + const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason) { const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); - lane_change_stop_pose_ = stop_point.point.pose; + lane_change_stop_pose_ = PoseWithDetailOpt(PoseWithDetail(stop_point.point.pose, reason)); } void NormalLaneChange::updateStopTime() diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index 3ba11d62a2be7..a0130fcd27041 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -58,22 +58,10 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) } double calc_dist_to_last_fit_width( - const lanelet::ConstLanelets & lanelets, const Pose & src_pose, + const lanelet::ConstLanelets & lanelets, const lanelet::BasicPolygon2d & lanelet_polygon, + const universe_utils::LineString2d & line_string, const Pose & src_pose, const BehaviorPathPlannerParameters & bpp_param, const double margin) { - if (lanelets.empty()) return 0.0; - - const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); - const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); - - if (center_line.size() <= 1) return 0.0; - - universe_utils::LineString2d line_string; - line_string.reserve(center_line.size() - 1); - std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { - boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); - }); - const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; universe_utils::MultiPolygon2d center_line_polygon; namespace strategy = boost::geometry::strategy::buffer; @@ -85,7 +73,7 @@ double calc_dist_to_last_fit_width( if (center_line_polygon.empty()) return 0.0; std::vector intersection_points; - boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + boost::geometry::intersection(lanelet_polygon, center_line_polygon, intersection_points); if (intersection_points.empty()) { return utils::getDistanceToEndOfLane(src_pose, lanelets); @@ -102,6 +90,46 @@ double calc_dist_to_last_fit_width( return std::max(distance - (bpp_param.base_link2front + margin), 0.0); } +double calc_dist_to_last_fit_width( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const double margin) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & current_lanes_polygon = common_data_ptr->lanes_polygon_ptr->current; + const auto & bpp_param = *common_data_ptr->bpp_param_ptr; + + universe_utils::LineString2d line_string; + line_string.reserve(path.points.size() - 1); + std::for_each(path.points.begin() + 1, path.points.end(), [&line_string](const auto & point) { + const auto & position = point.point.pose.position; + boost::geometry::append(line_string, universe_utils::Point2d(position.x, position.y)); + }); + + const auto & src_pose = path.points.front().point.pose; + return calc_dist_to_last_fit_width( + current_lanes, current_lanes_polygon, line_string, src_pose, bpp_param, margin); +} + +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets & lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + return calc_dist_to_last_fit_width( + lanelets, lane_polygon, line_string, src_pose, bpp_param, margin); +} + double calc_maximum_prepare_length(const CommonDataPtr & common_data_ptr) { const auto max_prepare_duration = common_data_ptr->lc_param_ptr->trajectory.max_prepare_duration; @@ -417,7 +445,7 @@ double calc_actual_prepare_duration( if (max_acc < eps) { return params.max_prepare_duration; } - return (min_lc_velocity - current_velocity) / max_acc; + return std::max((min_lc_velocity - current_velocity) / max_acc, params.min_prepare_duration); }); return std::max(params.max_prepare_duration - active_signal_duration, min_prepare_duration); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp index ff0bd4437c21c..bf0af0d133dc7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/markers.cpp @@ -14,11 +14,12 @@ #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 +#include #include #include @@ -26,10 +27,11 @@ #include #include +#include + #include #include #include -#include #include #include @@ -39,34 +41,66 @@ using autoware::universe_utils::createDefaultMarker; using autoware::universe_utils::createMarkerScale; using geometry_msgs::msg::Point; -MarkerArray showAllValidLaneChangePath(const std::vector & lanes, std::string && ns) +MarkerArray showAllValidLaneChangePath( + const std::vector & lane_change_paths, std::string && ns) { - if (lanes.empty()) { + if (lane_change_paths.empty()) { return MarkerArray{}; } MarkerArray marker_array; - int32_t id{0}; const auto current_time{rclcpp::Clock{RCL_ROS_TIME}.now()}; const auto colors = colors::colors_list(); - const auto loop_size = std::min(lanes.size(), colors.size()); + const auto loop_size = std::min(lane_change_paths.size(), colors.size()); marker_array.markers.reserve(loop_size); for (std::size_t idx = 0; idx < loop_size; ++idx) { - if (lanes.at(idx).path.points.empty()) { + int32_t id{0}; + const auto & lc_path = lane_change_paths.at(idx); + if (lc_path.path.points.empty()) { continue; } - + std::string ns_with_idx = ns + "[" + std::to_string(idx) + "]"; const auto & color = colors.at(idx); - Marker marker = createDefaultMarker( - "map", current_time, ns, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), color); - marker.points.reserve(lanes.at(idx).path.points.size()); + const auto & points = lc_path.path.points; + auto marker = createDefaultMarker( + "map", current_time, ns_with_idx, ++id, Marker::LINE_STRIP, createMarkerScale(0.1, 0.1, 0.0), + color); + marker.points.reserve(points.size()); - for (const auto & point : lanes.at(idx).path.points) { + for (const auto & point : points) { marker.points.push_back(point.point.pose.position); } + const auto & info = lc_path.info; + auto text_marker = createDefaultMarker( + "map", current_time, ns_with_idx, ++id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.1, 0.1, 0.8), colors::yellow()); + const auto prep_idx = points.size() / 4; + text_marker.pose = points.at(prep_idx).point.pose; + text_marker.pose.position.z += 2.0; + text_marker.text = fmt::format( + "vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | t: {time:.3f}[s] | L: {length:.3f}[m]", + fmt::arg("vel", info.velocity.prepare), + fmt::arg("lon_acc", info.longitudinal_acceleration.prepare), + fmt::arg("time", info.duration.prepare), fmt::arg("length", info.length.prepare)); + marker_array.markers.push_back(text_marker); + + const auto lc_idx = points.size() / 2; + text_marker.id = ++id; + text_marker.pose = points.at(lc_idx).point.pose; + text_marker.text = fmt::format( + "type: {type} | vel: {vel:.3f}[m/s] | lon_acc: {lon_acc:.3f}[m/s2] | lat_acc: " + "{lat_acc:.3f}[m/s2] | t: " + "{time:.3f}[s] | L: {length:.3f}[m]", + fmt::arg("type", magic_enum::enum_name(lc_path.type)), + fmt::arg("vel", info.velocity.lane_changing), + fmt::arg("lon_acc", info.longitudinal_acceleration.lane_changing), + fmt::arg("lat_acc", info.lateral_acceleration), fmt::arg("time", info.duration.lane_changing), + fmt::arg("length", info.length.lane_changing)); + marker_array.markers.push_back(text_marker); + marker_array.markers.push_back(marker); } return marker_array; @@ -133,7 +167,9 @@ MarkerArray showFilteredObjects( return marker_array; } -MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) +MarkerArray showExecutionInfo( + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose) { auto default_text_marker = [&]() { return createDefaultMarker( @@ -146,24 +182,101 @@ MarkerArray showExecutionInfo(const Debug & debug_data, const geometry_msgs::msg auto safety_check_info_text = default_text_marker(); safety_check_info_text.pose = ego_pose; safety_check_info_text.pose.position.z += 4.0; + const auto lc_state = interface_debug_data.lc_state; + const auto & failing_reason = interface_debug_data.failing_reason; + + safety_check_info_text.text = fmt::format( + "{stuck} | {return_lane} | {state} : {reason}", + fmt::arg("stuck", scene_debug_data.is_stuck ? "is stuck" : ""), + fmt::arg( + "return_lane", scene_debug_data.is_able_to_return_to_current_lane ? "" : "can't return"), + fmt::arg("state", magic_enum::enum_name(lc_state)), fmt::arg("reason", failing_reason)); + marker_array.markers.push_back(safety_check_info_text); + return marker_array; +} - std::ostringstream ss; +MarkerArray ShowLaneChangeMetricsInfo( + const Debug & debug_data, const geometry_msgs::msg::Pose & pose) +{ + MarkerArray marker_array; - ss << "\nDistToEndOfCurrentLane: " << std::setprecision(5) - << debug_data.distance_to_end_of_current_lane - << "\nDistToLaneChangeFinished: " << debug_data.distance_to_lane_change_finished - << (debug_data.is_stuck ? "\nVehicleStuck" : "") - << (debug_data.is_able_to_return_to_current_lane ? "\nAbleToReturnToCurrentLane" : "") - << (debug_data.is_abort ? "\nAborting" : "") - << "\nDistanceToAbortFinished: " << debug_data.distance_to_abort_finished; + auto text_marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "sampling_metrics", 0, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.6, 0.6, 0.6), colors::yellow()); + text_marker.pose = autoware::universe_utils::calcOffsetPose(pose, 10.0, 15.0, 0.0); + + if (!debug_data.lane_change_metrics.empty()) { + text_marker.text = + fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lat_accel[m/s2]") + + fmt::format("{:^18}|", "lon_accel[m/s2]") + fmt::format("{:^17}|", "velocity[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^20}|", "max_length_th[m]") + fmt::format("{:^15}\n", "path_index"); + for (const auto & metrics : debug_data.lane_change_metrics) { + text_marker.text += fmt::format("{:-<190}\n", ""); + const auto & p_m = metrics.prep_metric; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^10.3f}", p_m.lat_accel) + + fmt::format("{:^21.3f}", p_m.actual_lon_accel) + fmt::format("{:^12.3f}", p_m.velocity) + + fmt::format("{:^15.3f}", p_m.duration) + fmt::format("{:^15.3f}", p_m.length) + + fmt::format("{:^17.3f}", metrics.max_prepare_length) + fmt::format("{:^15}\n", "-"); + text_marker.text += fmt::format("{:<20}\n", "lc_metrics:"); + for (const auto & lc_m : metrics.lc_metrics) { + const auto & metric = lc_m.first; + const auto path_index = lc_m.second < 0 ? "-" : std::to_string(lc_m.second); + text_marker.text += fmt::format("{:<15}", "") + fmt::format("{:^10.3f}", metric.lat_accel) + + fmt::format("{:^21.3f}", metric.actual_lon_accel) + + fmt::format("{:^12.3f}", metric.velocity) + + fmt::format("{:^15.3f}", metric.duration) + + fmt::format("{:^15.3f}", metric.length) + + fmt::format("{:^17.3f}", metrics.max_lane_changing_length) + + fmt::format("{:^15}\n", path_index); + } + } + marker_array.markers.push_back(text_marker); + } + + if (!debug_data.frenet_states.empty()) { + text_marker.text = fmt::format("{:<12}", "") + fmt::format("{:^18}|", "lon_accel[m/s2]") + + fmt::format("{:^17}|", "lon_vel[m/s]") + + fmt::format("{:^15}|", "duration[s]") + fmt::format("{:^15}|", "length[m]") + + fmt::format("{:^17}|", "lat_accel[m/s2]") + + fmt::format("{:^15}|", "lat_vel[m/s2]") + fmt::format("{:^15}|", "s[m]") + + fmt::format("{:^15}|", "d[m]") + fmt::format("{:^20}\n", "max_length_th[m]"); + for (const auto & metrics : debug_data.frenet_states) { + text_marker.text += fmt::format("{:-<250}\n", ""); + const auto & p_m = metrics.prep_metric; + const auto max_len = metrics.max_lane_changing_length; + text_marker.text += + fmt::format("{:<17}", "prep_metrics:") + fmt::format("{:^13.3f}", p_m.actual_lon_accel) + + fmt::format("{:^15.3f}", p_m.velocity) + fmt::format("{:^15.3f}", p_m.duration) + + fmt::format("{:^12.3f}", p_m.length) + + fmt::format("{:^13}", "") + // Empty string for lat_accel + fmt::format("{:^13}", "") + // Empty string for lat_vel + fmt::format("{:^15}", "") + // Empty string for s + fmt::format("{:^15}", "") + // Empty string for d // Empty string for d + fmt::format("{:^20.3f}\n", max_len); // Empty string for max_length_t + const auto & lc_m = metrics.sampling_parameter.target_state; // Assuming lc_metric exists + const auto duration = metrics.sampling_parameter.target_duration; + text_marker.text += + fmt::format("{:<17}", "frenet_state:") + + fmt::format("{:^15.3f}", lc_m.longitudinal_acceleration) + + fmt::format("{:^13.3f}", lc_m.longitudinal_velocity) + fmt::format("{:^17.3f}", duration) + + fmt::format("{:^10.3f}", lc_m.position.s) + + fmt::format("{:^19.3f}", lc_m.lateral_acceleration) + + fmt::format("{:^10.3f}", lc_m.lateral_velocity) + + fmt::format("{:^18.3f}", lc_m.position.s) + fmt::format("{:^15.3f}", lc_m.position.d) + + fmt::format("{:^16.3f}\n", max_len); // Empty string for max_length_t + } + + marker_array.markers.push_back(text_marker); + } - safety_check_info_text.text = ss.str(); - marker_array.markers.push_back(safety_check_info_text); return marker_array; } MarkerArray createDebugMarkerArray( - const Debug & debug_data, const geometry_msgs::msg::Pose & ego_pose) + const InterfaceDebug & interface_debug_data, const Debug & scene_debug_data, + const geometry_msgs::msg::Pose & ego_pose) { using lanelet::visualization::laneletsAsTriangleMarkerArray; using marker_utils::showPolygon; @@ -172,30 +285,32 @@ MarkerArray createDebugMarkerArray( using marker_utils::lane_change_markers::showAllValidLaneChangePath; using marker_utils::lane_change_markers::showFilteredObjects; - const auto & debug_collision_check_object = debug_data.collision_check_objects; + const auto & debug_collision_check_object = scene_debug_data.collision_check_objects; const auto & debug_collision_check_object_after_approval = - debug_data.collision_check_objects_after_approval; - const auto & debug_valid_paths = debug_data.valid_paths; - const auto & debug_filtered_objects = debug_data.filtered_objects; + scene_debug_data.collision_check_objects_after_approval; + const auto & debug_valid_paths = scene_debug_data.valid_paths; + const auto & debug_filtered_objects = scene_debug_data.filtered_objects; MarkerArray debug_marker; const auto add = [&debug_marker](const MarkerArray & added) { autoware::universe_utils::appendMarkerArray(added, &debug_marker); }; - if (!debug_data.execution_area.points.empty()) { + if (!scene_debug_data.execution_area.points.empty()) { add(createPolygonMarkerArray( - debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); + scene_debug_data.execution_area, "execution_area", 0, 0.16, 1.0, 0.69, 0.1)); } - add(showExecutionInfo(debug_data, ego_pose)); + add(showExecutionInfo(interface_debug_data, scene_debug_data, ego_pose)); + add(ShowLaneChangeMetricsInfo(scene_debug_data, ego_pose)); // lanes add(laneletsAsTriangleMarkerArray( - "current_lanes", debug_data.current_lanes, colors::light_yellow(0.2))); - add(laneletsAsTriangleMarkerArray("target_lanes", debug_data.target_lanes, colors::aqua(0.2))); + "current_lanes", scene_debug_data.current_lanes, colors::light_yellow(0.2))); + add(laneletsAsTriangleMarkerArray( + "target_lanes", scene_debug_data.target_lanes, colors::aqua(0.2))); add(laneletsAsTriangleMarkerArray( - "target_backward_lanes", debug_data.target_backward_lanes, colors::blue(0.2))); + "target_backward_lanes", scene_debug_data.target_backward_lanes, colors::blue(0.2))); add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths")); add(showFilteredObjects(debug_filtered_objects, "object_filtered")); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp new file mode 100644 index 0000000000000..df45da3f08fa1 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/path.cpp @@ -0,0 +1,755 @@ +// 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_lane_change_module/utils/path.hpp" + +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/utils/calculation.hpp" +#include "autoware/behavior_path_lane_change_module/utils/utils.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 + +namespace +{ +using autoware::behavior_path_planner::LaneChangeInfo; +using autoware::behavior_path_planner::PathPointWithLaneId; +using autoware::behavior_path_planner::PathShifter; +using autoware::behavior_path_planner::PathWithLaneId; +using autoware::behavior_path_planner::ShiftedPath; +using autoware::behavior_path_planner::lane_change::CommonDataPtr; +using autoware::behavior_path_planner::lane_change::LCParamPtr; + +using autoware::behavior_path_planner::LaneChangePhaseMetrics; +using autoware::behavior_path_planner::ShiftLine; +using autoware::behavior_path_planner::lane_change::TrajectoryGroup; +using autoware::frenet_planner::FrenetState; +using autoware::frenet_planner::SamplingParameters; +using autoware::route_handler::Direction; +using autoware::sampler_common::FrenetPoint; +using autoware::sampler_common::transform::Spline2D; +using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; +using geometry_msgs::msg::Pose; + +template +T sign(const Direction direction) +{ + return static_cast(direction == Direction::LEFT ? 1.0 : -1.0); +} + +double calc_resample_interval( + 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); +} + +PathWithLaneId get_reference_path_from_target_lane( + const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, + const double lane_changing_length, const double resample_interval) +{ + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; + const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; + const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; + const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; + + const auto 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_lc_buffer; + return std::min(dist_from_lc_start, s_goal); + } + return std::min(dist_from_lc_start, target_lane_length - next_lc_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 autoware::behavior_path_planner::utils::resamplePathWithSpline( + lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_length}); +} + +ShiftLine get_lane_changing_shift_line( + 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 = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_start_pose.position); + shift_line.end_idx = autoware::motion_utils::findNearestIndex( + reference_path.points, lane_changing_end_pose.position); + + return shift_line; +} + +ShiftedPath get_shifted_path( + const PathWithLaneId & target_lane_reference_path, const LaneChangeInfo & lane_change_info) +{ + const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; + + PathShifter path_shifter; + path_shifter.setPath(target_lane_reference_path); + path_shifter.addShiftLine(lane_change_info.shift_line); + path_shifter.setLongitudinalAcceleration(longitudinal_acceleration.lane_changing); + const auto initial_lane_changing_velocity = lane_change_info.velocity.lane_changing; + path_shifter.setVelocity(initial_lane_changing_velocity); + path_shifter.setLateralAccelerationLimit(std::abs(lane_change_info.lateral_acceleration)); + + constexpr auto offset_back = false; + ShiftedPath shifted_path; + [[maybe_unused]] const auto success = path_shifter.generate(&shifted_path, offset_back); + return shifted_path; +} + +std::optional exceed_yaw_threshold( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, + const double yaw_th_rad) +{ + const auto & prepare = prepare_segment.points; + const auto & lane_changing = lane_changing_segment.points; + + if (prepare.size() <= 1 || lane_changing.size() <= 1) { + return std::nullopt; + } + + const auto & p1 = std::prev(prepare.end() - 1)->point.pose; + const auto & p2 = std::next(lane_changing.begin())->point.pose; + const auto yaw_diff_rad = std::abs(autoware::universe_utils::normalizeRadian( + tf2::getYaw(p1.orientation) - tf2::getYaw(p2.orientation))); + if (yaw_diff_rad > yaw_th_rad) { + return yaw_diff_rad; + } + return std::nullopt; +} + +Spline2D init_reference_spline(const std::vector & target_lanes_ref_path) +{ + std::vector xs; + std::vector ys; + xs.reserve(target_lanes_ref_path.size()); + ys.reserve(target_lanes_ref_path.size()); + for (const auto & p : target_lanes_ref_path) { + xs.push_back(p.point.pose.position.x); + ys.push_back(p.point.pose.position.y); + } + + return {xs, ys}; +} + +FrenetState init_frenet_state( + const FrenetPoint & start_position, const LaneChangePhaseMetrics & prepare_metrics) +{ + FrenetState initial_state; + initial_state.position = start_position; + initial_state.longitudinal_velocity = prepare_metrics.velocity; + initial_state.lateral_velocity = + 0.0; // TODO(Maxime): this can be sampled if we want but it would impact the LC duration + initial_state.longitudinal_acceleration = prepare_metrics.sampled_lon_accel; + initial_state.lateral_acceleration = prepare_metrics.lat_accel; + return initial_state; +} + +std::optional init_sampling_parameters( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prepare_metrics, + const FrenetState & initial_state, const Spline2D & ref_spline, const Pose & lc_start_pose) +{ + const auto & trajectory = common_data_ptr->lc_param_ptr->trajectory; + const auto min_lc_vel = trajectory.min_lane_changing_velocity; + const auto [min_lateral_acc, max_lateral_acc] = + trajectory.lat_acc_map.find(std::max(min_lc_vel, prepare_metrics.velocity)); + const auto duration = autoware::motion_utils::calc_shift_time_from_jerk( + std::abs(initial_state.position.d), trajectory.lateral_jerk, max_lateral_acc); + const auto final_velocity = + std::max(min_lc_vel, prepare_metrics.velocity + prepare_metrics.sampled_lon_accel * duration); + const auto lc_length = duration * (prepare_metrics.velocity + final_velocity) * 0.5; + const auto target_s = initial_state.position.s + lc_length; + const auto initial_yaw = tf2::getYaw(lc_start_pose.orientation); + const auto target_lat_vel = + (1.0 - ref_spline.curvature(target_s + 1e-3) * initial_state.position.d) * + std::tan(initial_yaw - ref_spline.yaw(target_s)); + + if (std::isnan(target_lat_vel)) { + return std::nullopt; + } + + SamplingParameters sampling_parameters; + const auto & safety = common_data_ptr->lc_param_ptr->safety; + sampling_parameters.resolution = safety.collision_check.prediction_time_resolution; + sampling_parameters.parameters.emplace_back(); + sampling_parameters.parameters.back().target_duration = duration; + sampling_parameters.parameters.back().target_state.position = {target_s, 0.0}; + // TODO(Maxime): not sure if we should use curvature at initial or target s + sampling_parameters.parameters.back().target_state.lateral_velocity = + sign(common_data_ptr->direction) * target_lat_vel; + sampling_parameters.parameters.back().target_state.lateral_acceleration = 0.0; + sampling_parameters.parameters.back().target_state.longitudinal_velocity = final_velocity; + sampling_parameters.parameters.back().target_state.longitudinal_acceleration = + prepare_metrics.sampled_lon_accel; + return sampling_parameters; +} + +std::vector calc_curvatures( + const std::vector & points, const Pose & current_pose) +{ + const auto nearest_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(points, current_pose.position); + + // Ignore all path points behind ego vehicle. + if (points.size() <= nearest_segment_idx + 2) { + return {}; + } + + std::vector curvatures; + curvatures.reserve(points.size() - nearest_segment_idx + 2); + for (const auto & [p1, p2, p3] : ranges::views::zip( + points | ranges::views::drop(nearest_segment_idx), + points | ranges::views::drop(nearest_segment_idx + 1), + points | ranges::views::drop(nearest_segment_idx + 2))) { + const auto point1 = autoware::universe_utils::getPoint(p1); + const auto point2 = autoware::universe_utils::getPoint(p2); + const auto point3 = autoware::universe_utils::getPoint(p3); + + curvatures.push_back(autoware::universe_utils::calcCurvature(point1, point2, point3)); + } + + return curvatures; +} + +double calc_average_curvature(const std::vector & curvatures) +{ + const auto filter_zeros = [](const auto & k) { return k != 0.0; }; + auto filtered_k = curvatures | ranges::views::filter(filter_zeros); + if (filtered_k.empty()) { + return 0.0; + } + const auto sums_of_curvatures = [](float sum, const double k) { return sum + std::abs(k); }; + const auto sum_of_k = ranges::accumulate(filtered_k, 0.0, sums_of_curvatures); + const auto count_k = static_cast(ranges::distance(filtered_k)); + return sum_of_k / count_k; +} + +LineString2d get_linestring_bound(const lanelet::ConstLanelets & lanes, const Direction direction) +{ + LineString2d line_string; + const auto & get_bound = [&](const lanelet::ConstLanelet & lane) { + return (direction == Direction::LEFT) ? lane.leftBound2d() : lane.rightBound2d(); + }; + + const auto reserve_size = ranges::accumulate( + lanes, 0UL, + [&](auto sum, const lanelet::ConstLanelet & lane) { return sum + get_bound(lane).size(); }); + line_string.reserve(reserve_size); + for (const auto & lane : lanes) { + ranges::for_each(get_bound(lane), [&line_string](const auto & point) { + boost::geometry::append(line_string, Point2d(point.x(), point.y())); + }); + } + return line_string; +} + +Point2d shift_point(const Point2d & p1, const Point2d & p2, const double offset) +{ + // Calculate the perpendicular vector + double dx = p2.x() - p1.x(); + double dy = p2.y() - p1.y(); + double length = std::sqrt(dx * dx + dy * dy); + + // Normalize and find the perpendicular direction + double nx = -dy / length; + double ny = dx / length; + + return {p1.x() + nx * offset, p1.y() + ny * offset}; +} + +bool check_out_of_bound_paths( + const CommonDataPtr & common_data_ptr, const std::vector & lane_changing_poses, + const LineString2d & lane_boundary, const Direction direction) +{ + const auto distance = (0.5 * common_data_ptr->bpp_param_ptr->vehicle_width + 0.1); + const auto offset = sign(direction) * distance; // invert direction + if (lane_changing_poses.size() <= 2) { + return true; // Remove candidates with insufficient poses + } + + LineString2d path_ls; + path_ls.reserve(lane_changing_poses.size()); + + const auto segments = lane_changing_poses | ranges::views::sliding(2); + ranges::for_each(segments | ranges::views::drop(1), [&](const auto & segment) { + const auto & p1 = segment[0].position; + const auto & p2 = segment[1].position; + boost::geometry::append(path_ls, shift_point({p2.x, p2.y}, {p1.x, p1.y}, offset)); + }); + + return boost::geometry::disjoint(path_ls, lane_boundary); // Remove if disjoint +} + +double calc_limit(const CommonDataPtr & common_data_ptr, const Pose & lc_end_pose) +{ + const auto dist_to_target_end = std::invoke([&]() { + if (common_data_ptr->lanes_ptr->target_lane_in_goal_section) { + return autoware::motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->route_handler_ptr->getGoalPose().position); + } + return autoware::motion_utils::calcSignedArcLength( + common_data_ptr->target_lanes_path.points, lc_end_pose.position, + common_data_ptr->target_lanes_path.points.back().point.pose.position); + }); + + // v2 = u2 + 2ad + // u = sqrt(2ad) + return std::clamp( + std::sqrt( + std::abs(2.0 * common_data_ptr->bpp_param_ptr->min_acc * std::max(dist_to_target_end, 0.0))), + common_data_ptr->lc_param_ptr->trajectory.min_lane_changing_velocity, + common_data_ptr->bpp_param_ptr->max_vel); +} + +}; // namespace + +namespace autoware::behavior_path_planner::utils::lane_change +{ +using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::PathType; + +bool get_prepare_segment( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const double prep_length, PathWithLaneId & prepare_segment) +{ + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto backward_path_length = common_data_ptr->bpp_param_ptr->backward_path_length; + + if (current_lanes.empty() || target_lanes.empty()) { + throw std::logic_error("Empty lanes!"); + } + + prepare_segment = prev_module_path; + const size_t current_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, common_data_ptr->get_ego_pose(), 3.0, 1.0); + utils::clipPathLength(prepare_segment, current_seg_idx, prep_length, backward_path_length); + + if (prepare_segment.points.empty()) return false; + + const auto & lc_start_pose = prepare_segment.points.back().point.pose; + + // TODO(Quda, Azu): Is it possible to remove these checks if we ensure prepare segment length is + // larger than distance to target lane start + if (!is_valid_start_point(common_data_ptr, lc_start_pose)) return false; + + // lane changing start is at the end of prepare segment + const auto target_length_from_lane_change_start_pose = + utils::getArcLengthToTargetLanelet(current_lanes, target_lanes.front(), lc_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 > std::numeric_limits::epsilon()) { + throw std::logic_error("lane change start is behind target lanelet!"); + } + + const auto curvatures = calc_curvatures(prepare_segment.points, common_data_ptr->get_ego_pose()); + + // curvatures may be empty if ego is near the terminal start of the path. + if (curvatures.empty()) { + return true; + } + + const auto average_curvature = calc_average_curvature(curvatures); + + RCLCPP_DEBUG(get_logger(), "average curvature: %.3f", average_curvature); + return average_curvature <= common_data_ptr->lc_param_ptr->trajectory.th_prepare_curvature; +} + +LaneChangePath get_candidate_path( + const CommonDataPtr & common_data_ptr, const LaneChangePhaseMetrics & prep_metric, + const LaneChangePhaseMetrics & lc_metric, const PathWithLaneId & prep_segment, + const std::vector> & sorted_lane_ids, const double shift_length) +{ + const auto & route_handler = *common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + + const auto resample_interval = calc_resample_interval(lc_metric.length, prep_metric.velocity); + + if (prep_segment.points.empty()) { + throw std::logic_error("Empty prepare segment!"); + } + + const auto & lc_start_pose = prep_segment.points.back().point.pose; + const auto target_lane_reference_path = get_reference_path_from_target_lane( + common_data_ptr, lc_start_pose, lc_metric.length, resample_interval); + + if (target_lane_reference_path.points.empty()) { + throw std::logic_error("Empty target reference!"); + } + + const auto lc_end_pose = std::invoke([&]() { + const auto dist_to_lc_start = + lanelet::utils::getArcCoordinates(target_lanes, lc_start_pose).length; + const auto dist_to_lc_end = dist_to_lc_start + lc_metric.length; + return route_handler.get_pose_from_2d_arc_length(target_lanes, dist_to_lc_end); + }); + + const auto shift_line = get_lane_changing_shift_line( + lc_start_pose, lc_end_pose, target_lane_reference_path, shift_length); + + LaneChangeInfo lane_change_info{prep_metric, lc_metric, lc_start_pose, lc_end_pose, shift_line}; + + if ( + lane_change_info.length.sum() + common_data_ptr->transient_data.next_dist_buffer.min > + common_data_ptr->transient_data.dist_to_terminal_end) { + throw std::logic_error("invalid candidate path length!"); + } + + return utils::lane_change::construct_candidate_path( + lane_change_info, prep_segment, target_lane_reference_path, sorted_lane_ids); +} + +LaneChangePath construct_candidate_path( + const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, + const PathWithLaneId & target_lane_reference_path, + const std::vector> & sorted_lane_ids) +{ + const auto & shift_line = lane_change_info.shift_line; + const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; + + auto shifted_path = get_shifted_path(target_lane_reference_path, lane_change_info); + + if (shifted_path.path.points.empty()) { + throw std::logic_error("Failed to generate shifted path."); + } + + if (shifted_path.path.points.size() < shift_line.end_idx + 1) { + throw std::logic_error("Path points are removed by PathShifter."); + } + + const auto lc_end_idx_opt = autoware::motion_utils::findNearestIndex( + shifted_path.path.points, lane_change_info.lane_changing_end); + + if (!lc_end_idx_opt) { + throw std::logic_error("Lane change end idx not found on target path."); + } + + std::vector prev_ids; + std::vector prev_sorted_ids; + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lc_end_idx_opt) { + const auto & current_ids = point.lane_ids; + point.lane_ids = + replace_with_sorted_ids(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); + continue; + } + const auto nearest_idx = + autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); + point.lane_ids = target_lane_reference_path.points.at(*nearest_idx).lane_ids; + } + + constexpr auto yaw_diff_th = autoware::universe_utils::deg2rad(5.0); + if ( + const auto yaw_diff_opt = + exceed_yaw_threshold(prepare_segment, shifted_path.path, yaw_diff_th)) { + std::stringstream err_msg; + err_msg << "Excessive yaw difference " << yaw_diff_opt.value() << " which exceeds the " + << yaw_diff_th << " radian threshold."; + throw std::logic_error(err_msg.str()); + } + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + candidate_path.info = lane_change_info; + candidate_path.type = PathType::ConstantJerk; + + return candidate_path; +} + +std::vector generate_frenet_candidates( + const CommonDataPtr & common_data_ptr, const PathWithLaneId & prev_module_path, + const std::vector & prep_metrics) +{ + std::vector trajectory_groups; + universe_utils::StopWatch sw; + + const auto & transient_data = common_data_ptr->transient_data; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto direction = common_data_ptr->direction; + const auto current_lane_boundary = get_linestring_bound(current_lanes, direction); + + for (const auto & metric : prep_metrics) { + PathWithLaneId prepare_segment; + try { + if (!utils::lane_change::get_prepare_segment( + common_data_ptr, prev_module_path, metric.length, prepare_segment)) { + RCLCPP_DEBUG(get_logger(), "Reject: failed to get valid prepare segment!"); + continue; + } + } catch (const std::exception & e) { + RCLCPP_WARN(get_logger(), "%s", e.what()); + break; + } + const auto lc_start_pose = prepare_segment.points.back().point.pose; + + const auto dist_to_end_from_lc_start = + calculation::calc_dist_from_pose_to_terminal_end( + common_data_ptr, target_lanes, lc_start_pose) - + common_data_ptr->lc_param_ptr->lane_change_finish_judge_buffer; + const auto max_lc_len = transient_data.lane_changing_length.max; + const auto max_lane_changing_length = std::min(dist_to_end_from_lc_start, max_lc_len); + + constexpr auto resample_interval = 0.5; + const auto target_lane_reference_path = get_reference_path_from_target_lane( + common_data_ptr, lc_start_pose, max_lane_changing_length, resample_interval); + if (target_lane_reference_path.points.empty()) { + continue; + } + + std::vector target_ref_path_sums{0.0}; + target_ref_path_sums.reserve(target_lane_reference_path.points.size() - 1); + double ref_s = 0.0; + for (auto it = target_lane_reference_path.points.begin(); + std::next(it) != target_lane_reference_path.points.end(); ++it) { + ref_s += universe_utils::calcDistance2d(*it, *std::next(it)); + target_ref_path_sums.push_back(ref_s); + } + + const auto reference_spline = init_reference_spline(target_lane_reference_path.points); + + const auto initial_state = init_frenet_state( + reference_spline.frenet({lc_start_pose.position.x, lc_start_pose.position.y}), metric); + + RCLCPP_DEBUG( + get_logger(), "Initial state [s=%2.2f, d=%2.2f, s'=%2.2f, d'=%2.2f, s''=%2.2f, d''=%2.2f]", + initial_state.position.s, initial_state.position.d, initial_state.longitudinal_velocity, + initial_state.lateral_velocity, initial_state.longitudinal_acceleration, + initial_state.lateral_acceleration); + + const auto sampling_parameters_opt = init_sampling_parameters( + common_data_ptr, metric, initial_state, reference_spline, lc_start_pose); + + if (!sampling_parameters_opt) { + continue; + } + + auto frenet_trajectories = frenet_planner::generateTrajectories( + reference_spline, initial_state, *sampling_parameters_opt); + + trajectory_groups.reserve(trajectory_groups.size() + frenet_trajectories.size()); + for (const auto & traj : frenet_trajectories) { + if (!trajectory_groups.empty()) { + const auto diff = std::abs( + traj.frenet_points.back().s - + trajectory_groups.back().lane_changing.frenet_points.back().s); + if (diff < common_data_ptr->lc_param_ptr->trajectory.th_lane_changing_length_diff) { + continue; + } + } + + const auto out_of_bound = + check_out_of_bound_paths(common_data_ptr, traj.poses, current_lane_boundary, direction); + + if (out_of_bound) { + continue; + } + + trajectory_groups.emplace_back( + prepare_segment, target_lane_reference_path, target_ref_path_sums, metric, traj, + initial_state, max_lane_changing_length); + } + } + + const auto limit_vel = [&](TrajectoryGroup & group) { + const auto max_vel = calc_limit(common_data_ptr, group.lane_changing.poses.back()); + for (auto & vel : group.lane_changing.longitudinal_velocities) { + vel = std::clamp(vel, 0.0, max_vel); + } + }; + + ranges::for_each(trajectory_groups, limit_vel); + + ranges::sort(trajectory_groups, [&](const auto & p1, const auto & p2) { + return calc_average_curvature(p1.lane_changing.curvatures) < + calc_average_curvature(p2.lane_changing.curvatures); + }); + + return trajectory_groups; +} + +std::optional get_candidate_path( + const TrajectoryGroup & trajectory_group, const CommonDataPtr & common_data_ptr, + const std::vector> & sorted_lane_ids) +{ + if (trajectory_group.lane_changing.frenet_points.empty()) { + return std::nullopt; + } + + ShiftedPath shifted_path; + std::vector prev_ids; + std::vector prev_sorted_ids; + const auto & lane_changing_candidate = trajectory_group.lane_changing; + const auto & target_lane_ref_path = trajectory_group.target_lane_ref_path; + const auto & prepare_segment = trajectory_group.prepare; + const auto & prepare_metric = trajectory_group.prepare_metric; + const auto & initial_state = trajectory_group.initial_state; + const auto & target_ref_sums = trajectory_group.target_lane_ref_path_dist; + auto zipped_candidates = ranges::views::zip( + lane_changing_candidate.poses, lane_changing_candidate.frenet_points, + lane_changing_candidate.longitudinal_velocities, lane_changing_candidate.lateral_velocities, + lane_changing_candidate.curvatures); + + shifted_path.path.points.reserve(zipped_candidates.size()); + shifted_path.shift_length.reserve(zipped_candidates.size()); + for (const auto & [pose, frenet_point, longitudinal_velocity, lateral_velocity, curvature] : + zipped_candidates) { + // Find the reference index + const auto & s = frenet_point.s; + auto ref_i_itr = std::find_if( + target_ref_sums.begin(), target_ref_sums.end(), + [s](const double ref_s) { return ref_s > s; }); + auto ref_i = std::distance(target_ref_sums.begin(), ref_i_itr); + + PathPointWithLaneId point; + point.point.pose = pose; + point.point.longitudinal_velocity_mps = static_cast(longitudinal_velocity); + point.point.lateral_velocity_mps = static_cast(lateral_velocity); + point.point.heading_rate_rps = static_cast(curvature); + point.point.pose.position.z = target_lane_ref_path.points[ref_i].point.pose.position.z; + const auto & current_ids = target_lane_ref_path.points[ref_i].lane_ids; + point.lane_ids = + replace_with_sorted_ids(current_ids, sorted_lane_ids, prev_ids, prev_sorted_ids); + + // Add to shifted path + shifted_path.shift_length.push_back(frenet_point.d); + shifted_path.path.points.push_back(point); + } + + if (shifted_path.path.points.empty()) { + return std::nullopt; + } + + for (auto & point : shifted_path.path.points) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + static_cast(shifted_path.path.points.back().point.longitudinal_velocity_mps)); + } + + const auto th_yaw_diff_deg = common_data_ptr->lc_param_ptr->frenet.th_yaw_diff_deg; + if ( + const auto yaw_diff_opt = exceed_yaw_threshold( + prepare_segment, shifted_path.path, autoware::universe_utils::deg2rad(th_yaw_diff_deg))) { + const auto yaw_diff_deg = autoware::universe_utils::rad2deg(yaw_diff_opt.value()); + const auto err_msg = fmt::format( + "Excessive yaw difference {yaw_diff:2.2f}[deg]. The threshold is {th_yaw_diff:2.2f}[deg].", + fmt::arg("yaw_diff", yaw_diff_deg), fmt::arg("th_yaw_diff", th_yaw_diff_deg)); + throw std::logic_error(err_msg); + } + + LaneChangeInfo info; + info.longitudinal_acceleration = { + prepare_metric.actual_lon_accel, lane_changing_candidate.longitudinal_accelerations.front()}; + info.velocity = {prepare_metric.velocity, initial_state.longitudinal_velocity}; + info.duration = { + prepare_metric.duration, lane_changing_candidate.sampling_parameter.target_duration}; + info.length = {prepare_metric.length, lane_changing_candidate.lengths.back()}; + info.lane_changing_start = prepare_segment.points.back().point.pose; + info.lane_changing_end = lane_changing_candidate.poses.back(); + + ShiftLine sl; + + sl.start = lane_changing_candidate.poses.front(); + sl.end = lane_changing_candidate.poses.back(); + sl.start_shift_length = 0.0; + sl.end_shift_length = initial_state.position.d; + sl.start_idx = 0UL; + sl.end_idx = shifted_path.shift_length.size() - 1; + + info.shift_line = sl; + info.terminal_lane_changing_velocity = lane_changing_candidate.longitudinal_velocities.back(); + info.lateral_acceleration = lane_changing_candidate.lateral_accelerations.front(); + + LaneChangePath candidate_path; + candidate_path.path = utils::combinePath(prepare_segment, shifted_path.path); + candidate_path.info = info; + candidate_path.shifted_path = shifted_path; + candidate_path.frenet_path = trajectory_group; + candidate_path.type = PathType::FrenetPlanner; + + return candidate_path; +} + +void append_target_ref_to_candidate( + LaneChangePath & frenet_candidate, const double th_curvature_smoothing) +{ + const auto & target_lane_ref_path = frenet_candidate.frenet_path.target_lane_ref_path.points; + const auto & lc_end_pose = frenet_candidate.info.lane_changing_end; + const auto lc_end_idx = + motion_utils::findNearestIndex(target_lane_ref_path, lc_end_pose.position); + auto & candidate_path = frenet_candidate.path.points; + if (target_lane_ref_path.size() <= lc_end_idx + 2) { + return; + } + const auto add_size = target_lane_ref_path.size() - (lc_end_idx + 1); + candidate_path.reserve(candidate_path.size() + add_size); + const auto & points = target_lane_ref_path; + for (const auto & [p2, p3] : ranges::views::zip( + points | ranges::views::drop(lc_end_idx + 1), + points | ranges::views::drop(lc_end_idx + 2))) { + const auto point1 = autoware::universe_utils::getPoint(candidate_path.back().point.pose); + const auto point2 = autoware::universe_utils::getPoint(p2); + const auto point3 = autoware::universe_utils::getPoint(p3); + const auto curvature = + std::abs(autoware::universe_utils::calcCurvature(point1, point2, point3)); + if (curvature < th_curvature_smoothing) { + candidate_path.push_back(p2); + } + } + candidate_path.push_back(target_lane_ref_path.back()); +} +} // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 4d46f8270fac3..9002270185847 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -14,34 +14,42 @@ #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_lane_change_module/structs/data.hpp" +#include "autoware/behavior_path_lane_change_module/structs/path.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/traffic_light_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/universe_utils/math/unit_conversion.hpp" +// for the geometry types +#include +#include +// for the svg mapper #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 @@ -56,19 +64,21 @@ #include #include #include +#include #include namespace autoware::behavior_path_planner::utils::lane_change { using autoware::route_handler::RouteHandler; using autoware::universe_utils::LineString2d; +using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; +using behavior_path_planner::lane_change::PathType; using geometry_msgs::msg::Pose; using tier4_planning_msgs::msg::PathWithLaneId; -using lanelet::ArcCoordinates; using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() @@ -84,29 +94,20 @@ bool is_mandatory_lane_change(const ModuleType lc_type) lc_type == LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE; } -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); -} - -void setPrepareVelocity( +void set_prepare_velocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { - if (current_velocity < prepare_velocity) { - // acceleration - for (auto & point : prepare_segment.points) { - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); - } - } else { + if (current_velocity >= prepare_velocity) { // deceleration prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( prepare_segment.points.back().point.longitudinal_velocity_mps, static_cast(prepare_velocity)); + return; + } + // acceleration + for (auto & point : prepare_segment.points) { + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(prepare_velocity)); } } @@ -128,34 +129,9 @@ lanelet::ConstLanelets get_target_neighbor_lanes( } } } - return neighbor_lanes; } -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; -} - bool path_footprint_exceeds_target_lane_bound( const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info, const double margin) @@ -186,152 +162,6 @@ bool path_footprint_exceeds_target_lane_bound( return false; } -std::optional construct_candidate_path( - const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, - const PathWithLaneId & prepare_segment, const PathWithLaneId & target_lane_reference_path, - const std::vector> & sorted_lane_ids) -{ - const auto & shift_line = lane_change_info.shift_line; - 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 = autoware::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 = - autoware::motion_utils::findNearestIndex(target_lane_reference_path.points, point.point.pose); - point.lane_ids = target_lane_reference_path.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 - const auto & current_lanes = common_data_ptr->lanes_ptr->current; - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - if ( - enable_path_check_in_lanelet && - !isPathInLanelets(shifted_path.path, current_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(autoware::universe_utils::normalizeRadian( - tf2::getYaw(prepare_segment_second_last_point.orientation) - - tf2::getYaw(lane_change_start_from_shifted.orientation))); - if (yaw_diff2 > autoware::universe_utils::deg2rad(5.0)) { - RCLCPP_DEBUG( - get_logger(), "Excessive yaw difference %.3f which exceeds the 5 degrees threshold.", - autoware::universe_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 get_reference_path_from_target_Lane( - const CommonDataPtr & common_data_ptr, const Pose & lane_changing_start_pose, - const double lane_changing_length, const double resample_interval) -{ - const auto & route_handler = *common_data_ptr->route_handler_ptr; - const auto & target_lanes = common_data_ptr->lanes_ptr->target; - const auto target_lane_length = common_data_ptr->transient_data.target_lane_length; - const auto is_goal_in_route = common_data_ptr->lanes_ptr->target_lane_in_goal_section; - const auto next_lc_buffer = common_data_ptr->transient_data.next_dist_buffer.min; - const auto forward_path_length = common_data_ptr->bpp_param_ptr->forward_path_length; - - const ArcCoordinates lane_change_start_arc_position = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); - - const double s_start = lane_change_start_arc_position.length; - const double s_end = 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_lc_buffer; - return std::min(dist_from_lc_start, s_goal); - } - return std::min(dist_from_lc_start, target_lane_length - next_lc_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 get_lane_changing_shift_line( - 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 = autoware::motion_utils::findNearestIndex( - reference_path.points, lane_changing_start_pose.position); - shift_line.end_idx = autoware::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) @@ -561,18 +391,26 @@ std::vector> get_sorted_lane_ids(const CommonDataPtr & comm return sorted_lane_ids; } -std::vector replaceWithSortedIds( - const std::vector & original_lane_ids, - const std::vector> & sorted_lane_ids) +std::vector replace_with_sorted_ids( + const std::vector & current_lane_ids, + const std::vector> & sorted_lane_ids, std::vector & prev_lane_ids, + std::vector & prev_sorted_lane_ids) { - for (const auto original_id : original_lane_ids) { + if (current_lane_ids == prev_lane_ids) { + return prev_sorted_lane_ids; + } + + for (const auto original_id : current_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; + prev_lane_ids = current_lane_ids; + prev_sorted_lane_ids = sorted_id; + return prev_sorted_lane_ids; } } } - return original_lane_ids; + + return current_lane_ids; } CandidateOutput assignToCandidate( @@ -627,6 +465,7 @@ std::vector convert_to_predicted_path( const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; const auto resolution = lc_param_ptr->safety.collision_check.prediction_time_resolution; std::vector predicted_path; + predicted_path.reserve(static_cast(std::ceil(duration / resolution))); // prepare segment for (double t = 0.0; t < prepare_time; t += resolution) { @@ -643,6 +482,7 @@ std::vector convert_to_predicted_path( initial_velocity + prepare_acc * prepare_time, 0.0, lane_change_path.info.velocity.prepare); const auto offset = initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; + for (double t = prepare_time; t < duration; t += resolution) { const auto delta_t = t - prepare_time; const auto velocity = std::clamp( @@ -652,6 +492,7 @@ std::vector convert_to_predicted_path( 0.5 * lane_changing_acceleration * delta_t * delta_t + offset; const auto pose = autoware::motion_utils::calcInterpolatedPose( path.points, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); } @@ -1097,19 +938,17 @@ double get_min_dist_to_current_lanes_obj( continue; } - // calculate distance from path front to the stationary object polygon on the ego lane. - for (const auto & polygon_p : object.initial_polygon.outer()) { - const auto p_fp = autoware::universe_utils::toMsg(polygon_p.to_3d()); - const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); - - // ignore if the point is not on ego path - if (std::abs(lateral_fp) > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { - continue; - } - - const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp); - min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); + // check if object is on ego path + const auto obj_half_width = object.shape.dimensions.y / 2; + const auto obj_lat_dist_to_path = + std::abs(motion_utils::calcLateralOffset(path_points, object.initial_pose.position)) - + obj_half_width; + if (obj_lat_dist_to_path > (common_data_ptr->bpp_param_ptr->vehicle_width / 2)) { + continue; } + + min_dist_to_obj = std::min(min_dist_to_obj, dist_to_obj); + break; } return min_dist_to_obj; } @@ -1246,6 +1085,43 @@ bool filter_target_lane_objects( return false; } +std::vector get_preceding_lanes(const CommonDataPtr & common_data_ptr) +{ + const auto & route_handler_ptr = common_data_ptr->route_handler_ptr; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; + const auto & ego_pose = common_data_ptr->get_ego_pose(); + const auto backward_lane_length = common_data_ptr->lc_param_ptr->backward_lane_length; + + const auto preceding_lanes_list = + utils::getPrecedingLanelets(*route_handler_ptr, target_lanes, ego_pose, backward_lane_length); + + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + std::unordered_set current_lanes_id; + for (const auto & lane : current_lanes) { + current_lanes_id.insert(lane.id()); + } + const auto is_overlapping = [&](const lanelet::ConstLanelet & lane) { + return current_lanes_id.find(lane.id()) != current_lanes_id.end(); + }; + + std::vector non_overlapping_lanes_vec; + for (const auto & lanes : preceding_lanes_list) { + auto lanes_reversed = lanes | ranges::views::reverse; + auto overlapped_itr = ranges::find_if(lanes_reversed, is_overlapping); + + if (overlapped_itr == lanes_reversed.begin()) { + continue; + } + + // Lanes are not reversed by default. Avoid returning reversed lanes to prevent undefined + // behavior. + lanelet::ConstLanelets non_overlapping_lanes(overlapped_itr.base(), lanes.end()); + non_overlapping_lanes_vec.push_back(non_overlapping_lanes); + } + + return non_overlapping_lanes_vec; +} + bool object_path_overlaps_lanes( const ExtendedPredictedObject & object, const lanelet::BasicPolygon2d & lanes_polygon) { @@ -1253,4 +1129,85 @@ bool object_path_overlaps_lanes( return !boost::geometry::disjoint(path, lanes_polygon); }); } + +std::vector> convert_to_predicted_paths( + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, + const size_t deceleration_sampling_num) +{ + static constexpr double floating_err_th{1e-3}; + const auto bpp_param = *common_data_ptr->bpp_param_ptr; + const auto global_min_acc = bpp_param.min_acc; + const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; + + const auto min_acc = std::min(lane_changing_acc, global_min_acc); + const auto sampling_num = + std::abs(min_acc - lane_changing_acc) > floating_err_th ? deceleration_sampling_num : 1; + const auto acc_resolution = (min_acc - lane_changing_acc) / static_cast(sampling_num); + + const auto ego_predicted_path = [&](size_t n) { + if (lane_change_path.type == PathType::FrenetPlanner) { + return convert_to_predicted_path( + common_data_ptr, lane_change_path.frenet_path, deceleration_sampling_num); + } + auto acc = lane_changing_acc + static_cast(n) * acc_resolution; + return utils::lane_change::convert_to_predicted_path(common_data_ptr, lane_change_path, acc); + }; + + return ranges::views::iota(0UL, sampling_num) | ranges::views::transform(ego_predicted_path) | + ranges::to(); +} + +std::vector convert_to_predicted_path( + const CommonDataPtr & common_data_ptr, const lane_change::TrajectoryGroup & frenet_candidate, + [[maybe_unused]] const size_t deceleration_sampling_num) +{ + const auto initial_velocity = common_data_ptr->get_ego_speed(); + const auto prepare_time = frenet_candidate.prepare_metric.duration; + const auto resolution = + common_data_ptr->lc_param_ptr->safety.collision_check.prediction_time_resolution; + const auto prepare_acc = frenet_candidate.prepare_metric.sampled_lon_accel; + std::vector predicted_path; + const auto & path = frenet_candidate.prepare.points; + const auto & vehicle_pose = common_data_ptr->get_ego_pose(); + const auto & bpp_param_ptr = common_data_ptr->bpp_param_ptr; + const auto nearest_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path, vehicle_pose, bpp_param_ptr->ego_nearest_dist_threshold, + bpp_param_ptr->ego_nearest_yaw_threshold); + + const auto vehicle_pose_frenet = + convertToFrenetPoint(path, vehicle_pose.position, nearest_seg_idx); + + for (double t = 0.0; t < prepare_time; t += resolution) { + const auto velocity = + std::clamp(initial_velocity + prepare_acc * t, 0.0, frenet_candidate.prepare_metric.velocity); + const auto length = initial_velocity * t + 0.5 * prepare_acc * t * t; + const auto pose = + autoware::motion_utils::calcInterpolatedPose(path, vehicle_pose_frenet.length + length); + predicted_path.emplace_back(t, pose, velocity); + } + + const auto & poses = frenet_candidate.lane_changing.poses; + const auto & velocities = frenet_candidate.lane_changing.longitudinal_velocities; + const auto & times = frenet_candidate.lane_changing.times; + + for (const auto [t, pose, velocity] : + ranges::views::zip(times, poses, velocities) | ranges::views::drop(1)) { + predicted_path.emplace_back(prepare_time + t, pose, velocity); + } + + return predicted_path; +} + +bool is_valid_start_point(const lane_change::CommonDataPtr & common_data_ptr, const Pose & pose) +{ + const lanelet::BasicPoint2d lc_start_point(pose.position.x, pose.position.y); + + const auto & target_neighbor_poly = common_data_ptr->lanes_polygon_ptr->target_neighbor; + const auto & target_lane_poly = common_data_ptr->lanes_polygon_ptr->target; + + // Check the target lane because the previous approved path might be shifted by avoidance module + return boost::geometry::covered_by(lc_start_point, target_neighbor_poly) || + boost::geometry::covered_by(lc_start_point, target_lane_poly); +} } // namespace autoware::behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 555657fe34af9..f6b3e9eeaf715 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,10 +12,7 @@ // 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 "autoware_test_utils/autoware_test_utils.hpp" +#include "autoware/behavior_path_planner/test_utils.hpp" #include @@ -24,78 +21,18 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::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 autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_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::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); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_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"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); + auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"lane_change"}, {"autoware::behavior_path_planner::LaneChangeRightModuleManager", + "autoware::behavior_path_planner::LaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); @@ -113,7 +50,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"lane_change"}, {"autoware::behavior_path_planner::LaneChangeRightModuleManager", + "autoware::behavior_path_planner::LaneChangeLeftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp index ea36e4dc6960a..5fb445788672c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_scene.cpp @@ -14,7 +14,7 @@ #include "autoware/behavior_path_lane_change_module/manager.hpp" #include "autoware/behavior_path_lane_change_module/scene.hpp" -#include "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" #include "autoware_test_utils/mock_data_parser.hpp" @@ -242,15 +242,14 @@ TEST_F(TestNormalLaneChange, testFilteredObjects) const auto & filtered_objects = get_filtered_objects(); - // Note: There's 1 stopping object in current lanes, however, it was filtered out. const auto filtered_size = filtered_objects.current_lane.size() + filtered_objects.target_lane_leading.size() + filtered_objects.target_lane_trailing.size() + filtered_objects.others.size(); EXPECT_EQ(filtered_size, planner_data_->dynamic_object->objects.size()); - EXPECT_EQ(filtered_objects.current_lane.size(), 0); + EXPECT_EQ(filtered_objects.current_lane.size(), 1); EXPECT_EQ(filtered_objects.target_lane_leading.size(), 2); EXPECT_EQ(filtered_objects.target_lane_trailing.size(), 0); - EXPECT_EQ(filtered_objects.others.size(), 2); + EXPECT_EQ(filtered_objects.others.size(), 1); } TEST_F(TestNormalLaneChange, testGetPathWhenValid) @@ -258,6 +257,7 @@ TEST_F(TestNormalLaneChange, testGetPathWhenValid) constexpr auto is_approved = true; ego_pose_ = autoware::test_utils::createPose(1.0, 1.75, 0.0, 0.0, 0.0, 0.0); planner_data_->self_odometry = set_odometry(ego_pose_); + normal_lane_change_->setData(planner_data_); set_previous_approved_path(); normal_lane_change_->update_lanes(!is_approved); normal_lane_change_->update_filtered_objects(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp index 68547a491324d..147a53672f30a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/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 "autoware/behavior_path_lane_change_module/utils/data_structs.hpp" +#include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include #include diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_planner/CHANGELOG.rst index 654a5b4ed0d87..ce092fbe73c79 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/CHANGELOG.rst @@ -2,6 +2,61 @@ Changelog for package autoware_behavior_path_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(start_planner): visualize planner evaluation table in rviz (`#10029 `_) + visualize planner evaluation table in rviz +* feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset (`#9967 `_) + * feat(autoware_planning_test_manager): remove dependency of tier4_planning_msgs::msg::LateralOffset + * fix + --------- +* refactor(behavior_path_planner): common test functions (`#9963 `_) + * feat: common test code in behavior_path_planner + * deal with other modules + * fix typo + * update + --------- +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix(planning): text revisions (`#9886 `_) + * fix(planning): text revisions + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* docs(bpp): revise explanation for Failure modules (`#9863 `_) +* feat(behavior_path_planner): use autoware internal stamped messages (`#9750 `_) + * feat(behavior_path_planner): use autoware internal stamped messages + * fix universe_utils + --------- +* Contributors: Atto Armoo, Fumiya Watanabe, Kyoichi Sugahara, Mamoru Sobue, Satoshi OTA, Takayuki Murooka, Zulfaqar Azmi + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt index cadaacd01d9c9..f43b454cfb100 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(magic_enum CONFIG REQUIRED) ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp src/behavior_path_planner_node.cpp + src/test_utils.cpp ) target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md index ab8d02dc83f92..81cb945802595 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/README.md @@ -4,11 +4,11 @@ The Behavior Path Planner's main objective is to significantly enhance the safet 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. +Moreover, the planner responds to the behavior of 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 compliance with 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. + The [Planning Component Design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/) documentation 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 @@ -22,23 +22,23 @@ Essentially, the module has three primary responsibilities: ### Supported Scene Modules -Behavior Path Planner has following scene modules +Behavior Path Planner has the 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) | +| Name | Description | Details | +| :------------------------- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------------- | +| Lane Following | This module generates a reference path from lanelet centerline. | LINK | +| Static Obstacle Avoidance | This module generates an avoidance path when there are objects that should be avoided. | [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 a lane change path when there are objects that should be avoided. | [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 the ego vehicle is in a driving lane and the goal is in the shoulder lane. The ego vehicle will stop at the goal. | [LINK](../autoware_behavior_path_goal_planner_module/README.md) | +| Start Planner | This module is performed when the ego vehicle is stationary and the footprint of the ego vehicle is included in the shoulder lane. This module ends when the ego vehicle merges into the road. | [LINK](../autoware_behavior_path_start_planner_module/README.md) | +| Side Shift | This module shifts the path to the left or right based on external instructions, intended for remote control applications. | [LINK](../autoware_behavior_path_side_shift_module/README.md) | !!! Note - click on the following images to view the video of their execution + Click on the following images to view videos of their execution
@@ -59,13 +59,13 @@ Behavior Path Planner has following scene modules 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? +#### How to add or implement new module -All scene modules are implemented by inheriting base class `scene_module_interface.hpp`. +All scene modules are implemented by inheriting the base class `scene_module_interface.hpp`. !!! Warning - The remainder of this subsection is work in progress (WIP). + The remainder of this subsection is a work in progress (WIP). ### Planner Manager @@ -77,65 +77,65 @@ The Planner Manager's responsibilities include: !!! 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). + 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. + For more in-depth information, refer to the [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/autoware_default_adapi/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. +| 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 the perception module | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | Occupancy grid map from the perception module. This is used for only the Goal Planner module | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | Traffic signal 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 the planning module to know if vehicle is in autonomous mode or if it can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/autoware_default_adapi/document/operation-mode.md) | + +- ○ Mandatory: The planning module would not work if anyone of these were not present. +- △ Optional: Some modules would not work, but the 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/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` | +| 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/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` | +| Name | Type | Description | QoS Durability | +| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------------- | -------------- | +| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | Debug message for avoidance. Notifies users of reasons avoidance path cannot be generated | `volatile` | +| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | Debug message for lane change. Notifies users of unsafe conditions 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 module | `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). + For specific information about which topics are being subscribed to 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 +## How to Enable or Disable 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`. +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: +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. @@ -143,14 +143,12 @@ The `default_preset.yaml` file acts as a configuration file for enabling or disa 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 +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 the following segment corresponds to launch_static_obstacle_avoidance_module from `default_preset.yaml`: ```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 @@ -163,13 +161,13 @@ The `ShiftLine` struct (as seen [here](https://github.com/autowarefoundation/aut 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. +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 +## 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: @@ -207,7 +205,7 @@ Static drivable area expansion operates under assumptions about the correct arra !!! note - Further details can is provided in [Drivable Area Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). + Further details can be found in [Drivable Area Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). ### Dynamic Drivable Area Logic @@ -233,7 +231,7 @@ The `TurnIndicatorsCommand` message structure has a command field that can take !!! warning - Rerouting is a feature that was still under progress. Further information will be included on a later date. + The rerouting feature is under development. Further information will be included at a later date. ## Parameters and Configuration @@ -278,5 +276,5 @@ preset ## 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. +1. The Goal Planner module cannot be simultaneously executed together with other modules. +2. The module is not designed as a plugin. Integrating a custom module is not straightforward. Users have to modify part of the Behavior Path Planner's main code. diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md index 1e8532eba2f3a..3b787303d8d20 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -533,9 +533,9 @@ If this case happened in the slot, `is_upstream_waiting_approved` is set to true ### Failure modules -The failure modules return the status `ModuleStatus::FAILURE`. The manager removes the module from approved modules stack as well as waiting approval modules, but the failure module is not moved to candidate modules stack. +If a module returns `ModuleStatus::FAILURE`, the manager removes the failed module. Additionally, all modules after the failed module are removed, even if they did not return `ModuleStatus::FAILURE`. These modules are not added back to the candidate modules stack and will instead run again from the beginning. Once these modules are removed, the output of the module prior to the failed module will be used as the planner's output. -As a result, the module A's output is used as approved modules stack. +As shown in the example below, modules B, A, and C are running. When module A returns `ModuleStatus::FAILURE`, both module A and C are removed from the approved modules stack. Module B's output is then used as the final output of the planner. ![failure_modules](../image/manager/failure_modules.drawio.svg) diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp index ebe7c47e6fab2..d88c93712b1e3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp @@ -17,10 +17,11 @@ #include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware/universe_utils/ros/debug_publisher.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "planner_manager.hpp" -#include +#include #include #include @@ -51,7 +52,7 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::SteeringFactorInterface; +using autoware::planning_factor_interface::PlanningFactorInterface; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::PredictedObject; @@ -72,6 +73,7 @@ using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; +using DebugPublisher = autoware::universe_utils::DebugPublisher; class BehaviorPathPlannerNode : public rclcpp::Node { @@ -121,7 +123,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::Publisher::SharedPtr modified_goal_publisher_; rclcpp::Publisher::SharedPtr reroute_availability_publisher_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; rclcpp::TimerBase::SharedPtr timer_; std::map::SharedPtr> path_candidate_publishers_; @@ -136,7 +137,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::shared_ptr planner_manager_; - SteeringFactorInterface steering_factor_interface_; + std::unique_ptr planning_factor_interface_; std::mutex mutex_pd_; // mutex for planner_data_ std::mutex mutex_manager_; // mutex for bt_manager_ or planner_manager_ @@ -187,6 +188,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node // debug rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; + std::unique_ptr debug_start_planner_evaluation_table_publisher_ptr_; /** * @brief publish reroute availability diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp index 3bed1e6a88bd8..741202590779c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include @@ -44,8 +46,8 @@ using tier4_planning_msgs::msg::PathWithLaneId; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; using DebugPublisher = autoware::universe_utils::DebugPublisher; -using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; -using DebugStringMsg = tier4_debug_msgs::msg::StringStamped; +using DebugDoubleMsg = autoware_internal_debug_msgs::msg::Float64Stamped; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; struct SceneModuleUpdateInfo { diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/test_utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/test_utils.hpp new file mode 100644 index 0000000000000..5e1718419f139 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/test_utils.hpp @@ -0,0 +1,42 @@ +// 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_PLANNER__TEST_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_PATH_PLANNER__TEST_UTILS_HPP_ + +#include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +using autoware::behavior_path_planner::BehaviorPathPlannerNode; +using autoware::planning_test_manager::PlanningInterfaceTestManager; + +std::shared_ptr generateTestManager(); + +std::shared_ptr generateNode( + const std::vector & module_name_vec, + const std::vector & plugin_name_vec); + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node); +} // namespace autoware::behavior_path_planner + +#endif // AUTOWARE__BEHAVIOR_PATH_PLANNER__TEST_UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml index 6141480d1597a..3b5b28101ff3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_planner - 0.40.0 + 0.41.0 The behavior_path_planner package @@ -21,6 +21,7 @@ Kyoichi Sugahara Takayuki Murooka Go Sakayori + Mamoru Sobue Apache License 2.0 @@ -29,7 +30,6 @@ Fumiya Watanabe Zulfaqar Azmi Kosuke Takeuchi - Yutaka Shimizu Takayuki Murooka Ryohsuke Mitsudome @@ -40,6 +40,7 @@ autoware_behavior_path_planner_common autoware_freespace_planning_algorithms autoware_frenet_planner + autoware_internal_debug_msgs autoware_interpolation autoware_lane_departure_checker autoware_lanelet2_extension @@ -47,6 +48,7 @@ autoware_object_recognition_utils autoware_path_sampler autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_planning_test_manager autoware_signal_processing diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp index 7758ab530e88c..38c5aa0085fb9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -30,9 +31,12 @@ namespace autoware::behavior_path_planner { using autoware::vehicle_info_utils::VehicleInfoUtils; using tier4_planning_msgs::msg::PathChangeModuleId; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) -: Node("behavior_path_planner", node_options) +: Node("behavior_path_planner", node_options), + planning_factor_interface_{ + std::make_unique(this, "behavior_path_planner")} { using std::placeholders::_1; using std::chrono_literals::operator""ms; @@ -51,13 +55,15 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto durable_qos = rclcpp::QoS(1).transient_local(); modified_goal_publisher_ = create_publisher("~/output/modified_goal", durable_qos); - pub_steering_factors_ = - create_publisher("/planning/steering_factor/intersection", 1); reroute_availability_publisher_ = create_publisher("~/output/is_reroute_available", 1); + debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); + debug_start_planner_evaluation_table_publisher_ptr_ = + std::make_unique(this, "~/debug/start_planner_evaluation_table"); + debug_turn_signal_info_publisher_ = create_publisher("~/debug/turn_signal_info", 1); bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -115,8 +121,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_search_time, turn_signal_intersection_angle_threshold_deg); } - steering_factor_interface_.init(PlanningBehavior::INTERSECTION); - // Start timer { const auto planning_hz = declare_parameter("planning_hz"); @@ -463,33 +467,22 @@ void BehaviorPathPlannerNode::publish_steering_factor( const auto [intersection_flag, approaching_intersection_flag] = planner_data->turn_signal_decider.getIntersectionTurnSignalFlag(); if (intersection_flag || approaching_intersection_flag) { - const uint16_t steering_factor_direction = std::invoke([&turn_signal]() { - if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - const auto [intersection_pose, intersection_distance] = planner_data->turn_signal_decider.getIntersectionPoseAndDistance(); - steering_factor_interface_.set( - {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); - } else { - steering_factor_interface_.reset(); - } - - autoware_adapi_v1_msgs::msg::SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = this->now(); + const uint16_t planning_factor_direction = std::invoke([&turn_signal]() { + if (turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { + return PlanningFactor::TURN_LEFT; + } + return PlanningFactor::TURN_RIGHT; + }); - const auto steering_factor = steering_factor_interface_.get(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); + planning_factor_interface_->add( + intersection_distance, intersection_distance, intersection_pose, intersection_pose, + planning_factor_direction, SafetyFactorArray{}); } - pub_steering_factors_->publish(steering_factor_array); + planning_factor_interface_->publish(); } void BehaviorPathPlannerNode::publish_reroute_availability() const @@ -621,6 +614,11 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg( if (avoidance_debug_message) { debug_avoidance_msg_array_publisher_->publish(*avoidance_debug_message); } + const auto start_planner_debug_message = debug_messages_data_ptr->getStartPlannerModuleDebugMsg(); + if (start_planner_debug_message) { + debug_start_planner_evaluation_table_publisher_ptr_->publish( + "start_planner_evaluation_table", *(start_planner_debug_message)); + } } void BehaviorPathPlannerNode::publishPathCandidate( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp index 041b7ad20a107..322209d46732b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp @@ -182,8 +182,7 @@ BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & da std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->updateObserver(); m->publishRTCStatus(); - m->publishSteeringFactor(); - m->publishVelocityFactor(); + m->publish_planning_factors(); }); generateCombinedDrivableArea(result_output.valid_output, data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp new file mode 100644 index 0000000000000..4930ead6d355d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/src/test_utils.cpp @@ -0,0 +1,118 @@ +// 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_planner/test_utils.hpp" + +#include +#include +#include + +#include + +#include +#include +#include + +namespace autoware::behavior_path_planner +{ +namespace +{ +using tier4_planning_msgs::msg::LateralOffset; + +void publishLateralOffset( + std::shared_ptr test_manager, + std::shared_ptr test_target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto pub_lateral_offset = test_manager->getTestNode()->create_publisher( + "behavior_path_planner/input/lateral_offset", 1); + pub_lateral_offset->publish(LateralOffset{}); + autoware::test_utils::spinSomeNodes(test_node, test_target_node, 3); +} +} // namespace + +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( + const std::vector & module_name_vec, + const std::vector & plugin_name_vec) +{ + auto node_options = rclcpp::NodeOptions{}; + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto behavior_path_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); + + std::vector plugin_names; + for (const auto & plugin_name : plugin_name_vec) { + plugin_names.emplace_back(plugin_name); + } + + std::vector params; + params.emplace_back("launch_modules", plugin_names); + node_options.parameter_overrides(params); + + const auto get_behavior_path_module_config = [](const std::string & module) { + const auto package_name = "autoware_behavior_path_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + auto yaml_files = std::vector{ + autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_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"}; + for (const auto & module_name : module_name_vec) { + yaml_files.push_back(get_behavior_path_module_config(module_name)); + } + + autoware::test_utils::updateNodeOptions(node_options, yaml_files); + + 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"); + publishLateralOffset(test_manager, test_target_node); +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp index 63b7ccf3fff12..0c933345e7647 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner/test/test_autoware_behavior_path_planner_node_interface.cpp @@ -13,10 +13,7 @@ // limitations under the License. #include "autoware/behavior_path_planner/behavior_path_planner_node.hpp" - -#include -#include -#include +#include "autoware/behavior_path_planner/test_utils.hpp" #include @@ -25,73 +22,15 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::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 autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - - std::vector module_names; - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_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"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode({}, {}); publishMandatoryTopics(test_manager, test_target_node); @@ -109,7 +48,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode({}, {}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CHANGELOG.rst index 6c72ac310e31f..d20daf5d2a0c6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CHANGELOG.rst @@ -2,6 +2,48 @@ Changelog for package autoware_behavior_path_planner_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(start_planner): visualize planner evaluation table in rviz (`#10029 `_) + visualize planner evaluation table in rviz +* feat(static_obstacle_avoidance): output safety factor (`#10000 `_) + * feat(safety_check): convert to SafetyFactor + * feat(static_obstacle_avoidance): use safety factor + * fix(bpp): output detail + --------- +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix(autoware_behavior_path_planner_common): fix bugprone-errors (`#9700 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, Kyoichi Sugahara, Mamoru Sobue, Satoshi OTA, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp index 391ac92411220..c5c0972972773 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -21,12 +21,11 @@ #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include -#include -#include #include #include #include #include +#include #include #include #include @@ -36,7 +35,6 @@ #include #include -#include #include #include #include @@ -54,18 +52,16 @@ namespace autoware::behavior_path_planner { -using autoware::motion_utils::SteeringFactorInterface; -using autoware::motion_utils::VelocityFactorInterface; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using autoware::planning_factor_interface::PlanningFactorInterface; using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::calcOffsetPose; using autoware::universe_utils::generateUUID; -using autoware_adapi_v1_msgs::msg::PlanningBehavior; -using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactor; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; using visualization_msgs::msg::MarkerArray; @@ -86,13 +82,15 @@ class 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) + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) : 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)), + planning_factor_interface_{planning_factor_interface}, time_keeper_(std::make_shared()) { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { @@ -184,8 +182,6 @@ class SceneModuleInterface unlockNewModuleLaunch(); unlockOutputPath(); - reset_factor(); - processOnExit(); } @@ -258,16 +254,6 @@ class SceneModuleInterface ModuleStatus getCurrentStatus() const { return current_state_; } - void reset_factor() - { - steering_factor_interface_.reset(); - velocity_factor_interface_.reset(); - } - - auto get_steering_factor() const -> SteeringFactor { return steering_factor_interface_.get(); } - - auto get_velocity_factor() const -> VelocityFactor { return velocity_factor_interface_.get(); } - std::string name() const { return name_; } PoseWithDetailOpt getStopPose() const @@ -558,11 +544,12 @@ class SceneModuleInterface } } - void setVelocityFactor(const PathWithLaneId & path) + void set_longitudinal_planning_factor(const PathWithLaneId & path) { if (stop_pose_.has_value()) { - velocity_factor_interface_.set( - path.points, getEgoPose(), stop_pose_.value().pose, VelocityFactor::APPROACHING, "stop"); + planning_factor_interface_->add( + path.points, getEgoPose(), stop_pose_.value().pose, PlanningFactor::STOP, + SafetyFactorArray{}, true, 0.0, 0.0, stop_pose_.value().detail); return; } @@ -570,8 +557,9 @@ class SceneModuleInterface return; } - velocity_factor_interface_.set( - path.points, getEgoPose(), slow_pose_.value().pose, VelocityFactor::APPROACHING, "slow down"); + planning_factor_interface_->add( + path.points, getEgoPose(), slow_pose_.value().pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}, true, 0.0, 0.0, slow_pose_.value().detail); } void setDrivableLanes(const std::vector & drivable_lanes); @@ -627,9 +615,7 @@ class SceneModuleInterface std::unordered_map> objects_of_interest_marker_interface_ptr_map_; - mutable SteeringFactorInterface steering_factor_interface_; - - mutable VelocityFactorInterface velocity_factor_interface_; + mutable std::shared_ptr planning_factor_interface_; mutable PoseWithDetailOpt stop_pose_{std::nullopt}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 73b133952935b..446829d1d0f9d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -24,8 +24,6 @@ #include #include -#include -#include #include #include @@ -42,8 +40,6 @@ using autoware::motion_utils::createDeadLineVirtualWallMarker; using autoware::motion_utils::createSlowDownVirtualWallMarker; using autoware::motion_utils::createStopVirtualWallMarker; using autoware::universe_utils::toHexString; -using autoware_adapi_v1_msgs::msg::SteeringFactorArray; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using unique_identifier_msgs::msg::UUID; using SceneModulePtr = std::shared_ptr; using SceneModuleObserver = std::weak_ptr; @@ -106,45 +102,7 @@ class SceneModuleManagerInterface } } - void publishSteeringFactor() - { - SteeringFactorArray steering_factor_array; - steering_factor_array.header.frame_id = "map"; - steering_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto steering_factor = m.lock()->get_steering_factor(); - if (steering_factor.behavior != PlanningBehavior::UNKNOWN) { - steering_factor_array.factors.emplace_back(steering_factor); - } - } - - pub_steering_factors_->publish(steering_factor_array); - } - - void publishVelocityFactor() - { - VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = node_->now(); - - for (const auto & m : observers_) { - if (m.expired()) { - continue; - } - - const auto velocity_factor = m.lock()->get_velocity_factor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - } - - pub_velocity_factors_->publish(velocity_factor_array); - } + void publish_planning_factors() { planning_factor_interface_->publish(); } void publishVirtualWall() const { @@ -304,10 +262,6 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_drivable_lanes_; - rclcpp::Publisher::SharedPtr pub_steering_factors_; - - rclcpp::Publisher::SharedPtr pub_velocity_factors_; - rclcpp::Publisher::SharedPtr pub_processing_time_; std::string name_; @@ -318,6 +272,8 @@ class SceneModuleManagerInterface std::unique_ptr idle_module_ptr_; + std::shared_ptr planning_factor_interface_; + std::unordered_map> rtc_interface_ptr_map_; std::unordered_map> diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp index e2d5d276070ee..f3921e4778593 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" +#include #include namespace autoware::behavior_path_planner @@ -31,16 +32,20 @@ class SideShiftModule; using tier4_planning_msgs::msg::AvoidanceDebugMsg; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; class SceneModuleVisitor { public: void visitAvoidanceModule(const StaticObstacleAvoidanceModule * module) const; + void visitStartPlannerModule(const StartPlannerModule * module) const; std::shared_ptr getAvoidanceModuleDebugMsg() const; + std::shared_ptr getStartPlannerModuleDebugMsg() const; protected: mutable std::shared_ptr avoidance_visitor_; + mutable std::shared_ptr start_planner_visitor_; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index ab62c369c41e4..23ff10a2a0abf 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -283,6 +284,9 @@ double calculateRoughDistanceToObjects( CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); void updateCollisionCheckDebugMap( CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); + +tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( + const CollisionCheckDebugMap & debug_map); } // namespace autoware::behavior_path_planner::utils::path_safety_checker #endif // AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml index f23a9980f1237..0a3d919ead23b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_planner_common - 0.40.0 + 0.41.0 The autoware_behavior_path_planner_common package @@ -52,6 +52,7 @@ autoware_object_recognition_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_route_handler autoware_rtc_interface diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp index b91db3a740cfe..11be3b28a7f94 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_manager_interface.cpp @@ -61,10 +61,11 @@ void SceneModuleManagerInterface::initInterface( pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); pub_processing_time_ = node->create_publisher( "~/processing_time/" + name_, 20); - pub_steering_factors_ = - node->create_publisher("/planning/steering_factor/" + name_, 1); - pub_velocity_factors_ = - node->create_publisher("/planning/velocity_factors/" + name_, 1); + } + + // planning factor + { + planning_factor_interface_ = std::make_shared(node, name_); } // misc diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp index e6ccfc74866c4..45c4f17c32698 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp @@ -24,4 +24,10 @@ std::shared_ptr SceneModuleVisitor::getAvoidanceModuleDe { return avoidance_visitor_; } + +std::shared_ptr SceneModuleVisitor::getStartPlannerModuleDebugMsg() const +{ + return start_planner_visitor_; +} + } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 0768c620dd054..3b272fd8c6722 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -1127,9 +1127,7 @@ std::vector getBoundWithHatchedRoadMarkings( get_corresponding_polygon_index(*current_polygon, bound_point.id())); } } else { - if (!polygon) { - will_close_polygon = true; - } else if (polygon.value().id() != current_polygon.value().id()) { + if (!polygon || polygon.value().id() != current_polygon.value().id()) { will_close_polygon = true; } else { current_polygon_border_indices.push_back( @@ -1496,9 +1494,9 @@ std::vector postProcess( [](const lanelet::ConstLineString3d & points, std::vector & bound) { for (const auto & bound_p : points) { const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); - if (bound.empty()) { - bound.push_back(cp); - } else if (autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + if ( + bound.empty() || + autoware::universe_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { bound.push_back(cp); } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index e65c67065bc54..33f46420ad537 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -80,7 +80,7 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG for (uint32_t i = 0; i < height; i++) { is_obstacle_table.at(i).resize(width); for (uint32_t j = 0; j < width; j++) { - const int cost = costmap_.data[i * width + j]; + const int cost = costmap_.data[i * width + j]; // NOLINT if (cost < 0 || param_.obstacle_threshold <= cost) { is_obstacle_table[i][j] = true; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 5ae62f51bdb52..b448ae50492ac 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -883,4 +883,18 @@ double calculateRoughDistanceToObjects( return min_distance; } +tier4_planning_msgs::msg::SafetyFactorArray to_safety_factor_array( + const CollisionCheckDebugMap & debug_map) +{ + tier4_planning_msgs::msg::SafetyFactorArray safety_factors; + for (const auto & [uuid, data] : debug_map) { + tier4_planning_msgs::msg::SafetyFactor safety_factor; + safety_factor.type = tier4_planning_msgs::msg::SafetyFactor::OBJECT; + safety_factor.is_safe = data.is_safe; + safety_factor.object_id = autoware::universe_utils::toUUIDMsg(uuid); + safety_factor.points.push_back(data.current_obj_pose.position); + safety_factors.factors.push_back(safety_factor); + } + return safety_factors; +} } // namespace autoware::behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/CHANGELOG.rst index 73fd1b4c7532a..1efdc5df429c1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_behavior_path_sampling_planner_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* Contributors: Fumiya Watanabe, Mamoru Sobue + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp index 28c310ae20ec2..f25c073e879da 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/manager.hpp @@ -38,7 +38,7 @@ class SamplingPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp index c9b75f7fa96de..f84fe45d08ca4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -86,7 +86,8 @@ class SamplingPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml index a5d78061448ed..d91c9d4c45138 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_sampling_planner_module - 0.40.0 + 0.41.0 The autoware_behavior_path_sampling_planner_module package Daniel Sanchez diff --git a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index fac3cc92de1d6..5bb57f17ce813 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -42,8 +42,9 @@ SamplingPlannerModule::SamplingPlannerModule( 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 + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { internal_params_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CHANGELOG.rst index 6ed62ffc8b913..e0f4874c5e986 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package autoware_behavior_path_side_shift_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* refactor(behavior_path_planner): common test functions (`#9963 `_) + * feat: common test code in behavior_path_planner + * deal with other modules + * fix typo + * update + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* Contributors: Fumiya Watanabe, Mamoru Sobue, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp index b495e6619a1c7..2b4a20ddc8cca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/manager.hpp @@ -39,7 +39,7 @@ class SideShiftModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp index 15d81ff45abcf..c8a5e47287477 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/include/autoware/behavior_path_side_shift_module/scene.hpp @@ -45,7 +45,8 @@ class SideShiftModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml index 9bc2b608381fa..5b40fe1d74169 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_side_shift_module - 0.40.0 + 0.41.0 The autoware_behavior_path_side_shift_module package Satoshi Ota diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp index 22ee7ab77e077..9536c1ba17336 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -42,8 +42,9 @@ SideShiftModule::SideShiftModule( 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 + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters} { } diff --git a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index c8661cdf1d912..c248a40eb151a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,11 +12,7 @@ // 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 "autoware/behavior_path_planner/test_utils.hpp" #include @@ -25,77 +21,16 @@ #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::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 autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_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); - - autoware::test_utils::updateNodeOptions( - node_options, - {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_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"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = + generateNode({"side_shift"}, {"autoware::behavior_path_planner::SideShiftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); @@ -113,7 +48,8 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = + generateNode({"side_shift"}, {"autoware::behavior_path_planner::SideShiftModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CHANGELOG.rst index 0cb96bc9b3bf4..dd4bcb5f51f29 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CHANGELOG.rst @@ -2,6 +2,41 @@ Changelog for package autoware_behavior_path_start_planner_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(start_planner): visualize planner evaluation table in rviz (`#10029 `_) + visualize planner evaluation table in rviz +* fix(start_planner, goal_planner): refactor lane departure checker initialization (`#9944 `_) +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* test(start_planner): disable GenerateValidFreespacePullOutPath test (`#9937 `_) +* test(autoware_behavior_path_start_planner_module): add test helper and implement unit tests for FreespacePullOut (`#9832 `_) + * refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners + * refactor(autoware_behavior_path_start_planner_module): remove TimeKeeper parameter from pull-out planners + * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling + * refactor(planner): add planner_data parameter to plan methods in pull out planners + * refactor(autoware_behavior_path_start_planner_module): remove LaneDepartureChecker dependency from pull-out planners + --------- +* refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling (`#9791 `_) + * refactor(lane_departure_checker): improve LaneDepartureChecker initialization and parameter handling + --------- +* refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners (`#9827 `_) + * refactor(autoware_behavior_path_start_planner_module): remove unnecessary time_keeper parameter from pull-out planners + --------- +* fix(behavior_path_planner): add freespace_planning_algorithms dependency (`#9800 `_) +* test(autoware_behavior_path_start_planner_module): add unit tests for shift shift pull out planner (`#9776 `_) + feat(behavior_path_planner): add unit tests for ShiftPullOut path planning +* refactor(autoware_behavior_path_start_planner_module): add data_structs.cpp and init method for StartPlannerParameters (`#9736 `_) + feat(autoware_behavior_path_start_planner_module): add data_structs.cpp and init method for StartPlannerParameters +* test(autoware_behavior_path_start_planner_module): add unit tests for geometric shift pull out planner (`#9640 `_) + * feat(behavior_path_planner): add unit tests for geometric pull-out planner and improve collision check + * feat(behavior_path_planner): add boolean parameter for divide_pull_out_path and update tests + --------- +* Contributors: Fumiya Watanabe, Kyoichi Sugahara, Mamoru Sobue, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt index 2da3051702103..5961b98bdda08 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -12,7 +12,29 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/freespace_pull_out.cpp src/geometric_pull_out.cpp src/shift_pull_out.cpp + src/data_structs.cpp src/util.cpp ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Add test helper library + ament_auto_add_library(start_planner_test_helper SHARED + test/src/start_planner_test_helper.cpp + ) + target_include_directories(start_planner_test_helper PUBLIC + test/include + ) + + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + ${TEST_SOURCES} + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} + start_planner_test_helper + ) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp index 9c439e9b3b921..4a2ca54e7d38c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp @@ -26,7 +26,6 @@ #include #include - namespace autoware::behavior_path_planner { @@ -51,29 +50,34 @@ struct PlannerDebugData { public: PlannerType planner_type; - std::vector conditions_evaluation; - double required_margin{0.0}; double backward_distance{0.0}; + double required_margin{0.0}; + std::vector conditions_evaluation; - auto header_str() const + static std::string double_to_str(double value, int precision = 1) { - std::stringstream ss; - ss << std::left << std::setw(20) << "| Planner type " << std::setw(20) << "| Required margin " - << std::setw(20) << "| Backward distance " << std::setw(25) << "| Condition evaluation |" - << "\n"; - return ss.str(); + std::ostringstream oss; + oss << std::fixed << std::setprecision(precision) << value; + return oss.str(); } - auto str() const + static std::string to_planner_type_name(PlannerType pt) { - std::stringstream ss; - for (const auto & result : conditions_evaluation) { - ss << std::left << std::setw(23) << magic_enum::enum_name(planner_type) << std::setw(23) - << (std::to_string(required_margin) + "[m]") << std::setw(23) - << (std::to_string(backward_distance) + "[m]") << std::setw(25) << result << "\n"; + // Adding whitespace for column width alignment in RViz display + switch (pt) { + case PlannerType::NONE: + return "NONE "; + case PlannerType::SHIFT: + return "SHIFT "; + case PlannerType::GEOMETRIC: + return "GEOMETRIC "; + case PlannerType::STOP: + return "STOP "; + case PlannerType::FREESPACE: + return "FREESPACE "; + default: + return "UNKNOWN"; } - ss << std::setw(40); - return ss.str(); } }; struct StartPlannerDebugData @@ -97,6 +101,7 @@ struct StartPlannerDebugData struct StartPlannerParameters { + static StartPlannerParameters init(rclcpp::Node & node); double th_arrived_distance{0.0}; double th_stopped_velocity{0.0}; double th_stopped_time{0.0}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp index e358e35ed7794..e1a7ae455e374 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -35,15 +35,16 @@ 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, - std::shared_ptr time_keeper); + FreespacePullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } std::optional plan( - const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) override; + const Pose & start_pose, const Pose & end_pose, + const std::shared_ptr & planner_data, + PlannerDebugData & planner_debug_data) override; + + friend class TestFreespacePullOut; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp index 70ca2bdb10d37..5d36c996cca3a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -33,18 +33,20 @@ class GeometricPullOut : public PullOutPlannerBase public: explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr - lane_departure_checker, - std::shared_ptr time_keeper); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan( const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; std::shared_ptr lane_departure_checker_; + + friend class TestGeometricPullOut; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp index 5d3d224673124..68b1e67e484c3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/manager.hpp @@ -38,7 +38,7 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp index f606679e7f2da..0316d6a14a4f9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -40,37 +40,35 @@ class PullOutPlannerBase public: explicit PullOutPlannerBase( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr time_keeper) - : time_keeper_(time_keeper) + std::shared_ptr time_keeper = + std::make_shared()) + : parameters_{parameters}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, + vehicle_footprint_{vehicle_info_.createFootprint()}, + time_keeper_(time_keeper) { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - vehicle_footprint_ = vehicle_info_.createFootprint(); - parameters_ = parameters; } virtual ~PullOutPlannerBase() = default; - void setPlannerData(const std::shared_ptr & planner_data) - { - planner_data_ = planner_data; - } - void setCollisionCheckMargin(const double collision_check_margin) { collision_check_margin_ = collision_check_margin; }; virtual PlannerType getPlannerType() const = 0; virtual std::optional plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) = 0; + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, + PlannerDebugData & planner_debug_data) = 0; protected: bool isPullOutPathCollided( autoware::behavior_path_planner::PullOutPath & pull_out_path, + const std::shared_ptr & planner_data, double collision_check_distance_from_end) const; - std::shared_ptr planner_data_; + StartPlannerParameters parameters_; autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; - StartPlannerParameters parameters_; double collision_check_margin_; mutable std::shared_ptr time_keeper_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp index d1b4c9dabdd7f..1000437bdea56 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/shift_pull_out.hpp @@ -35,17 +35,19 @@ class ShiftPullOut : public PullOutPlannerBase public: explicit ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, - std::shared_ptr time_keeper); + std::shared_ptr time_keeper = + std::make_shared()); PlannerType getPlannerType() const override { return PlannerType::SHIFT; }; std::optional plan( const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose); + const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & behavior_path_parameters); double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); @@ -56,6 +58,8 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr lane_departure_checker_; + friend class TestShiftPullOut; + private: // Calculate longitudinal distance based on the acceleration limit, curvature limit, and the // minimum distance requirement. diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp index 47eecb54e2b2a..3978738798386 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp @@ -26,11 +26,12 @@ #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 "data_structs.hpp" -#include #include #include +#include #include #include @@ -51,9 +52,9 @@ using autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilter 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 PriorityOrder = std::vector>>; +using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped; struct PullOutStatus { @@ -88,7 +89,8 @@ class StartPlannerModule : public SceneModuleInterface const std::shared_ptr & parameters, const std::unordered_map> & rtc_interface_ptr_map, std::unordered_map> & - objects_of_interest_marker_interface_ptr_map); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface); ~StartPlannerModule() override { @@ -147,16 +149,16 @@ class StartPlannerModule : public SceneModuleInterface } void resetStatus(); - void acceptVisitor( - [[maybe_unused]] const std::shared_ptr & visitor) const override - { - } + void acceptVisitor(const std::shared_ptr & visitor) const override; // Condition to disable simultaneous execution bool isDrivingForward() const { return status_.driving_forward; } bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; } + std::string get_planner_evaluation_table() const { return planner_evaluation_table_; } + private: + friend class SceneModuleVisitor; struct StartPlannerData { StartPlannerParameters parameters; @@ -200,18 +202,18 @@ class StartPlannerModule : public SceneModuleInterface bool requiresDynamicObjectsCollisionDetection() const; - uint16_t getSteeringFactorDirection( + uint16_t getPlanningFactorDirection( const autoware::behavior_path_planner::BehaviorModuleOutput & output) const { switch (output.turn_signal_info.turn_signal.command) { case TurnIndicatorsCommand::ENABLE_LEFT: - return SteeringFactor::LEFT; + return PlanningFactor::SHIFT_LEFT; case TurnIndicatorsCommand::ENABLE_RIGHT: - return SteeringFactor::RIGHT; + return PlanningFactor::SHIFT_RIGHT; default: - return SteeringFactor::STRAIGHT; + return PlanningFactor::NONE; } }; @@ -284,6 +286,7 @@ ego pose. std::vector> start_planners_; PullOutStatus status_; mutable StartPlannerDebugData debug_data_; + std::string planner_evaluation_table_; // Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this // module's output. If the ego vehicle is in this lanelet, the calculation is skipped. @@ -310,8 +313,6 @@ ego pose. std::vector searchPullOutStartPoseCandidates( const PathWithLaneId & back_path_from_start_pose) const; - std::shared_ptr lane_departure_checker_; - // turn signal TurnSignalInfo calcTurnSignalInfo(); @@ -355,6 +356,10 @@ ego pose. const StartPlannerParameters & parameters, const std::shared_ptr & planner_data, const PullOutStatus & pull_out_status); + std::string create_planner_evaluation_table( + const std::vector & planner_debug_data_vector) const; + void set_planner_evaluation_table(const std::vector & debug_data_vector); + void setDebugData(); void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const; }; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml index c7cabb403f164..c31707a413c6a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_start_planner_module - 0.40.0 + 0.41.0 The autoware_behavior_path_start_planner_module package Kosuke Takeuchi @@ -22,6 +22,7 @@ autoware_behavior_path_planner autoware_behavior_path_planner_common + autoware_freespace_planning_algorithms autoware_motion_utils autoware_rtc_interface autoware_universe_utils diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp new file mode 100644 index 0000000000000..f613b9345ce42 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/data_structs.cpp @@ -0,0 +1,340 @@ +// 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_start_planner_module/data_structs.hpp" + +#include "autoware/behavior_path_start_planner_module/manager.hpp" + +#include +#include + +#include + +namespace autoware::behavior_path_planner +{ + +StartPlannerParameters StartPlannerParameters::init(rclcpp::Node & node) +{ + using autoware::universe_utils::getOrDeclareParameter; + StartPlannerParameters p; + { + const std::string ns = "start_planner."; + + p.th_arrived_distance = getOrDeclareParameter(node, ns + "th_arrived_distance"); + p.th_stopped_velocity = getOrDeclareParameter(node, ns + "th_stopped_velocity"); + p.th_stopped_time = getOrDeclareParameter(node, ns + "th_stopped_time"); + p.prepare_time_before_start = + getOrDeclareParameter(node, ns + "prepare_time_before_start"); + p.th_distance_to_middle_of_the_road = + getOrDeclareParameter(node, ns + "th_distance_to_middle_of_the_road"); + p.skip_rear_vehicle_check = getOrDeclareParameter(node, ns + "skip_rear_vehicle_check"); + p.extra_width_margin_for_rear_obstacle = + getOrDeclareParameter(node, ns + "extra_width_margin_for_rear_obstacle"); + p.collision_check_margins = + getOrDeclareParameter>(node, ns + "collision_check_margins"); + p.collision_check_margin_from_front_object = + getOrDeclareParameter(node, ns + "collision_check_margin_from_front_object"); + p.th_moving_object_velocity = + getOrDeclareParameter(node, ns + "th_moving_object_velocity"); + p.center_line_path_interval = + getOrDeclareParameter(node, ns + "center_line_path_interval"); + // shift pull out + p.enable_shift_pull_out = getOrDeclareParameter(node, ns + "enable_shift_pull_out"); + p.check_shift_path_lane_departure = + getOrDeclareParameter(node, ns + "check_shift_path_lane_departure"); + p.allow_check_shift_path_lane_departure_override = + getOrDeclareParameter(node, ns + "allow_check_shift_path_lane_departure_override"); + p.shift_collision_check_distance_from_end = + getOrDeclareParameter(node, ns + "shift_collision_check_distance_from_end"); + p.minimum_shift_pull_out_distance = + getOrDeclareParameter(node, ns + "minimum_shift_pull_out_distance"); + p.lateral_acceleration_sampling_num = + getOrDeclareParameter(node, ns + "lateral_acceleration_sampling_num"); + p.lateral_jerk = getOrDeclareParameter(node, ns + "lateral_jerk"); + p.maximum_lateral_acc = getOrDeclareParameter(node, ns + "maximum_lateral_acc"); + p.minimum_lateral_acc = getOrDeclareParameter(node, ns + "minimum_lateral_acc"); + p.maximum_curvature = getOrDeclareParameter(node, ns + "maximum_curvature"); + p.end_pose_curvature_threshold = + getOrDeclareParameter(node, ns + "end_pose_curvature_threshold"); + p.maximum_longitudinal_deviation = + getOrDeclareParameter(node, ns + "maximum_longitudinal_deviation"); + // geometric pull out + p.enable_geometric_pull_out = + getOrDeclareParameter(node, ns + "enable_geometric_pull_out"); + p.geometric_collision_check_distance_from_end = + getOrDeclareParameter(node, ns + "geometric_collision_check_distance_from_end"); + p.divide_pull_out_path = getOrDeclareParameter(node, ns + "divide_pull_out_path"); + p.parallel_parking_parameters.pull_out_velocity = + getOrDeclareParameter(node, ns + "geometric_pull_out_velocity"); + p.parallel_parking_parameters.pull_out_arc_path_interval = + getOrDeclareParameter(node, ns + "arc_path_interval"); + p.parallel_parking_parameters.pull_out_lane_departure_margin = + getOrDeclareParameter(node, ns + "lane_departure_margin"); + p.lane_departure_check_expansion_margin = + getOrDeclareParameter(node, ns + "lane_departure_check_expansion_margin"); + p.parallel_parking_parameters.pull_out_max_steer_angle = + getOrDeclareParameter(node, 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 = getOrDeclareParameter( + node, + ns + "search_priority"); // "efficient_path" or "short_back_distance" + p.enable_back = getOrDeclareParameter(node, ns + "enable_back"); + p.backward_velocity = getOrDeclareParameter(node, ns + "backward_velocity"); + p.max_back_distance = getOrDeclareParameter(node, ns + "max_back_distance"); + p.backward_search_resolution = + getOrDeclareParameter(node, ns + "backward_search_resolution"); + p.backward_path_update_duration = + getOrDeclareParameter(node, ns + "backward_path_update_duration"); + p.ignore_distance_from_lane_end = + getOrDeclareParameter(node, ns + "ignore_distance_from_lane_end"); + // stop condition + p.maximum_deceleration_for_stop = + getOrDeclareParameter(node, ns + "stop_condition.maximum_deceleration_for_stop"); + p.maximum_jerk_for_stop = + getOrDeclareParameter(node, ns + "stop_condition.maximum_jerk_for_stop"); + } + { + const std::string ns = "start_planner.object_types_to_check_for_path_generation."; + p.object_types_to_check_for_path_generation.check_car = + getOrDeclareParameter(node, ns + "check_car"); + p.object_types_to_check_for_path_generation.check_truck = + getOrDeclareParameter(node, ns + "check_truck"); + p.object_types_to_check_for_path_generation.check_bus = + getOrDeclareParameter(node, ns + "check_bus"); + p.object_types_to_check_for_path_generation.check_trailer = + getOrDeclareParameter(node, ns + "check_trailer"); + p.object_types_to_check_for_path_generation.check_unknown = + getOrDeclareParameter(node, ns + "check_unknown"); + p.object_types_to_check_for_path_generation.check_bicycle = + getOrDeclareParameter(node, ns + "check_bicycle"); + p.object_types_to_check_for_path_generation.check_motorcycle = + getOrDeclareParameter(node, ns + "check_motorcycle"); + p.object_types_to_check_for_path_generation.check_pedestrian = + getOrDeclareParameter(node, ns + "check_pedestrian"); + } + // freespace planner general params + { + const std::string ns = "start_planner.freespace_planner."; + p.enable_freespace_planner = getOrDeclareParameter(node, ns + "enable_freespace_planner"); + p.freespace_planner_algorithm = + getOrDeclareParameter(node, ns + "freespace_planner_algorithm"); + p.end_pose_search_start_distance = + getOrDeclareParameter(node, ns + "end_pose_search_start_distance"); + p.end_pose_search_end_distance = + getOrDeclareParameter(node, ns + "end_pose_search_end_distance"); + p.end_pose_search_interval = + getOrDeclareParameter(node, ns + "end_pose_search_interval"); + p.freespace_planner_velocity = getOrDeclareParameter(node, ns + "velocity"); + p.vehicle_shape_margin = getOrDeclareParameter(node, ns + "vehicle_shape_margin"); + p.freespace_planner_common_parameters.time_limit = + getOrDeclareParameter(node, ns + "time_limit"); + p.freespace_planner_common_parameters.max_turning_ratio = + getOrDeclareParameter(node, ns + "max_turning_ratio"); + p.freespace_planner_common_parameters.turning_steps = + getOrDeclareParameter(node, ns + "turning_steps"); + } + // freespace planner search config + { + const std::string ns = "start_planner.freespace_planner.search_configs."; + p.freespace_planner_common_parameters.theta_size = + getOrDeclareParameter(node, ns + "theta_size"); + p.freespace_planner_common_parameters.angle_goal_range = + getOrDeclareParameter(node, ns + "angle_goal_range"); + p.freespace_planner_common_parameters.curve_weight = + getOrDeclareParameter(node, ns + "curve_weight"); + p.freespace_planner_common_parameters.reverse_weight = + getOrDeclareParameter(node, ns + "reverse_weight"); + p.freespace_planner_common_parameters.lateral_goal_range = + getOrDeclareParameter(node, ns + "lateral_goal_range"); + p.freespace_planner_common_parameters.longitudinal_goal_range = + getOrDeclareParameter(node, 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 = + getOrDeclareParameter(node, ns + "obstacle_threshold"); + } + // freespace planner astar + { + const std::string ns = "start_planner.freespace_planner.astar."; + p.astar_parameters.search_method = + getOrDeclareParameter(node, ns + "search_method"); + p.astar_parameters.only_behind_solutions = + getOrDeclareParameter(node, ns + "only_behind_solutions"); + p.astar_parameters.use_back = getOrDeclareParameter(node, ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + getOrDeclareParameter(node, ns + "distance_heuristic_weight"); + } + // freespace planner rrtstar + { + const std::string ns = "start_planner.freespace_planner.rrtstar."; + p.rrt_star_parameters.enable_update = getOrDeclareParameter(node, ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + getOrDeclareParameter(node, ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = + getOrDeclareParameter(node, ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = + getOrDeclareParameter(node, ns + "neighbor_radius"); + p.rrt_star_parameters.margin = getOrDeclareParameter(node, ns + "margin"); + } + + 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 = + getOrDeclareParameter(node, ego_path_ns + "min_velocity"); + p.ego_predicted_path_params.acceleration = + getOrDeclareParameter(node, ego_path_ns + "min_acceleration"); + p.ego_predicted_path_params.time_horizon_for_front_object = + getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + getOrDeclareParameter(node, ego_path_ns + "time_horizon_for_rear_object"); + p.ego_predicted_path_params.time_resolution = + getOrDeclareParameter(node, ego_path_ns + "time_resolution"); + p.ego_predicted_path_params.delay_until_departure = + getOrDeclareParameter(node, 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 = + getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_horizon"); + p.objects_filtering_params.safety_check_time_resolution = + getOrDeclareParameter(node, obj_filter_ns + "safety_check_time_resolution"); + p.objects_filtering_params.object_check_forward_distance = + getOrDeclareParameter(node, obj_filter_ns + "object_check_forward_distance"); + p.objects_filtering_params.object_check_backward_distance = + getOrDeclareParameter(node, obj_filter_ns + "object_check_backward_distance"); + p.objects_filtering_params.ignore_object_velocity_threshold = + getOrDeclareParameter(node, obj_filter_ns + "ignore_object_velocity_threshold"); + p.objects_filtering_params.include_opposite_lane = + getOrDeclareParameter(node, obj_filter_ns + "include_opposite_lane"); + p.objects_filtering_params.invert_opposite_lane = + getOrDeclareParameter(node, obj_filter_ns + "invert_opposite_lane"); + p.objects_filtering_params.check_all_predicted_path = + getOrDeclareParameter(node, obj_filter_ns + "check_all_predicted_path"); + p.objects_filtering_params.use_all_predicted_path = + getOrDeclareParameter(node, obj_filter_ns + "use_all_predicted_path"); + p.objects_filtering_params.use_predicted_path_outside_lanelet = + getOrDeclareParameter(node, 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 = + getOrDeclareParameter(node, obj_types_ns + "check_car"); + p.objects_filtering_params.object_types_to_check.check_truck = + getOrDeclareParameter(node, obj_types_ns + "check_truck"); + p.objects_filtering_params.object_types_to_check.check_bus = + getOrDeclareParameter(node, obj_types_ns + "check_bus"); + p.objects_filtering_params.object_types_to_check.check_trailer = + getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + p.objects_filtering_params.object_types_to_check.check_unknown = + getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + p.objects_filtering_params.object_types_to_check.check_bicycle = + getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + p.objects_filtering_params.object_types_to_check.check_motorcycle = + getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + p.objects_filtering_params.object_types_to_check.check_pedestrian = + getOrDeclareParameter(node, 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 = + getOrDeclareParameter(node, obj_lane_ns + "check_current_lane"); + p.objects_filtering_params.object_lane_configuration.check_right_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_right_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_left_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_left_side_lane"); + p.objects_filtering_params.object_lane_configuration.check_shoulder_lane = + getOrDeclareParameter(node, obj_lane_ns + "check_shoulder_lane"); + p.objects_filtering_params.object_lane_configuration.check_other_lane = + getOrDeclareParameter(node, 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 = + getOrDeclareParameter(node, safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + getOrDeclareParameter(node, safety_check_ns + "hysteresis_factor_expand_rate"); + p.safety_check_params.backward_path_length = + getOrDeclareParameter(node, safety_check_ns + "backward_path_length"); + p.safety_check_params.forward_path_length = + getOrDeclareParameter(node, safety_check_ns + "forward_path_length"); + p.safety_check_params.publish_debug_marker = + getOrDeclareParameter(node, safety_check_ns + "publish_debug_marker"); + p.safety_check_params.collision_check_yaw_diff_threshold = + getOrDeclareParameter(node, safety_check_ns + "collision_check_yaw_diff_threshold"); + } + // RSSparams + { + const std::string rss_ns = safety_check_ns + "rss_params."; + p.safety_check_params.rss_params.rear_vehicle_reaction_time = + getOrDeclareParameter(node, rss_ns + "rear_vehicle_reaction_time"); + p.safety_check_params.rss_params.rear_vehicle_safety_time_margin = + getOrDeclareParameter(node, rss_ns + "rear_vehicle_safety_time_margin"); + p.safety_check_params.rss_params.lateral_distance_max_threshold = + getOrDeclareParameter(node, rss_ns + "lateral_distance_max_threshold"); + p.safety_check_params.rss_params.longitudinal_distance_min_threshold = + getOrDeclareParameter(node, rss_ns + "longitudinal_distance_min_threshold"); + p.safety_check_params.rss_params.longitudinal_velocity_delta_time = + getOrDeclareParameter(node, rss_ns + "longitudinal_velocity_delta_time"); + p.safety_check_params.rss_params.extended_polygon_policy = + getOrDeclareParameter(node, rss_ns + "extended_polygon_policy"); + } + // surround moving obstacle check + { + const std::string surround_moving_obstacle_check_ns = + "start_planner.surround_moving_obstacle_check."; + p.search_radius = + getOrDeclareParameter(node, surround_moving_obstacle_check_ns + "search_radius"); + p.th_moving_obstacle_velocity = getOrDeclareParameter( + node, surround_moving_obstacle_check_ns + "th_moving_obstacle_velocity"); + // ObjectTypesToCheck + { + const std::string obj_types_ns = surround_moving_obstacle_check_ns + "object_types_to_check."; + p.surround_moving_obstacles_type_to_check.check_car = + getOrDeclareParameter(node, obj_types_ns + "check_car"); + p.surround_moving_obstacles_type_to_check.check_truck = + getOrDeclareParameter(node, obj_types_ns + "check_truck"); + p.surround_moving_obstacles_type_to_check.check_bus = + getOrDeclareParameter(node, obj_types_ns + "check_bus"); + p.surround_moving_obstacles_type_to_check.check_trailer = + getOrDeclareParameter(node, obj_types_ns + "check_trailer"); + p.surround_moving_obstacles_type_to_check.check_unknown = + getOrDeclareParameter(node, obj_types_ns + "check_unknown"); + p.surround_moving_obstacles_type_to_check.check_bicycle = + getOrDeclareParameter(node, obj_types_ns + "check_bicycle"); + p.surround_moving_obstacles_type_to_check.check_motorcycle = + getOrDeclareParameter(node, obj_types_ns + "check_motorcycle"); + p.surround_moving_obstacles_type_to_check.check_pedestrian = + getOrDeclareParameter(node, obj_types_ns + "check_pedestrian"); + } + } + + // debug + { + const std::string debug_ns = "start_planner.debug."; + p.print_debug_info = getOrDeclareParameter(node, debug_ns + "print_debug_info"); + } + + return p; +} +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index 42698f799c6b3..6893c7d11d09d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -28,15 +28,11 @@ namespace autoware::behavior_path_planner { -FreespacePullOut::FreespacePullOut( - rclcpp::Node & node, const StartPlannerParameters & parameters, - const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, - std::shared_ptr time_keeper) -: PullOutPlannerBase{node, parameters, time_keeper}, - velocity_{parameters.freespace_planner_velocity} +FreespacePullOut::FreespacePullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) +: PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} { autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( - vehicle_info, parameters.vehicle_shape_margin); + vehicle_info_, parameters.vehicle_shape_margin); if (parameters.freespace_planner_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; planner_ = std::make_unique( @@ -51,13 +47,14 @@ FreespacePullOut::FreespacePullOut( } std::optional FreespacePullOut::plan( - const Pose & start_pose, const Pose & end_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & end_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - const auto & route_handler = planner_data_->route_handler; - const double backward_path_length = planner_data_->parameters.backward_path_length; - const double forward_path_length = planner_data_->parameters.forward_path_length; + const auto & route_handler = planner_data->route_handler; + const double backward_path_length = planner_data->parameters.backward_path_length; + const double forward_path_length = planner_data->parameters.forward_path_length; - planner_->setMap(*planner_data_->costmap); + planner_->setMap(*planner_data->costmap); try { if (!planner_->makePlan(start_pose, end_pose)) { @@ -69,11 +66,11 @@ std::optional FreespacePullOut::plan( } const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths const auto pull_out_lanes = - start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + start_planner_utils::getPullOutLanes(planner_data, backward_path_length); const auto lanes = utils::combineLanelets(road_lanes, pull_out_lanes); const PathWithLaneId path = diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index bbc6c05725434..ed09e0c0a33d6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -36,35 +36,39 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr - lane_departure_checker, std::shared_ptr time_keeper) : PullOutPlannerBase{node, parameters, time_keeper}, - parallel_parking_parameters_{parameters.parallel_parking_parameters}, - lane_departure_checker_(lane_departure_checker) + parallel_parking_parameters_{parameters.parallel_parking_parameters} { + auto lane_departure_checker_params = autoware::lane_departure_checker::Param{}; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + lane_departure_checker_ = + std::make_shared( + lane_departure_checker_params, vehicle_info_); planner_.setParameters(parallel_parking_parameters_); } std::optional GeometricPullOut::plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { PullOutPath output; // combine road lane and pull out lane const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + planner_data->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); - const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); + const auto pull_out_lanes = getPullOutLanes(planner_data, backward_path_length); // check if the ego is at left or right side of road lane center const bool left_side_start = 0 < getArcCoordinates(road_lanes, start_pose).distance; planner_.setTurningRadius( - planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); - planner_.setPlannerData(planner_data_); + planner_data->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); + planner_.setPlannerData(planner_data); const bool found_valid_path = planner_.planPullOut( start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_); if (!found_valid_path) { @@ -122,7 +126,8 @@ std::optional GeometricPullOut::plan( output.start_pose = planner_.getArcPaths().at(0).points.front().point.pose; output.end_pose = planner_.getArcPaths().at(1).points.back().point.pose; - if (isPullOutPathCollided(output, parameters_.geometric_collision_check_distance_from_end)) { + if (isPullOutPathCollided( + output, planner_data, parameters_.geometric_collision_check_distance_from_end)) { planner_debug_data.conditions_evaluation.emplace_back("collision"); return {}; } diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp index f5739ce322248..9ea51d56ee1cc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -29,318 +29,18 @@ 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_distance_to_middle_of_the_road = - node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); - p.skip_rear_vehicle_check = node->declare_parameter(ns + "skip_rear_vehicle_check"); - 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"); - 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.end_pose_curvature_threshold = - node->declare_parameter(ns + "end_pose_curvature_threshold"); - 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_arc_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"); - // 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 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"); - } - // 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.max_turning_ratio = - node->declare_parameter(ns + "max_turning_ratio"); - p.freespace_planner_common_parameters.turning_steps = - node->declare_parameter(ns + "turning_steps"); - } - // 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.search_method = node->declare_parameter(ns + "search_method"); - 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"); - } - - 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"); - p.safety_check_params.collision_check_yaw_diff_threshold = - node->declare_parameter(safety_check_ns + "collision_check_yaw_diff_threshold"); - } - // 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"); - p.safety_check_params.rss_params.extended_polygon_policy = - node->declare_parameter(rss_ns + "extended_polygon_policy"); - } - // surround moving obstacle check - { - const std::string surround_moving_obstacle_check_ns = - "start_planner.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 - { - const 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 - { - const std::string debug_ns = "start_planner.debug."; - p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); - } - + StartPlannerParameters parameters = StartPlannerParameters::init(*node); // validation of parameters - if (p.lateral_acceleration_sampling_num < 1) { + if (parameters.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 + << parameters.lateral_acceleration_sampling_num << std::endl << "Terminating the program..."); exit(EXIT_FAILURE); } - parameters_ = std::make_shared(p); + parameters_ = std::make_shared(parameters); } void StartPlannerModuleManager::updateModuleParams( diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp index f7713ea2e91b2..a475b3ad582cb 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/pull_out_planner_base.cpp @@ -14,18 +14,24 @@ #include "autoware/behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include + namespace autoware::behavior_path_planner { bool PullOutPlannerBase::isPullOutPathCollided( autoware::behavior_path_planner::PullOutPath & pull_out_path, + const std::shared_ptr & planner_data, double collision_check_distance_from_end) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); // check for collisions - const auto & dynamic_objects = planner_data_->dynamic_object; + const auto & dynamic_objects = planner_data->dynamic_object; + if (!dynamic_objects) { + return false; + } const auto pull_out_lanes = start_planner_utils::getPullOutLanes( - planner_data_, planner_data_->parameters.backward_path_length + parameters_.max_back_distance); + planner_data, planner_data->parameters.backward_path_length + parameters_.max_back_distance); // extract stop objects in pull out lane for collision check const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity( *dynamic_objects, parameters_.th_moving_object_velocity); diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 4759559619870..16a3d4ffd842a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -41,31 +41,38 @@ using start_planner_utils::getPullOutLanes; ShiftPullOut::ShiftPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - std::shared_ptr & lane_departure_checker, std::shared_ptr time_keeper) -: PullOutPlannerBase{node, parameters, time_keeper}, lane_departure_checker_{lane_departure_checker} +: PullOutPlannerBase{node, parameters, time_keeper} { + autoware::lane_departure_checker::Param lane_departure_checker_params; + lane_departure_checker_params.footprint_extra_margin = + parameters.lane_departure_check_expansion_margin; + lane_departure_checker_ = + std::make_shared( + lane_departure_checker_params, vehicle_info_, time_keeper_); } std::optional ShiftPullOut::plan( - const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) { - const auto & route_handler = planner_data_->route_handler; - const auto & common_parameters = planner_data_->parameters; + const auto & route_handler = planner_data->route_handler; + const auto & common_parameters = planner_data->parameters; const double backward_path_length = - planner_data_->parameters.backward_path_length + parameters_.max_back_distance; + planner_data->parameters.backward_path_length + parameters_.max_back_distance; const auto road_lanes = utils::getExtendedCurrentLanes( - planner_data_, backward_path_length, std::numeric_limits::max(), + planner_data, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths - auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); + auto pull_out_paths = + calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose, common_parameters); if (pull_out_paths.empty()) { planner_debug_data.conditions_evaluation.emplace_back("no path found"); return std::nullopt; } - const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + const auto lanelet_map_ptr = planner_data->route_handler->getLaneletMapPtr(); std::vector fused_id_start_to_end{}; std::optional fused_polygon_start_to_end = std::nullopt; @@ -159,9 +166,10 @@ std::optional ShiftPullOut::plan( continue; } shift_path.points = cropped_path.points; - shift_path.header = planner_data_->route_handler->getRouteHeader(); + shift_path.header = planner_data->route_handler->getRouteHeader(); - if (isPullOutPathCollided(pull_out_path, parameters_.shift_collision_check_distance_from_end)) { + if (isPullOutPathCollided( + pull_out_path, planner_data, parameters_.shift_collision_check_distance_from_end)) { planner_debug_data.conditions_evaluation.emplace_back("collision"); continue; } @@ -227,7 +235,8 @@ bool ShiftPullOut::refineShiftedPathToStartPose( std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose) + const Pose & start_pose, const Pose & goal_pose, + const BehaviorPathPlannerParameters & behavior_path_parameters) { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -238,9 +247,8 @@ std::vector ShiftPullOut::calcPullOutPaths( } // rename parameter - const auto & common_parameters = planner_data_->parameters; - const double forward_path_length = common_parameters.forward_path_length; - const double backward_path_length = common_parameters.backward_path_length; + const double forward_path_length = behavior_path_parameters.forward_path_length; + const double backward_path_length = behavior_path_parameters.backward_path_length; const double lateral_jerk = parameters_.lateral_jerk; const double minimum_lateral_acc = parameters_.minimum_lateral_acc; const double maximum_lateral_acc = parameters_.maximum_lateral_acc; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index c223390e454d1..5ad519af20cad 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -62,36 +62,26 @@ StartPlannerModule::StartPlannerModule( 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 + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT parameters_{parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { - lane_departure_checker_ = std::make_shared(time_keeper_); - lane_departure_checker_->setVehicleInfo(vehicle_info_); - autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_params.footprint_extra_margin = - parameters->lane_departure_check_expansion_margin; - - lane_departure_checker_->setParam(lane_departure_checker_params); - // set enabled planner if (parameters_->enable_shift_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, time_keeper_)); } if (parameters_->enable_geometric_pull_out) { - start_planners_.push_back( - std::make_shared(node, *parameters, lane_departure_checker_, time_keeper_)); + start_planners_.push_back(std::make_shared(node, *parameters, time_keeper_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } if (parameters_->enable_freespace_planner) { - freespace_planner_ = - std::make_unique(node, *parameters, vehicle_info_, time_keeper_); + freespace_planner_ = std::make_unique(node, *parameters); const auto freespace_planner_period_ns = rclcpp::Rate(1.0).period(); freespace_planner_timer_cb_group_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -100,9 +90,6 @@ StartPlannerModule::StartPlannerModule( std::bind(&StartPlannerModule::onFreespacePlannerTimer, this), freespace_planner_timer_cb_group_); } - - steering_factor_interface_.init(PlanningBehavior::START_PLANNER); - velocity_factor_interface_.init(PlanningBehavior::START_PLANNER); } void StartPlannerModule::onFreespacePlannerTimer() @@ -743,9 +730,9 @@ BehaviorModuleOutput StartPlannerModule::plan() setDrivableAreaInfo(output); - setVelocityFactor(output.path); + set_longitudinal_planning_factor(output.path); - const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -755,9 +742,9 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, status_.pull_out_path.start_pose, + status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); setDebugData(); return output; } @@ -765,9 +752,9 @@ BehaviorModuleOutput StartPlannerModule::plan() path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, + planning_factor_direction, SafetyFactorArray{}); setDebugData(); @@ -850,7 +837,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() setDrivableAreaInfo(output); - const auto steering_factor_direction = getSteeringFactorDirection(output); + const auto planning_factor_direction = getPlanningFactorDirection(output); if (status_.driving_forward) { const double start_distance = autoware::motion_utils::calcSignedArcLength( @@ -860,10 +847,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.end_pose.position); updateRTCStatus(start_distance, finish_distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, steering_factor_direction, SteeringFactor::APPROACHING, - ""); + planning_factor_interface_->add( + start_distance, finish_distance, status_.pull_out_path.start_pose, + status_.pull_out_path.end_pose, planning_factor_direction, SafetyFactorArray{}); setDebugData(); return output; @@ -872,9 +858,9 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() stop_path.points, planner_data_->self_odometry->pose.pose.position, status_.pull_out_path.start_pose.position); updateRTCStatus(0.0, distance); - steering_factor_interface_.set( - {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + planning_factor_interface_->add( + 0.0, distance, status_.pull_out_path.start_pose, status_.pull_out_path.end_pose, + planning_factor_direction, SafetyFactorArray{}); setDebugData(); @@ -908,16 +894,6 @@ void StartPlannerModule::planWithPriority( if (start_pose_candidates.empty()) return; - auto get_accumulated_debug_stream = [](const std::vector & debug_data_vector) { - std::stringstream ss; - if (debug_data_vector.empty()) return ss; - ss << debug_data_vector.front().header_str(); - for (const auto & debug_data : debug_data_vector) { - ss << debug_data.str(); - } - return ss; - }; - const PriorityOrder order_priority = determinePriorityOrder(search_priority, start_pose_candidates.size()); @@ -932,20 +908,13 @@ void StartPlannerModule::planWithPriority( collision_check_margin, debug_data_vector)) { debug_data_.selected_start_pose_candidate_index = index; debug_data_.margin_for_start_pose_candidate = collision_check_margin; - if (parameters_->print_debug_info) { - const auto ss = get_accumulated_debug_stream(debug_data_vector); - DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str()); - } + set_planner_evaluation_table(debug_data_vector); return; } } } } - - if (parameters_->print_debug_info) { - const auto ss = get_accumulated_debug_stream(debug_data_vector); - DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str()); - } + set_planner_evaluation_table(debug_data_vector); updateStatusIfNoSafePathFound(); } @@ -986,17 +955,16 @@ bool StartPlannerModule::findPullOutPath( const bool backward_is_unnecessary = backwards_distance < epsilon; planner->setCollisionCheckMargin(collision_check_margin); - planner->setPlannerData(planner_data_); PlannerDebugData debug_data{ - planner->getPlannerType(), {}, collision_check_margin, backwards_distance}; + planner->getPlannerType(), backwards_distance, collision_check_margin, {}}; - const auto pull_out_path = planner->plan(start_pose_candidate, goal_pose, debug_data); + const auto pull_out_path = + planner->plan(start_pose_candidate, goal_pose, planner_data_, debug_data); debug_data_vector.push_back(debug_data); // If no path is found, return false if (!pull_out_path) { return false; } - if (backward_is_unnecessary) { updateStatusWithCurrentPath(*pull_out_path, start_pose_candidate, planner->getPlannerType()); return true; @@ -1595,12 +1563,9 @@ std::optional StartPlannerModule::planFreespacePath( for (const auto & p : center_line_path.points) { const Pose end_pose = p.point.pose; - freespace_planner_->setPlannerData(planner_data); - PlannerDebugData debug_data{freespace_planner_->getPlannerType(), {}, 0.0, 0.0}; - auto freespace_path = freespace_planner_->plan(current_pose, end_pose, debug_data); - DEBUG_PRINT( - "\nFreespace Pull out path search results\n%s%s", debug_data.header_str().c_str(), - debug_data.str().c_str()); + PlannerDebugData debug_data{freespace_planner_->getPlannerType(), 0.0, 0.0, {}}; + auto freespace_path = + freespace_planner_->plan(current_pose, end_pose, planner_data, debug_data); if (!freespace_path) { continue; } @@ -1643,6 +1608,83 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } +std::string StartPlannerModule::create_planner_evaluation_table( + const std::vector & planner_debug_data_vector) const +{ + if (planner_debug_data_vector.empty()) { + return ""; + } + + const std::string header_planner_type = "Planner type "; + const std::string header_required_margin = "Required margin [m]"; + const std::string header_backward_distance = "Backward distance [m]"; + const std::string header_condition_eval = "Condition"; + + std::ostringstream oss; + oss << "-----------------------------------------------------------------------------------------" + "----------------" + << "\n"; + oss << "| " << std::left << header_planner_type << " | " << header_required_margin << " | " + << header_backward_distance << " | " << header_condition_eval << " \n"; + oss << "-----------------------------------------------------------------------------------------" + "----------------" + << "\n"; + + for (const auto & d : planner_debug_data_vector) { + const std::string pt_str = PlannerDebugData::to_planner_type_name(d.planner_type); + const std::string rm_str = + PlannerDebugData::double_to_str(d.required_margin, 1) + " "; + const std::string bd_str = PlannerDebugData::double_to_str(d.backward_distance, 1) + + " "; + + if (d.conditions_evaluation.empty()) { + oss << "| " << std::left << pt_str << " | " << rm_str << " | " << bd_str << " | " + << "Unexpected empty condition evaluation" + << " \n"; + } else { + for (size_t i = 0; i < d.conditions_evaluation.size(); ++i) { + const std::string cond_with_index = + "#" + std::to_string(i + 1) + ": " + d.conditions_evaluation[i]; + + oss << "| " << std::left << pt_str << " | " << rm_str << " | " << bd_str << " | " + << cond_with_index << " \n"; + } + } + } + + return oss.str(); +} + +void StartPlannerModule::set_planner_evaluation_table( + const std::vector & debug_data_vector) +{ + planner_evaluation_table_.clear(); + if (debug_data_vector.empty()) { + return; + } + + planner_evaluation_table_ = create_planner_evaluation_table(debug_data_vector); +} + +void StartPlannerModule::acceptVisitor(const std::shared_ptr & visitor) const +{ + if (visitor) { + visitor->visitStartPlannerModule(this); + } +} + +void SceneModuleVisitor::visitStartPlannerModule(const StartPlannerModule * module) const +{ + auto debug_msg = std::make_shared(); + auto debug_info = module->get_planner_evaluation_table(); + if (debug_info.empty()) return; + + debug_msg->stamp = rclcpp::Clock{RCL_ROS_TIME}.now(); + debug_msg->data = debug_info; + + start_planner_visitor_ = debug_msg; +} + void StartPlannerModule::setDebugData() { using autoware::universe_utils::createDefaultMarker; diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.hpp new file mode 100644 index 0000000000000..a1d294b279ff6 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/include/start_planner_test_helper.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. +#pragma once + +#include +#include + +#include + +namespace autoware::behavior_path_planner::testing +{ + +class StartPlannerTestHelper +{ +public: + static rclcpp::NodeOptions make_node_options(); + + static void set_odometry( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose); + static void set_route( + std::shared_ptr & planner_data, const int route_start_lane_id, + const int route_goal_lane_id); + static void set_costmap( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose, + const double grid_resolution, const double grid_length_x, const double grid_length_y); +}; + +} // namespace autoware::behavior_path_planner::testing diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp new file mode 100644 index 0000000000000..0f0720edc4a15 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/src/start_planner_test_helper.cpp @@ -0,0 +1,98 @@ +// 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. +// pull_out_test_utils.cpp +#include "start_planner_test_helper.hpp" + +#include +#include + +#include +#include + +namespace autoware::behavior_path_planner::testing +{ +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +rclcpp::NodeOptions StartPlannerTestHelper::make_node_options() +{ + // Load common configuration files + auto node_options = rclcpp::NodeOptions{}; + + const auto common_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); + const auto nearest_search_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); + const auto vehicle_info_param_path = + get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); + const auto behavior_path_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); + const auto drivable_area_expansion_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); + const auto scene_module_manager_param_path = get_absolute_path_to_config( + "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); + const auto start_planner_param_path = get_absolute_path_to_config( + "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); + + autoware::test_utils::updateNodeOptions( + node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, + behavior_path_planner_param_path, drivable_area_expansion_param_path, + scene_module_manager_param_path, start_planner_param_path}); + + return node_options; +} + +void StartPlannerTestHelper::set_odometry( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose) +{ + auto odometry = std::make_shared(); + odometry->pose.pose = start_pose; + odometry->header.frame_id = "map"; + planner_data->self_odometry = odometry; +} + +void StartPlannerTestHelper::set_route( + std::shared_ptr & planner_data, const int route_start_lane_id, + const int route_goal_lane_id) +{ + const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); + auto route_handler = std::make_shared(map_bin_msg); + + const auto route = makeBehaviorRouteFromLaneId( + route_start_lane_id, route_goal_lane_id, "autoware_test_utils", + "road_shoulder/lanelet2_map.osm"); + route_handler->setRoute(route); + planner_data->route_handler = route_handler; +} + +void StartPlannerTestHelper::set_costmap( + std::shared_ptr & planner_data, const geometry_msgs::msg::Pose & start_pose, + const double grid_resolution, const double grid_length_x, const double grid_length_y) +{ + nav_msgs::msg::OccupancyGrid costmap; + costmap.header.frame_id = "map"; + costmap.info.width = static_cast(grid_length_x / grid_resolution); + costmap.info.height = static_cast(grid_length_y / grid_resolution); + costmap.info.resolution = grid_resolution; + + costmap.info.origin.position.x = start_pose.position.x - grid_length_x / 2; + costmap.info.origin.position.y = start_pose.position.y - grid_length_y / 2; + costmap.data = std::vector(costmap.info.width * costmap.info.height, 0); + + planner_data->costmap = std::make_shared(costmap); +} + +} // namespace autoware::behavior_path_planner::testing diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp new file mode 100644 index 0000000000000..5078c463d7c36 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_freespace_pull_out.cpp @@ -0,0 +1,113 @@ +// 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 "start_planner_test_helper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::behavior_path_planner::FreespacePullOut; +using autoware::behavior_path_planner::StartPlannerParameters; +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_msgs::msg::LaneletRoute; +using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +namespace autoware::behavior_path_planner +{ + +class TestFreespacePullOut : public ::testing::Test +{ +public: + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) + { + return freespace_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); + } + +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = + rclcpp::Node::make_shared("freespace_pull_out", StartPlannerTestHelper::make_node_options()); + + planner_data_ = std::make_shared(); + planner_data_->init_parameters(*node_); + + initialize_freespace_pull_out_planner(); + } + + void TearDown() override { rclcpp::shutdown(); } + + // Member variables + std::shared_ptr node_; + std::shared_ptr freespace_pull_out_; + std::shared_ptr planner_data_; + +private: + void initialize_freespace_pull_out_planner() + { + auto parameters = StartPlannerParameters::init(*node_); + freespace_pull_out_ = std::make_shared(*node_, parameters); + } +}; + +TEST_F(TestFreespacePullOut, DISABLED_GenerateValidFreespacePullOutPath) +{ + const auto start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(299.796).y(303.529).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(-0.748629).w( + 0.662990)); + + const auto goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(280.721).y(301.025).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.991718).w( + 0.128435)); + + StartPlannerTestHelper::set_odometry(planner_data_, start_pose); + StartPlannerTestHelper::set_route(planner_data_, 508, 720); + StartPlannerTestHelper::set_costmap(planner_data_, start_pose, 0.3, 70.0, 70.0); + + // Plan the pull out path + PlannerDebugData debug_data; + auto result = call_plan(start_pose, goal_pose, planner_data_, debug_data); + + // Assert that a valid Freespace pull out path is generated + ASSERT_TRUE(result.has_value()) << "Freespace pull out path generation failed."; + // EXPECT_EQ(result->partial_paths.size(), 2UL) + // << "Freespace pull out path does not have the expected number of partial paths."; + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") + << "Freespace pull out path planning did not succeed."; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp new file mode 100644 index 0000000000000..4548c058b871d --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp @@ -0,0 +1,110 @@ +// 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 "start_planner_test_helper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +using autoware::behavior_path_planner::GeometricPullOut; +using autoware::behavior_path_planner::StartPlannerParameters; +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_msgs::msg::LaneletRoute; +using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +namespace autoware::behavior_path_planner +{ + +class TestGeometricPullOut : public ::testing::Test +{ +public: + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) + { + return geometric_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); + } + +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = + rclcpp::Node::make_shared("geometric_pull_out", StartPlannerTestHelper::make_node_options()); + + initialize_geometric_pull_out_planner(); + } + void TearDown() override { rclcpp::shutdown(); } + + // Member variables + std::shared_ptr node_; + std::shared_ptr geometric_pull_out_; + +private: + void initialize_geometric_pull_out_planner() + { + auto parameters = StartPlannerParameters::init(*node_); + + geometric_pull_out_ = std::make_shared(*node_, parameters); + } +}; + +TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) +{ + const auto start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(362.181).y(362.164).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.709650).w( + 0.704554)); + + const auto goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(365.658).y(507.253).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( + 0.708314)); + + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node_); + StartPlannerTestHelper::set_odometry(planner_data, start_pose); + StartPlannerTestHelper::set_route(planner_data, 4619, 4635); + + // Plan the pull out path + PlannerDebugData debug_data; + auto result = call_plan(start_pose, goal_pose, planner_data, debug_data); + + // Assert that a valid geometric pull out path is generated + ASSERT_TRUE(result.has_value()) << "Geometric pull out path generation failed."; + EXPECT_EQ(result->partial_paths.size(), 2UL) + << "Generated geometric pull out path does not have the expected number of partial paths."; + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") + << "Geometric pull out path planning did not succeed."; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp new file mode 100644 index 0000000000000..d2c7c60e1a4ca --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_shift_pull_out.cpp @@ -0,0 +1,111 @@ +// 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 "start_planner_test_helper.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +using autoware::behavior_path_planner::ShiftPullOut; +using autoware::behavior_path_planner::StartPlannerParameters; +using autoware::test_utils::get_absolute_path_to_config; +using autoware_planning_msgs::msg::LaneletRoute; +using RouteSections = std::vector; +using autoware::behavior_path_planner::testing::StartPlannerTestHelper; +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; + +namespace autoware::behavior_path_planner +{ + +class TestShiftPullOut : public ::testing::Test +{ +public: + std::optional call_plan( + const Pose & start_pose, const Pose & goal_pose, + const std::shared_ptr & planner_data, PlannerDebugData & planner_debug_data) + { + return shift_pull_out_->plan(start_pose, goal_pose, planner_data, planner_debug_data); + } + +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = + rclcpp::Node::make_shared("shift_pull_out", StartPlannerTestHelper::make_node_options()); + + initialize_shift_pull_out_planner(); + } + + void TearDown() override { rclcpp::shutdown(); } + + // Member variables + std::shared_ptr node_; + std::shared_ptr shift_pull_out_; + +private: + void initialize_shift_pull_out_planner() + { + auto parameters = StartPlannerParameters::init(*node_); + + shift_pull_out_ = std::make_shared(*node_, parameters); + } +}; + +TEST_F(TestShiftPullOut, GenerateValidShiftPullOutPath) +{ + const auto start_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(362.181).y(362.164).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.709650).w( + 0.704554)); + + const auto goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build().x(365.658).y(507.253).z(100.000)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( + 0.708314)); + + auto planner_data = std::make_shared(); + planner_data->init_parameters(*node_); + StartPlannerTestHelper::set_odometry(planner_data, start_pose); + StartPlannerTestHelper::set_route(planner_data, 4619, 4635); + // Plan the pull out path + PlannerDebugData debug_data; + auto result = call_plan(start_pose, goal_pose, planner_data, debug_data); + + // Assert that a valid shift pull out path is generated + ASSERT_TRUE(result.has_value()) << "shift pull out path generation failed."; + EXPECT_EQ(result->partial_paths.size(), 1UL) + << "Generated shift pull out path does not have the expected number of partial paths."; + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") + << "shift pull out path planning did not succeed."; +} + +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/CHANGELOG.rst b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/CHANGELOG.rst index e162c187de824..fc252da9c1880 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/CHANGELOG.rst +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/CHANGELOG.rst @@ -2,6 +2,29 @@ Changelog for package autoware_behavior_path_static_obstacle_avoidance_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(static_obstacle_avoidance): output safety factor (`#10000 `_) + * feat(safety_check): convert to SafetyFactor + * feat(static_obstacle_avoidance): use safety factor + * fix(bpp): output detail + --------- +* refactor(behavior_path_planner): common test functions (`#9963 `_) + * feat: common test code in behavior_path_planner + * deal with other modules + * fix typo + * update + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix(static_avoidance): add optional check (`#9782 `_) +* fix(autoware_behavior_path_static_obstacle_avoidance_module): fix bugprone-branch-clone (`#9701 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka, Zulfaqar Azmi, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp index 895390f91cc16..6296acd8e71f7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -41,7 +41,7 @@ class StaticObstacleAvoidanceModuleManager : public SceneModuleManagerInterface { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, - objects_of_interest_marker_interface_ptr_map_); + objects_of_interest_marker_interface_ptr_map_, planning_factor_interface_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp index 5848cc75f148e..46b6388fd5119 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -17,6 +17,7 @@ #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/utils/path_safety_checker/safety_check.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" @@ -46,7 +47,8 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface 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); + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; @@ -131,9 +133,10 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - steering_factor_interface_.set( - {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::LEFT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, left_shift.start_pose, left_shift.finish_pose, + PlanningFactor::SHIFT_LEFT, + utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check)); } } @@ -151,9 +154,10 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface } if (finish_distance > -1.0e-03) { - steering_factor_interface_.set( - {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::RIGHT, SteeringFactor::TURNING, ""); + planning_factor_interface_->add( + start_distance, finish_distance, right_shift.start_pose, right_shift.finish_pose, + PlanningFactor::SHIFT_RIGHT, + utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check)); } } } diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 5446e33073b13..e9cb603b5d69c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_path_static_obstacle_avoidance_module - 0.40.0 + 0.41.0 The autoware_behavior_path_static_obstacle_avoidance_module package Takamasa Horibe diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index be4ce4a125c68..e0f96c3ab2ed1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -79,14 +79,13 @@ StaticObstacleAvoidanceModule::StaticObstacleAvoidanceModule( 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) -: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + objects_of_interest_marker_interface_ptr_map, + const std::shared_ptr & planning_factor_interface) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT helper_{std::make_shared(parameters)}, parameters_{parameters}, generator_{parameters} { - steering_factor_interface_.init(PlanningBehavior::AVOIDANCE); - velocity_factor_interface_.init(PlanningBehavior::AVOIDANCE); } bool StaticObstacleAvoidanceModule::isExecutionRequested() const @@ -233,11 +232,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData( std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { - if (!not_use_adjacent_lane) { - data.drivable_lanes.push_back( - utils::static_obstacle_avoidance::generateExpandedDrivableLanes( - lanelet, planner_data_, parameters_)); - } else if (red_signal_lane_itr->id() != lanelet.id()) { + if (!not_use_adjacent_lane || red_signal_lane_itr->id() != lanelet.id()) { data.drivable_lanes.push_back( utils::static_obstacle_avoidance::generateExpandedDrivableLanes( lanelet, planner_data_, parameters_)); @@ -780,7 +775,7 @@ void StaticObstacleAvoidanceModule::updateEgoBehavior( insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); - setVelocityFactor(path.path); + set_longitudinal_planning_factor(path.path); } bool StaticObstacleAvoidanceModule::isSafePath( @@ -1213,13 +1208,15 @@ CandidateOutput StaticObstacleAvoidanceModule::planCandidate() const output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; - const uint16_t steering_factor_direction = std::invoke([&output]() { - return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + const uint16_t planning_factor_direction = std::invoke([&output]() { + return output.lateral_shift > 0.0 ? PlanningFactor::SHIFT_LEFT : PlanningFactor::SHIFT_RIGHT; }); - steering_factor_interface_.set( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - steering_factor_direction, SteeringFactor::APPROACHING, ""); + + planning_factor_interface_->add( + output.start_distance_to_path_change, output.finish_distance_to_path_change, sl_front.start, + sl_back.end, planning_factor_direction, + utils::path_safety_checker::to_safety_factor_array(debug_data_.collision_check), true, 0.0, + output.lateral_shift); output.path_candidate = shifted_path.path; return output; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index 744af35641a59..52d5ee49c9a68 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -264,9 +264,9 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline( AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin.has_value()) { - 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()) { + if ( + data.red_signal_lane.has_value() && + data.red_signal_lane.value().id() == o.overhang_lanelet.id()) { o.info = ObjectInfo::LIMIT_DRIVABLE_SPACE_TEMPORARY; } else { o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE; diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index b6a8fd04f93db..c2531e5f2bb9f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -12,89 +12,24 @@ // 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 "autoware/behavior_path_planner/test_utils.hpp" #include #include #include -using autoware::behavior_path_planner::BehaviorPathPlannerNode; -using autoware::planning_test_manager::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 autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_path_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_path_planner"); - - std::vector module_names; - module_names.emplace_back( - "autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"); - - std::vector params; - params.emplace_back("launch_modules", module_names); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_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_static_obstacle_avoidance_module") + - "/config/static_obstacle_avoidance.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"); -} +using autoware::behavior_path_planner::generateNode; +using autoware::behavior_path_planner::generateTestManager; +using autoware::behavior_path_planner::publishMandatoryTopics; TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionRoute) { rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_manager = generateTestManager(); + auto test_target_node = generateNode( + {"static_obstacle_avoidance"}, + {"autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory @@ -111,7 +46,9 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) rclcpp::init(0, nullptr); auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); + auto test_target_node = generateNode( + {"static_obstacle_avoidance"}, + {"autoware::behavior_path_planner::StaticObstacleAvoidanceModuleManager"}); publishMandatoryTopics(test_manager, test_target_node); // test for normal trajectory diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CHANGELOG.rst index 0826b1f8193a6..4fe378098ee17 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CHANGELOG.rst @@ -2,6 +2,59 @@ Changelog for package autoware_behavior_velocity_blind_spot_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* test(blind_spot): add unit tests for util functions (`#9597 `_) +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt index b4a688d711221..220c380eb0fd7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_blind_spot_module) +option(EXPORT_TEST_PLOT_FIGURE "Export plot figures in test" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) @@ -11,6 +13,20 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp src/decisions.cpp src/util.cpp + src/parameter.cpp ) -ament_auto_package(INSTALL_TO_SHARE config) +if(BUILD_TESTING) + if(EXPORT_TEST_PLOT_FIGURE) + add_definitions(-DEXPORT_TEST_PLOT_FIGURE "-Wno-attributes") # // cspell: ignore DEXPORT + endif() + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + # NOTE(soblin): pybind11::scoped_interpreter needs to be initialized globally, not in the FixtureClass instantiated for each test suite + ament_add_gtest(test_${PROJECT_NAME}_util + test/test_util.cpp + ) + target_link_libraries(test_${PROJECT_NAME}_util ${PROJECT_NAME}) +endif() + +ament_auto_package(INSTALL_TO_SHARE config test_data) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index f296ad03cbac6..41330e459a524 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp @@ -15,11 +15,12 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ +#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp" #include "autoware/behavior_velocity_blind_spot_module/scene.hpp" #include #include -#include +#include #include #include @@ -38,12 +39,12 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC const char * getModuleName() override { return "blind_spot"; } private: - BlindSpotModule::PlannerParam planner_param_; + PlannerParam planner_param_; void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class BlindSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp new file mode 100644 index 0000000000000..59322caecb62f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp @@ -0,0 +1,40 @@ +// 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_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ +struct PlannerParam +{ + static PlannerParam init(rclcpp::Node & node, const std::string & ns); + 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{}; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 1bfa41d86ffbe..2d7b79357b602 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -15,9 +15,10 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ +#include #include -#include #include +#include #include #include @@ -59,7 +60,7 @@ struct Safe using BlindSpotDecision = std::variant; -class BlindSpotModule : public SceneModuleInterface +class BlindSpotModule : public SceneModuleInterfaceWithRTC { public: struct DebugData @@ -70,24 +71,13 @@ class BlindSpotModule : public SceneModuleInterface }; 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); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp index 9d908414b6d95..e18d96709ef92 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ #include +#include #include @@ -25,6 +26,7 @@ #include #include #include +#include namespace autoware::behavior_velocity_planner { @@ -50,31 +52,49 @@ std::optional generateInterpolatedPathInfo( const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger); +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr); /** - * @brief Create half lanelet - * @param lanelet input lanelet - * @return Half lanelet + * @brief generate a new lanelet object on the `turn_direction` side of `lanelet` which is offset + * from `ignore_width_from_centerline` from the centerline of `lanelet` + * @return new lanelet object */ lanelet::ConstLanelet generateHalfLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection & turn_direction, const double ignore_width_from_centerline); +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring lanelet of the + * input `lanelet` by the width of `adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double adjacent_extend_width); + +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring opposite lanelet + * of the input `lanelet` by the width of `opposite_adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double opposite_adjacent_extend_width); +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id); + lanelet::ConstLanelets generateBlindSpotLanelets( const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width); + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width); /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index a2ea4a82a884d..8c2f96728abc1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_blind_spot_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_blind_spot_module package Mamoru Sobue @@ -17,12 +17,15 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_test_utils autoware_universe_utils geometry_msgs pluginlib @@ -31,6 +34,7 @@ tier4_planning_msgs visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp index 64e1435167a89..a978c074e0018 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/decisions.cpp @@ -101,8 +101,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as UNSAFE)"); } return; } @@ -132,8 +135,11 @@ void BlindSpotModule::reactRTCApprovalByDecision( decision.stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); const auto stop_pose = path->points.at(decision.stop_line_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, + "blind_spot(module is judging as SAFE and RTC is not approved)"); } return; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index cc63b42df68e4..ee6516f025709 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -34,23 +34,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(BlindSpotModuleManager::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"); + planner_param_ = PlannerParam::init(node, ns); } void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) @@ -75,7 +59,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa registerModule(std::make_shared( module_id, lane_id, turn_direction, planner_data_, planner_param_, - logger_.get_child("blind_spot_module"), clock_)); + logger_.get_child("blind_spot_module"), clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), @@ -83,7 +67,7 @@ void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa } } -std::function &)> +std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp new file mode 100644 index 0000000000000..0984de6f73262 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp @@ -0,0 +1,45 @@ +// 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_velocity_blind_spot_module/parameter.hpp" + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +PlannerParam PlannerParam::init(rclcpp::Node & node, const std::string & ns) +{ + using autoware::universe_utils::getOrDeclareParameter; + PlannerParam param; + param.use_pass_judge_line = getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + param.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + param.backward_detection_length = + getOrDeclareParameter(node, ns + ".backward_detection_length"); + param.ignore_width_from_center_line = + getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); + param.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + param.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + param.max_future_movement_time = + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + param.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); + param.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + param.ttc_ego_minimal_velocity = + getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + return param; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 7697786adb19d..12943912f04cd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -40,14 +40,16 @@ 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), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), 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->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id_), planner_data->route_handler_->getRoutingGraphPtr()); @@ -101,8 +103,9 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * pat } if (!blind_spot_lanelets_) { + const auto lane_ids_upto_intersection = find_lane_ids_upto(input_path, lane_id_); const auto blind_spot_lanelets = generateBlindSpotLanelets( - planner_data_->route_handler_, turn_direction_, lane_id_, input_path, + planner_data_->route_handler_, turn_direction_, lane_ids_upto_intersection, planner_param_.ignore_width_from_center_line, planner_param_.adjacent_extend_width, planner_param_.opposite_adjacent_extend_width); if (!blind_spot_lanelets.empty()) { @@ -179,27 +182,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path) return true; } -static std::optional getFirstPointIntersectsLineByFootprint( - const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_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 = autoware::universe_utils::transformVector( - footprint, autoware::universe_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) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp index 5c5aa0a26b3b4..8451661b2b71f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -88,6 +88,27 @@ std::optional generateInterpolatedPathInfo( return interpolated_path_info; } +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_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 = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); + if (boost::geometry::intersects(path_footprint, line2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr) @@ -200,15 +221,9 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) return lanelet::LineString3d(lanelet::InvalId, pts); } -lanelet::ConstLanelets generateBlindSpotLanelets( - const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width) +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); - const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); - std::vector lane_ids; /* get lane ids until intersection */ for (const auto & point : path.points) { @@ -216,19 +231,29 @@ lanelet::ConstLanelets generateBlindSpotLanelets( for (const auto id : point.lane_ids) { if (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 (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { + lane_ids.push_back(id); } } if (found_intersection_lane) break; } + return lane_ids; +} + +lanelet::ConstLanelets generateBlindSpotLanelets( + const std::shared_ptr route_handler, + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); lanelet::ConstLanelets blind_spot_lanelets; - for (const auto i : lane_ids) { + for (const auto i : lane_ids_upto_intersection) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); const auto ego_half_lanelet = generateHalfLanelet(lane, turn_direction, ignore_width_from_centerline); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e0ceca4e12951 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "blind_spot", "autoware::behavior_velocity_planner::BlindSpotModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp new file mode 100644 index 0000000000000..5c2a239e4142f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp @@ -0,0 +1,448 @@ +// 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 +#include +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#ifdef EXPORT_TEST_PLOT_FIGURE +#include +#include +#include +#include + +#include +#include // needed for passing std::string to Args + +#endif + +using autoware::test_utils::parse; + +class TestWithAdjLaneData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_adj_lane.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{2200}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithAdjLaneData, getSiblingStraightLanelet) +{ + const auto sibling_straight_lanelet_opt = + autoware::behavior_velocity_planner::getSiblingStraightLanelet( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), + route_handler->getRoutingGraphPtr()); + ASSERT_NO_FATAL_FAILURE({ ASSERT_TRUE(sibling_straight_lanelet_opt.has_value()); }); + EXPECT_EQ(sibling_straight_lanelet_opt.value().id(), 2100); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), ax, + autoware::test_utils::LaneConfig{"intersection turning lanes", LineConfig{"blue"}}); + + // for illustration + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(3010933), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2201), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2202), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2010), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + autoware::test_utils::plot_lanelet2_object( + sibling_straight_lanelet_opt.value(), ax, + LaneConfig{"sibling_straight_lanelet", LineConfig{"red"}}); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateHalfLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + + const auto half_lanelet = autoware::behavior_velocity_planner::generateHalfLanelet( + lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.ignore_width_from_center_line); + + /* + TODO(soblin): how to check if they overlap only on the left line string + EXPECT_EQ( + boost::geometry::within( + half_lanelet.polygon2d().basicPolygon(), lanelet.polygon2d().basicPolygon()), + true); + */ + EXPECT_EQ( + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / 2.0 > + boost::geometry::area(half_lanelet.polygon2d().basicPolygon()), + true); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"original", LineConfig{"blue", 1.5}}); + autoware::test_utils::plot_lanelet2_object( + half_lanelet, ax, LaneConfig{"half lanelet", LineConfig{"red", 0.75}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + // const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, 650)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateExtendedAdjacentLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + const auto adj_lanelet = *(route_handler->getRoutingGraphPtr()->adjacentLeft(lanelet)); + + const auto extended_adj_lanelet = + autoware::behavior_velocity_planner::generateExtendedAdjacentLanelet( + adj_lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.adjacent_extend_width); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"given", LineConfig{"blue", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + adj_lanelet, ax, autoware::test_utils::LaneConfig{"adjacent", LineConfig{"green", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + extended_adj_lanelet, ax, LaneConfig{"generated", LineConfig{"red", 2.0}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {2000, 2010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + const auto [y0, y1] = ax.get_ylim(); + const double height = y1 - y0; + ax.set_xlim(Args(300, 365)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_right) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::RIGHT, {3008067}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 1); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {3008057, 3008054, 3008056, 3008061, 3008062, 3008059, 3008067, 3008066}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(905, 920)); + ax.set_ylim(Args(650, 950)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateInterpolatedPathInfo) +{ + const auto interpolated_path_info_opt = + autoware::behavior_velocity_planner::generateInterpolatedPathInfo( + lane_id_, path_with_lane_id, rclcpp::get_logger("test")); + EXPECT_EQ(interpolated_path_info_opt.has_value(), true); + const auto & interpolated_path_info = interpolated_path_info_opt.value(); + EXPECT_EQ(interpolated_path_info.lane_id_interval.has_value(), true); + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + for (auto i = start; i <= end; ++i) { + interpolated_path.points.push_back(interpolated_path_info.path.points.at(i)); + } +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + using autoware::test_utils::PathWithLaneIdConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000, 3500}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + autoware::test_utils::plot_autoware_object( + path_with_lane_id, ax, PathWithLaneIdConfig{"original path", "red", 0.75}); + autoware::test_utils::plot_autoware_object( + interpolated_path, ax, PathWithLaneIdConfig{"interpolated path on lane 2010", "blue", 1.0}); + const auto [x0, x1] = ax.get_xlim(); + ax.set_xlim(Args(335, x1)); + ax.set_ylim(Args(640, 670)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +class TestWithShoulderData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_shoulder.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{1010}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithShoulderData, generateBlindSpotLaneletsShoulder_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {1000, 1010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {1000, 1010, 3010907, 3010879, 1200, 1100}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(330, 365)); + ax.set_ylim(Args(580, 625)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper left"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +int main(int argc, char ** argv) +{ +#ifdef EXPORT_TEST_PLOT_FIGURE + py::scoped_interpreter guard{}; +#endif + rclcpp::init(0, nullptr); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml new file mode 100644 index 0000000000000..97068d69cd296 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml @@ -0,0 +1,2698 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 563 + nanosec: 125840339 + frame_id: map + points: + - point: + pose: + position: + x: 330.010 + y: 643.206 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00655991 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 332.010 + y: 643.233 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656125 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 334.009 + y: 643.259 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658745 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 336.009 + y: 643.285 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00660718 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 338.009 + y: 643.312 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658719 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 340.009 + y: 643.338 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656661 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.009 + y: 643.364 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00657433 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.809 + y: 643.375 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0162350 + w: 0.999868 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - 2200 + - point: + pose: + position: + x: 344.008 + y: 643.414 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0355542 + w: 0.999368 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 346.003 + y: 643.556 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0600763 + w: 0.998194 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 347.986 + y: 643.796 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.134724 + w: 0.990883 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 349.912 + y: 644.329 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.199426 + w: 0.979913 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 351.776 + y: 645.121 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.281519 + w: 0.959556 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 353.448 + y: 646.194 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.345347 + w: 0.938475 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 354.997 + y: 647.513 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.407454 + w: 0.913226 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 356.322 + y: 648.989 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.480104 + w: 0.877212 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 357.418 + y: 650.701 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.544895 + w: 0.838504 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.228 + y: 652.525 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.610262 + w: 0.792199 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.741 + y: 654.467 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.646506 + w: 0.762909 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.070 + y: 656.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697457 + w: 0.716627 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.124 + y: 658.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.713869 + w: 0.700280 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.085 + y: 660.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714886 + w: 0.699241 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.052 + y: 661.935 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - 3500 + - point: + pose: + position: + x: 359.040 + y: 662.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.993 + y: 664.437 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.945 + y: 666.436 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715334 + w: 0.698782 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.900 + y: 668.374 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714518 + w: 0.699617 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.858 + y: 670.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712894 + w: 0.701272 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.825 + y: 672.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710606 + w: 0.703590 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.807 + y: 674.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + left_bound: + - x: 329.488 + y: 644.822 + z: 100.000 + - x: 332.672 + y: 644.864 + z: 100.000 + - x: 334.352 + y: 644.886 + z: 100.000 + - x: 336.351 + y: 644.913 + z: 100.000 + - x: 337.719 + y: 644.931 + z: 100.000 + - x: 340.352 + y: 644.965 + z: 100.000 + - x: 342.813 + y: 645.020 + z: 100.000 + - x: 344.277 + y: 645.102 + z: 100.000 + - x: 346.200 + y: 645.256 + z: 100.000 + - x: 347.374 + y: 645.428 + z: 100.000 + - x: 348.470 + y: 645.697 + z: 100.000 + - x: 349.568 + y: 646.035 + z: 100.000 + - x: 350.592 + y: 646.513 + z: 100.000 + - x: 351.572 + y: 647.043 + z: 100.000 + - x: 352.507 + y: 647.713 + z: 100.000 + - x: 353.327 + y: 648.476 + z: 100.000 + - x: 354.109 + y: 649.196 + z: 100.000 + - x: 354.661 + y: 649.830 + z: 100.000 + - x: 355.275 + y: 650.523 + z: 100.000 + - x: 356.177 + y: 652.020 + z: 100.000 + - x: 356.615 + y: 653.084 + z: 100.000 + - x: 356.929 + y: 654.139 + z: 100.000 + - x: 357.146 + y: 655.241 + z: 100.000 + - x: 357.284 + y: 656.160 + z: 100.000 + - x: 357.312 + y: 657.420 + z: 100.000 + - x: 357.425 + y: 658.703 + z: 100.000 + - x: 357.383 + y: 660.465 + z: 100.000 + - x: 357.349 + y: 661.894 + z: 100.000 + - x: 357.386 + y: 664.700 + z: 100.000 + - x: 357.338 + y: 666.699 + z: 100.000 + - x: 357.291 + y: 668.699 + z: 100.000 + - x: 357.243 + y: 670.698 + z: 100.000 + - x: 357.195 + y: 672.698 + z: 100.000 + - x: 357.148 + y: 674.697 + z: 100.000 + - x: 357.021 + y: 680.023 + z: 100.000 + right_bound: + - x: 329.531 + y: 641.578 + z: 100.000 + - x: 332.717 + y: 641.619 + z: 100.000 + - x: 334.395 + y: 641.641 + z: 100.000 + - x: 336.394 + y: 641.668 + z: 100.000 + - x: 337.761 + y: 641.686 + z: 100.000 + - x: 340.394 + y: 641.721 + z: 100.000 + - x: 342.805 + y: 641.730 + z: 100.000 + - x: 344.462 + y: 641.823 + z: 100.000 + - x: 346.283 + y: 641.878 + z: 100.000 + - x: 347.708 + y: 641.985 + z: 100.000 + - x: 349.158 + y: 642.284 + z: 100.000 + - x: 350.570 + y: 642.681 + z: 100.000 + - x: 351.954 + y: 643.219 + z: 100.000 + - x: 353.269 + y: 643.933 + z: 100.000 + - x: 354.499 + y: 644.727 + z: 100.000 + - x: 355.646 + y: 645.678 + z: 100.000 + - x: 356.516 + y: 646.483 + z: 100.000 + - x: 357.282 + y: 647.228 + z: 100.000 + - x: 357.690 + y: 647.819 + z: 100.000 + - x: 358.527 + y: 649.081 + z: 100.000 + - x: 359.237 + y: 650.418 + z: 100.000 + - x: 359.789 + y: 651.774 + z: 100.000 + - x: 360.225 + y: 653.220 + z: 100.000 + - x: 360.523 + y: 654.391 + z: 100.000 + - x: 360.701 + y: 655.780 + z: 100.000 + - x: 360.976 + y: 657.487 + z: 100.000 + - x: 360.804 + y: 658.842 + z: 100.000 + - x: 360.774 + y: 660.766 + z: 100.000 + - x: 360.755 + y: 661.975 + z: 100.000 + - x: 360.585 + y: 664.776 + z: 100.000 + - x: 360.537 + y: 666.776 + z: 100.000 + - x: 360.490 + y: 668.775 + z: 100.000 + - x: 360.442 + y: 670.774 + z: 100.000 + - x: 360.394 + y: 672.774 + z: 100.000 + - x: 360.347 + y: 674.773 + z: 100.000 + - x: 360.220 + y: 680.099 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 563 + nanosec: 160706650 + frame_id: map + objects: + - object_id: + uuid: + - 6 + - 144 + - 167 + - 221 + - 26 + - 196 + - 145 + - 186 + - 207 + - 95 + - 21 + - 211 + - 96 + - 13 + - 92 + - 5 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + covariance: + - 0.415785 + - -0.000621846 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000621846 + - 0.388031 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0108039 + initial_twist_with_covariance: + twist: + linear: + x: 9.96635 + y: 3.24754e-05 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 5.69744e-05 + covariance: + - 1.01780 + - -2.02150e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -3.54650e-05 + - -2.02150e-05 + - 0.00403854 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -3.54650e-05 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0124301 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 409.670 + y: 647.155 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 414.653 + y: 647.245 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 419.635 + y: 647.335 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 424.617 + y: 647.425 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 429.600 + y: 647.515 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 434.582 + y: 647.605 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 439.565 + y: 647.694 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 444.547 + y: 647.784 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 449.529 + y: 647.874 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 454.512 + y: 647.964 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 459.494 + y: 648.054 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 464.476 + y: 648.144 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 469.459 + y: 648.233 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 474.441 + y: 648.323 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 479.423 + y: 648.413 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 484.406 + y: 648.503 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 489.388 + y: 648.593 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 494.371 + y: 648.683 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 499.353 + y: 648.773 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 504.335 + y: 648.862 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.00000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.501209 + z: 0.690476 + - object_id: + uuid: + - 32 + - 119 + - 141 + - 16 + - 72 + - 85 + - 32 + - 153 + - 66 + - 136 + - 88 + - 177 + - 247 + - 166 + - 151 + - 80 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + covariance: + - 0.0657687 + - -0.000366900 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000366900 + - 0.0451891 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00300423 + initial_twist_with_covariance: + twist: + linear: + x: 10.0048 + y: -0.00124906 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00219133 + covariance: + - 0.522271 + - -6.09993e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000107016 + - -6.09993e-05 + - 0.00263830 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000107016 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00812034 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00453640 + w: 0.999990 + - position: + x: 333.933 + y: 644.723 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00622365 + w: 0.999981 + - position: + x: 338.935 + y: 644.640 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00828910 + w: 0.999966 + - position: + x: 343.877 + y: 644.600 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00409926 + w: 0.999992 + - position: + x: 348.624 + y: 645.122 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0547342 + w: 0.998501 + - position: + x: 352.898 + y: 646.993 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.204867 + w: 0.978790 + - position: + x: 356.221 + y: 650.176 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.372718 + w: 0.927945 + - position: + x: 358.029 + y: 654.521 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.554937 + w: 0.831892 + - position: + x: 358.484 + y: 659.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.673390 + w: 0.739287 + - position: + x: 358.380 + y: 664.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714418 + w: 0.699720 + - position: + x: 358.261 + y: 669.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 674.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 679.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.904 + y: 684.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 689.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.665 + y: 694.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.546 + y: 699.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.427 + y: 704.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.308 + y: 709.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.188 + y: 714.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.069 + y: 719.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.950 + y: 724.396 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.831 + y: 729.397 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.712 + y: 734.398 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.593 + y: 739.399 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.473 + y: 744.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.354 + y: 749.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.235 + y: 754.401 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.116 + y: 759.402 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.997 + y: 764.403 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.880 + y: 644.606 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00392303 + w: 0.999992 + - position: + x: 348.786 + y: 644.906 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0304731 + w: 0.999536 + - position: + x: 353.463 + y: 645.966 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.111241 + w: 0.993793 + - position: + x: 357.696 + y: 648.148 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.235723 + w: 0.971820 + - position: + x: 361.186 + y: 651.334 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.361617 + w: 0.932327 + - position: + x: 363.716 + y: 655.456 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.488304 + w: 0.872674 + - position: + x: 364.853 + y: 660.052 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.616332 + w: 0.787487 + - position: + x: 364.887 + y: 665.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704715 + w: 0.709490 + - position: + x: 364.768 + y: 670.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.648 + y: 675.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.529 + y: 680.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.410 + y: 685.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.291 + y: 690.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.172 + y: 695.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.053 + y: 700.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.933 + y: 705.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.814 + y: 710.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.695 + y: 715.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.576 + y: 720.008 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.457 + y: 725.009 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.338 + y: 730.010 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.218 + y: 735.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.099 + y: 740.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.980 + y: 745.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.861 + y: 750.013 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.742 + y: 755.014 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.622 + y: 760.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.873 + y: 644.613 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00329511 + w: 0.999995 + - position: + x: 348.698 + y: 645.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0415760 + w: 0.999135 + - position: + x: 353.223 + y: 646.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.147464 + w: 0.989067 + - position: + x: 357.115 + y: 649.075 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.296991 + w: 0.954880 + - position: + x: 359.997 + y: 652.871 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.444541 + w: 0.895759 + - position: + x: 361.525 + y: 657.337 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.581548 + w: 0.813512 + - position: + x: 361.651 + y: 662.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697878 + w: 0.716217 + - position: + x: 361.533 + y: 667.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715386 + w: 0.698729 + - position: + x: 361.414 + y: 672.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.295 + y: 677.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.176 + y: 682.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.057 + y: 687.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.938 + y: 692.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.818 + y: 697.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.699 + y: 702.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.580 + y: 707.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.461 + y: 712.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.342 + y: 717.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.222 + y: 722.210 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.103 + y: 727.211 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.984 + y: 732.212 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.865 + y: 737.213 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.746 + y: 742.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.626 + y: 747.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.507 + y: 752.215 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.388 + y: 757.216 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.269 + y: 762.217 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00457230 + w: 0.999990 + - position: + x: 333.933 + y: 644.721 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00644958 + w: 0.999979 + - position: + x: 338.935 + y: 644.632 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00883007 + w: 0.999961 + - position: + x: 343.938 + y: 644.526 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0105871 + w: 0.999944 + - position: + x: 348.941 + y: 644.416 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0110042 + w: 0.999939 + - position: + x: 353.943 + y: 644.319 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00972075 + w: 0.999953 + - position: + x: 358.946 + y: 644.250 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00691289 + w: 0.999976 + - position: + x: 363.948 + y: 644.219 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00309863 + w: 0.999995 + - position: + x: 368.950 + y: 644.228 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.000886733 + w: 1.00000 + - position: + x: 373.952 + y: 644.264 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00363761 + w: 0.999993 + - position: + x: 378.954 + y: 644.305 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00413557 + w: 0.999991 + - position: + x: 383.955 + y: 644.347 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00420318 + w: 0.999991 + - position: + x: 388.957 + y: 644.388 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00411113 + w: 0.999992 + - position: + x: 393.958 + y: 644.432 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00439488 + w: 0.999990 + - position: + x: 398.956 + y: 644.454 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00215849 + w: 0.999998 + - position: + x: 403.941 + y: 644.630 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0176845 + w: 0.999844 + - position: + x: 408.939 + y: 644.821 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 413.938 + y: 645.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 418.936 + y: 645.202 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 423.935 + y: 645.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 428.933 + y: 645.584 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 433.932 + y: 645.774 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 438.931 + y: 645.965 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 443.929 + y: 646.156 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 448.928 + y: 646.346 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 453.927 + y: 646.537 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 458.925 + y: 646.728 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 463.924 + y: 646.918 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 468.923 + y: 647.109 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 473.922 + y: 647.300 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.604998 + z: 0.767451 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 21 + nanosec: 17351334 + frame_id: map + start_pose: + position: + x: 301.475 + y: 642.905 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00500123 + w: 0.999987 + goal_pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + segments: + - preferred_primitive: + id: 2000 + primitive_type: "" + primitives: + - id: 2000 + primitive_type: lane + - id: 2001 + primitive_type: lane + - id: 2002 + primitive_type: lane + - preferred_primitive: + id: 2010 + primitive_type: "" + primitives: + - id: 2010 + primitive_type: lane + - preferred_primitive: + id: 2200 + primitive_type: "" + primitives: + - id: 2200 + primitive_type: lane + - preferred_primitive: + id: 3500 + primitive_type: "" + primitives: + - id: 3500 + primitive_type: lane + - id: 3501 + primitive_type: lane + - id: 3502 + primitive_type: lane + uuid: + uuid: + - 195 + - 84 + - 241 + - 105 + - 133 + - 218 + - 136 + - 83 + - 104 + - 191 + - 48 + - 109 + - 85 + - 60 + - 177 + - 115 + allow_modification: false +operation_mode: + stamp: + sec: 550 + nanosec: 913829317 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: /base_link + accel: + accel: + linear: + x: -0.770147 + y: 0.0409905 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 337.371 + y: 643.219 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00401753 + w: 0.999992 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 4.13155 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00992133 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml new file mode 100644 index 0000000000000..9a20cbee9c1ff --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml @@ -0,0 +1,2512 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 74 + nanosec: 11949715 + frame_id: map + points: + - point: + pose: + position: + x: 360.306 + y: 597.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710800 + w: 0.703394 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.285 + y: 599.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710829 + w: 0.703365 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.264 + y: 601.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710857 + w: 0.703336 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.243 + y: 603.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710881 + w: 0.703312 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.222 + y: 605.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710951 + w: 0.703242 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.200 + y: 607.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711036 + w: 0.703156 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.177 + y: 609.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711601 + w: 0.702584 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.152 + y: 611.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711932 + w: 0.702248 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.133 + y: 612.628 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716985 + w: 0.697089 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - 1200 + - point: + pose: + position: + x: 360.116 + y: 613.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.732064 + w: 0.681236 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.972 + y: 615.239 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.740537 + w: 0.672016 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.778 + y: 617.231 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.759018 + w: 0.651069 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.474 + y: 619.204 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.784627 + w: 0.619968 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.010 + y: 621.159 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.835622 + w: 0.549305 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 358.221 + y: 622.986 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.876871 + w: 0.480726 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 357.127 + y: 624.700 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.906015 + w: 0.423245 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 355.847 + y: 626.230 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.934815 + w: 0.355135 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 354.341 + y: 627.567 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.956562 + w: 0.291529 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 352.674 + y: 628.688 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.976933 + w: 0.213544 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 350.854 + y: 629.523 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.989029 + w: 0.147720 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 348.920 + y: 630.114 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.997418 + w: 0.0718077 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 346.951 + y: 630.399 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121684 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 344.951 + y: 630.448 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 1.00000 + w: 0.000253960 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.951 + y: 630.449 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999959 + w: 0.00906744 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.741 + y: 630.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - 2500 + - point: + pose: + position: + x: 340.951 + y: 630.409 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999893 + w: 0.0146423 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 339.014 + y: 630.352 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999803 + w: 0.0198557 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 337.016 + y: 630.273 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999732 + w: 0.0231414 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 335.018 + y: 630.180 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999702 + w: 0.0243973 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 333.020 + y: 630.083 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999705 + w: 0.0242858 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 332.731 + y: 630.068 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + left_bound: + - x: 358.794 + y: 596.731 + z: 100.000 + - x: 358.788 + y: 597.342 + z: 100.000 + - x: 358.758 + y: 600.198 + z: 100.000 + - x: 358.736 + y: 602.392 + z: 100.000 + - x: 358.718 + y: 604.199 + z: 100.000 + - x: 358.698 + y: 606.199 + z: 100.000 + - x: 358.686 + y: 607.440 + z: 100.000 + - x: 358.610 + y: 610.191 + z: 100.000 + - x: 358.512 + y: 612.597 + z: 100.000 + - x: 358.405 + y: 614.096 + z: 100.000 + - x: 358.218 + y: 616.079 + z: 100.000 + - x: 358.086 + y: 617.465 + z: 100.000 + - x: 357.913 + y: 618.622 + z: 100.000 + - x: 357.666 + y: 619.746 + z: 100.000 + - x: 357.285 + y: 620.810 + z: 100.000 + - x: 356.832 + y: 621.812 + z: 100.000 + - x: 356.284 + y: 622.823 + z: 100.000 + - x: 355.663 + y: 623.752 + z: 100.000 + - x: 354.964 + y: 624.613 + z: 100.000 + - x: 354.158 + y: 625.437 + z: 100.000 + - x: 353.306 + y: 626.152 + z: 100.000 + - x: 352.395 + y: 626.788 + z: 100.000 + - x: 351.400 + y: 627.366 + z: 100.000 + - x: 350.389 + y: 627.820 + z: 100.000 + - x: 349.337 + y: 628.212 + z: 100.000 + - x: 348.219 + y: 628.479 + z: 100.000 + - x: 347.083 + y: 628.681 + z: 100.000 + - x: 346.042 + y: 628.729 + z: 100.000 + - x: 344.050 + y: 628.720 + z: 100.000 + - x: 342.764 + y: 628.777 + z: 100.000 + - x: 340.080 + y: 628.723 + z: 100.000 + - x: 338.078 + y: 628.751 + z: 100.000 + - x: 336.079 + y: 628.710 + z: 100.000 + - x: 334.080 + y: 628.670 + z: 100.000 + - x: 332.082 + y: 628.629 + z: 100.000 + - x: 326.872 + y: 628.524 + z: 100.000 + right_bound: + - x: 361.830 + y: 596.762 + z: 100.000 + - x: 361.823 + y: 597.400 + z: 100.000 + - x: 361.792 + y: 600.231 + z: 100.000 + - x: 361.767 + y: 602.446 + z: 100.000 + - x: 361.748 + y: 604.230 + z: 100.000 + - x: 361.726 + y: 606.230 + z: 100.000 + - x: 361.712 + y: 607.493 + z: 100.000 + - x: 361.718 + y: 610.230 + z: 100.000 + - x: 361.754 + y: 612.657 + z: 100.000 + - x: 361.627 + y: 614.331 + z: 100.000 + - x: 361.519 + y: 616.330 + z: 100.000 + - x: 361.452 + y: 617.634 + z: 100.000 + - x: 361.277 + y: 619.056 + z: 100.000 + - x: 360.998 + y: 620.486 + z: 100.000 + - x: 360.581 + y: 621.905 + z: 100.000 + - x: 359.991 + y: 623.293 + z: 100.000 + - x: 359.319 + y: 624.583 + z: 100.000 + - x: 358.490 + y: 625.825 + z: 100.000 + - x: 357.546 + y: 626.984 + z: 100.000 + - x: 356.525 + y: 628.025 + z: 100.000 + - x: 355.377 + y: 628.981 + z: 100.000 + - x: 354.158 + y: 629.845 + z: 100.000 + - x: 352.881 + y: 630.552 + z: 100.000 + - x: 351.515 + y: 631.167 + z: 100.000 + - x: 350.098 + y: 631.615 + z: 100.000 + - x: 348.672 + y: 631.919 + z: 100.000 + - x: 347.236 + y: 632.124 + z: 100.000 + - x: 346.083 + y: 632.098 + z: 100.000 + - x: 344.082 + y: 632.081 + z: 100.000 + - x: 342.719 + y: 632.113 + z: 100.000 + - x: 340.012 + y: 632.058 + z: 100.000 + - x: 338.014 + y: 631.950 + z: 100.000 + - x: 336.015 + y: 631.909 + z: 100.000 + - x: 334.016 + y: 631.869 + z: 100.000 + - x: 332.017 + y: 631.829 + z: 100.000 + - x: 326.807 + y: 631.724 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 73 + nanosec: 980215670 + frame_id: map + objects: + - object_id: + uuid: + - 251 + - 180 + - 23 + - 85 + - 112 + - 118 + - 142 + - 88 + - 241 + - 29 + - 194 + - 27 + - 222 + - 164 + - 130 + - 251 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + covariance: + - 0.299126 + - -8.23923e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -8.23923e-05 + - 0.349251 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00879510 + initial_twist_with_covariance: + twist: + linear: + x: 9.98083 + y: 0.00185652 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00325706 + covariance: + - 0.970951 + - 0.000139981 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000245582 + - 0.000139981 + - 0.00384142 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.000245582 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0118235 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.373 + y: 672.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709614 + w: 0.704590 + - position: + x: 357.520 + y: 677.298 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.696604 + w: 0.717456 + - position: + x: 357.917 + y: 682.299 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.678600 + w: 0.734508 + - position: + x: 358.531 + y: 687.305 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.662668 + w: 0.748913 + - position: + x: 359.266 + y: 692.315 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.653723 + w: 0.756734 + - position: + x: 359.995 + y: 697.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.654217 + w: 0.756307 + - position: + x: 360.590 + y: 702.329 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.664107 + w: 0.747638 + - position: + x: 360.955 + y: 707.330 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.680844 + w: 0.732428 + - position: + x: 361.063 + y: 712.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.699424 + w: 0.714707 + - position: + x: 360.983 + y: 717.314 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712725 + w: 0.701443 + - position: + x: 360.864 + y: 722.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.746 + y: 727.292 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.627 + y: 732.280 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.508 + y: 737.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.389 + y: 742.258 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.270 + y: 747.247 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.151 + y: 752.236 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.032 + y: 757.225 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.913 + y: 762.214 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.794 + y: 767.203 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.675 + y: 772.192 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.557 + y: 777.181 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.438 + y: 782.170 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.319 + y: 787.159 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.200 + y: 792.148 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.081 + y: 797.137 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.962 + y: 802.126 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.843 + y: 807.115 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.724 + y: 812.104 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.605 + y: 817.093 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.230769 + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.340 + y: 672.302 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711946 + w: 0.702234 + - position: + x: 357.296 + y: 677.293 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710176 + w: 0.704024 + - position: + x: 357.286 + y: 682.284 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707820 + w: 0.706393 + - position: + x: 357.304 + y: 687.276 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705868 + w: 0.708343 + - position: + x: 357.334 + y: 692.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704976 + w: 0.709232 + - position: + x: 357.357 + y: 697.261 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705452 + w: 0.708758 + - position: + x: 357.355 + y: 702.252 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707256 + w: 0.706957 + - position: + x: 357.314 + y: 707.243 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709999 + w: 0.704203 + - position: + x: 357.231 + y: 712.233 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712949 + w: 0.701216 + - position: + x: 357.119 + y: 717.222 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715047 + w: 0.699076 + - position: + x: 357.000 + y: 722.211 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.881 + y: 727.199 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.762 + y: 732.188 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.643 + y: 737.177 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.524 + y: 742.166 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.405 + y: 747.155 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.286 + y: 752.144 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.167 + y: 757.133 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.049 + y: 762.122 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.930 + y: 767.111 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.811 + y: 772.100 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.692 + y: 777.089 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.573 + y: 782.078 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.454 + y: 787.067 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.335 + y: 792.056 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.216 + y: 797.045 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.097 + y: 802.033 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.978 + y: 807.022 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.859 + y: 812.011 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.741 + y: 817.000 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.769231 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.89999 + y: 0.500442 + z: 0.612431 + - object_id: + uuid: + - 218 + - 141 + - 152 + - 171 + - 185 + - 127 + - 84 + - 175 + - 69 + - 40 + - 199 + - 89 + - 96 + - 107 + - 61 + - 243 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + covariance: + - 0.0636258 + - -0.000170219 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000170219 + - 0.0826995 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00394708 + initial_twist_with_covariance: + twist: + linear: + x: 10.0303 + y: -0.00247386 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00434011 + covariance: + - 0.579437 + - -9.81341e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000172165 + - -9.81341e-05 + - 0.00311875 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000172165 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00959910 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.224 + y: 608.637 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.706633 + w: 0.707581 + - position: + x: 358.279 + y: 613.653 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.703235 + w: 0.710958 + - position: + x: 358.397 + y: 618.671 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698737 + w: 0.715379 + - position: + x: 358.567 + y: 623.690 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695068 + w: 0.718944 + - position: + x: 358.758 + y: 628.710 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.693532 + w: 0.720426 + - position: + x: 358.932 + y: 633.729 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.694735 + w: 0.719266 + - position: + x: 359.054 + y: 638.747 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698453 + w: 0.715656 + - position: + x: 359.097 + y: 643.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704045 + w: 0.710155 + - position: + x: 359.057 + y: 648.778 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709938 + w: 0.704265 + - position: + x: 358.958 + y: 653.791 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714096 + w: 0.700048 + - position: + x: 358.849 + y: 658.805 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714730 + w: 0.699401 + - position: + x: 358.740 + y: 663.817 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714768 + w: 0.699361 + - position: + x: 358.620 + y: 668.830 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.501 + y: 673.843 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.381 + y: 678.856 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.262 + y: 683.870 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 688.883 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 693.896 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.903 + y: 698.909 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 703.923 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.664 + y: 708.936 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.545 + y: 713.950 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.425 + y: 718.963 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.306 + y: 723.977 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.186 + y: 728.991 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.067 + y: 734.004 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.947 + y: 739.018 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.828 + y: 744.032 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.708 + y: 749.045 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.589 + y: 754.059 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.206 + y: 608.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707930 + w: 0.706283 + - position: + x: 358.149 + y: 613.642 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711130 + w: 0.703060 + - position: + x: 358.010 + y: 618.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716853 + w: 0.697224 + - position: + x: 357.024 + y: 623.117 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.779378 + w: 0.626554 + - position: + x: 354.441 + y: 627.006 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.881248 + w: 0.472653 + - position: + x: 350.340 + y: 629.665 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.958935 + w: 0.283625 + - position: + x: 345.469 + y: 630.694 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.994592 + w: 0.103864 + - position: + x: 340.508 + y: 630.814 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121264 + - position: + x: 335.554 + y: 630.768 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999989 + w: 0.00460609 + - position: + x: 330.597 + y: 630.677 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999957 + w: 0.00925679 + - position: + x: 325.638 + y: 630.576 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 320.674 + y: 630.476 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 315.708 + y: 630.376 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 310.737 + y: 630.276 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 305.763 + y: 630.175 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 300.785 + y: 630.075 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 295.803 + y: 629.974 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 290.817 + y: 629.874 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 285.828 + y: 629.773 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 280.835 + y: 629.672 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 275.839 + y: 629.571 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 270.840 + y: 629.470 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 265.838 + y: 629.370 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 260.833 + y: 629.268 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 255.825 + y: 629.167 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 250.816 + y: 629.066 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 245.805 + y: 628.965 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 240.792 + y: 628.864 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 235.778 + y: 628.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 230.764 + y: 628.662 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.644288 + z: 0.991445 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 19 + nanosec: 549630704 + frame_id: map + start_pose: + position: + x: 360.481 + y: 591.844 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.721675 + w: 0.692232 + goal_pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999615 + w: -0.0277448 + segments: + - preferred_primitive: + id: 1010 + primitive_type: "" + primitives: + - id: 1010 + primitive_type: lane + - preferred_primitive: + id: 1200 + primitive_type: "" + primitives: + - id: 1200 + primitive_type: lane + - preferred_primitive: + id: 2500 + primitive_type: "" + primitives: + - id: 2500 + primitive_type: lane + - id: 2501 + primitive_type: lane + uuid: + uuid: + - 83 + - 230 + - 40 + - 201 + - 182 + - 232 + - 31 + - 237 + - 204 + - 91 + - 39 + - 90 + - 153 + - 179 + - 6 + - 123 + allow_modification: false +operation_mode: + stamp: + sec: 65 + nanosec: 819714871 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: /base_link + accel: + accel: + linear: + x: -1.01094 + y: 0.00400509 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 360.302 + y: 604.493 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707950 + w: 0.706263 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 2.31221 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00173214 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CHANGELOG.rst index 608d6ab37fdc4..11f4bbb2b0a6c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CHANGELOG.rst @@ -2,6 +2,69 @@ Changelog for package autoware_behavior_velocity_crosswalk_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* docs(crosswalk): fix file add miss (`#10028 `_) +* docs(crosswalk): update ttc vs ttv docs (`#10025 `_) +* feat(crosswalk): update judgle time against the stopped objects (`#9988 `_) +* chore(crosswalk): port the same direction ignore block (`#9983 `_) +* feat(crosswalk): add pass marker (`#9952 `_) +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix: remove unnecessary parameters (`#9935 `_) +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs (`#9744 `_) + * feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs + * fix + --------- +* feat(behavior_velocity_planner): remove unnecessary tier4_api_msgs (`#9692 `_) +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka, Yuki TAKAGI + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt index 10b694ce22cbd..3a429008c1b8e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/CMakeLists.txt @@ -15,6 +15,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_crosswalk.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md index 857f590104aea..52e8c5b2a24a9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/README.md @@ -138,27 +138,24 @@ The decision is based on the following variables, along with the calculation of 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. +- A. **TTC >> TTV**: The object will pass early enough than the ego reach the collision point. - 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. +- C. **TTC << TTV**: The ego will pass early enough than the object reach the collision point. - No stop planning. +The following figure shows the decision result for each TTC and TTV with the parameters, `ego_pass_first_margin_x` is `{0}`, `ego_pass_first_margin_y` is `{4}`, `ego_pass_later_margin_x` is `{0}`, and `ego_pass_later_margin_y` is `{13}`. +
- +
-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.
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index bc0d4f613f10b..97e00ade8256c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -13,8 +13,6 @@ # For the Lanelet2 map with no explicit stop lines stop_distance_from_crosswalk: 3.5 # [m] make stop line away from crosswalk - # For the case where the crosswalk width is very wide - far_object_threshold: 10.0 # [m] 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 stop position is determined according to the object position. stop_distance_from_object_preferred: 3.0 # [m] stop_distance_from_object_limit: 3.0 # [m] diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg index 6eb1b25cf5642..b0c4a38125607 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg @@ -1,209 +1,344 @@ - + + - - - - - - -
-
-
- (5, 1) -
-
-
- - (5, 1) - - - - - - -
-
-
- (1, 4) -
-
-
- - (1, 4) - - - - - - -
-
-
- (2, 6) -
-
-
- - (2, 6) - - - - - - - - - - -
-
-
- (0, 1) -
-
-
- - (0, 1) - - - - - - -
-
-
- (3, 0) -
-
-
- - (3, 0) - - - - - - -
-
-
- Time-To-Vehicle [s] -
-
-
- - Time-To-Vehicle [s] - - - - - - - - - - - - - - - - - - - -
-
-
- Time-To-Collision [s] -
-
-
- - Time-To-Collision [s] - - - - - - -
-
-
- B -
-
-
- - B - - - - - - -
-
-
- A -
-
-
- - A - - - - - - -
-
-
- C -
-
-
- - C - + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ (13, 0) +
+
+
+ + (13, 0) + + + + + + + + + + + + +
+
+
+ Time-To-Vehicle [s] +
+
+
+ + Time-To-Vehicle [s] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Time-To-Collision [s] +
+
+
+ + Time-To-Collision [s] + + + + + + + + + + + + +
+
+
+ B +
+
+
+ + B + + + + + + + + + + + + +
+
+
+ A +
+
+
+ + A + + + + + + + + + + + + +
+
+
+ C +
+
+
+ + C + + + + + + + + + + + + +
+
+
+ (0, 4) +
+
+
+ + (0, 4) + + + + + + + + + + + + + + + + + + + + - + Text is not SVG - cannot display diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.svg new file mode 100644 index 0000000000000..ef03de6cac00d --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.svg @@ -0,0 +1,379 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ (13, 0) +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Time-To-Vehicle [s] +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Time-To-Collision [s] +
+
+
+ + + + + + + + + + + + + + +
+
+
+ B +
+
+
+ + + + + + + + + + + + + + +
+
+
+ A +
+
+
+ + + + + + + + + + + + + + +
+
+
+ C +
+
+
+ + + + + + + + + + + + + + +
+
+
+ (0, 4) +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp index 64c98017d3007..137f21adadafa 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/include/autoware/behavior_velocity_crosswalk_module/util.hpp @@ -78,6 +78,7 @@ struct DebugData std::vector stop_poses; std::vector slow_poses; + std::vector pass_poses; std::vector stop_factor_points; std::vector crosswalk_polygon; std::vector> ego_polygons; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml index 6b939a7dd0be9..0f0b45c2d8940 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_crosswalk_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_crosswalk_module package Satoshi Ota @@ -21,8 +21,11 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_grid_map_utils + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -41,8 +44,6 @@ pluginlib rclcpp sensor_msgs - tier4_api_msgs - tier4_debug_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py index 8a9de1a563249..5b13b6f5752d3 100755 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/scripts/time_to_collision_plotter.py @@ -19,10 +19,10 @@ from copy import deepcopy from ament_index_python.packages import get_package_share_directory +from autoware_internal_debug_msgs.msg import StringStamped import matplotlib.pyplot as plt import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import StringStamped import yaml diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp index 757803ef35578..0ec5a2a944059 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/debug.cpp @@ -213,6 +213,13 @@ autoware::motion_utils::VirtualWalls CrosswalkModule::createVirtualWalls() wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); virtual_walls.push_back(wall); } + wall.style = autoware::motion_utils::VirtualWallType::pass; + wall.text += debug_data_.virtual_wall_suffix; + for (const auto & p : debug_data_.pass_poses) { + wall.pose = calcOffsetPose(p, debug_data_.base_link2front, 0.0, 0.0); + virtual_walls.push_back(wall); + } + return virtual_walls; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 8ba05be36ae56..5bfd31b75976a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -192,8 +192,8 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) // 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_)); + node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_, + time_keeper_, planning_factor_interface_)); generateUUID(crosswalk_lanelet_id); updateRTCStatus( getUUID(crosswalk_lanelet_id), true, State::WAITING_FOR_EXECUTION, @@ -216,7 +216,7 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) } } -std::function &)> +std::function &)> CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; @@ -233,7 +233,7 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) crosswalk_id_set.insert(crosswalk.first->crosswalkLanelet().id()); } - return [crosswalk_id_set](const std::shared_ptr & scene_module) { + return [crosswalk_id_set](const std::shared_ptr & scene_module) { return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp index 693f6c27a380d..091a427e14949 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.hpp @@ -19,11 +19,10 @@ #include #include -#include +#include #include #include -#include #include #include @@ -49,8 +48,8 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const PathWithLaneId & path) override; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 1bea0626b0b2f..43f2f6617e5d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -34,12 +34,12 @@ #include #include #include +#include #include #include #include #include #include - namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -153,11 +153,11 @@ StopFactor createStopFactor( return stop_factor; } -tier4_debug_msgs::msg::StringStamped createStringStampedMessage( +autoware_internal_debug_msgs::msg::StringStamped createStringStampedMessage( const rclcpp::Time & now, const int64_t module_id_, const std::vector> & collision_points) { - tier4_debug_msgs::msg::StringStamped msg; + autoware_internal_debug_msgs::msg::StringStamped msg; msg.stamp = now; for (const auto & collision_point : collision_points) { std::stringstream ss; @@ -175,13 +175,15 @@ CrosswalkModule::CrosswalkModule( rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), planner_param_(planner_param), use_regulatory_element_(reg_elem_id) { - velocity_factor_.init(PlanningBehavior::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { @@ -199,8 +201,8 @@ CrosswalkModule::CrosswalkModule( road_ = lanelet_map_ptr->laneletLayer.get(lane_id); - collision_info_pub_ = - node.create_publisher("~/debug/collision_info", 1); + collision_info_pub_ = node.create_publisher( + "~/debug/collision_info", 1); } bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path) @@ -335,13 +337,8 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop - auto isVehicleType = [](const uint8_t label) { - return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE; - }; std::optional dist_nearest_cp; std::vector stop_factor_points; - const std::optional ego_crosswalk_passage_direction = - findEgoPassageDirectionAlongPath(sparse_resample_path); for (const auto & object : object_info_manager_.getObject()) { const auto & collision_point_opt = object.collision_point; if (collision_point_opt) { @@ -351,19 +348,6 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( continue; } - if ( - isVehicleType(object.classification) && ego_crosswalk_passage_direction && - collision_point.crosswalk_passage_direction) { - double direction_diff = std::abs(std::fmod( - collision_point.crosswalk_passage_direction.value() - - ego_crosswalk_passage_direction.value(), - M_PI_2)); - direction_diff = std::min(direction_diff, M_PI_2 - direction_diff); - if (direction_diff < planner_param_.vehicle_object_cross_angle_threshold) { - continue; - } - } - stop_factor_points.push_back(object.position); const auto dist_ego2cp = @@ -475,12 +459,13 @@ std::optional CrosswalkModule::calcStopPose( return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0; }(); if (selected_stop.dist < strong_brk_dist - p.overrun_threshold_length_for_no_stop_decision) { - RCLCPP_INFO( - logger_, + RCLCPP_INFO_THROTTLE( + logger_, *clock_, 1000, "Abandon to stop. " "Can not stop against the nearest pedestrian with a specified deceleration. " "dist to stop: %f, braking distance: %f", selected_stop.dist, strong_brk_dist); + debug_data_.pass_poses.push_back(selected_stop.pose); return std::nullopt; } @@ -898,9 +883,11 @@ void CrosswalkModule::applySlowDown( } } if (slowdown_pose) - velocity_factor_.set( - output.points, planner_data_->current_odometry->pose, *slowdown_pose, - VelocityFactor::APPROACHING); + planning_factor_interface_->add( + output.points, planner_data_->current_odometry->pose, *slowdown_pose, *slowdown_pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, + safety_slow_down_speed, 0.0 /*shift distance*/, "crosswalk_safety_slowdown_for_approaching"); } void CrosswalkModule::applySlowDownByLanelet2Map( @@ -1162,10 +1149,12 @@ void CrosswalkModule::updateObjectState( const auto collision_point = getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); + const std::optional ego_crosswalk_passage_direction = + findEgoPassageDirectionAlongPath(sparse_resample_path); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, has_traffic_light, collision_point, object.classification.front().label, planner_param_, - crosswalk_.polygon2d().basicPolygon(), attention_area); + crosswalk_.polygon2d().basicPolygon(), attention_area, ego_crosswalk_passage_direction); const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); if (collision_point) { @@ -1377,9 +1366,11 @@ void CrosswalkModule::planStop( // Plan stop insertDecelPointWithDebugInfo(stop_factor->stop_pose.position, 0.0, ego_path); - velocity_factor_.set( + planning_factor_interface_->add( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, - VelocityFactor::UNKNOWN); + stop_factor->stop_pose, tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, + 0.0 /*shift distance*/, "crosswalk_stop"); } bool CrosswalkModule::checkRestartSuppression( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 8577ed1669b48..8601119df8344 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -17,15 +17,15 @@ #include "autoware/behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include #include +#include #include #include -#include #include #include @@ -57,7 +57,6 @@ using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficLightElement; using lanelet::autoware::Crosswalk; -using tier4_api_msgs::msg::CrosswalkStatus; using tier4_planning_msgs::msg::PathWithLaneId; namespace @@ -113,7 +112,7 @@ double InterpolateMap( } } // namespace -class CrosswalkModule : public SceneModuleInterface +class CrosswalkModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -195,16 +194,13 @@ class CrosswalkModule : public SceneModuleInterface const rclcpp::Time & now, const geometry_msgs::msg::Point & position, const double vel, const bool is_ego_yielding, const std::optional & collision_point, const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon, - const bool is_object_away_from_path) + const bool is_object_away_from_path, + const std::optional & ego_crosswalk_passage_direction) { - const bool is_stopped = vel < planner_param.stop_object_velocity; + const bool is_object_stopped = vel < planner_param.stop_object_velocity; // Check if the object can be ignored - if (is_stopped) { - if (collision_state == CollisionState::IGNORE) { - return; - } - + if (is_object_stopped && is_ego_yielding) { if (!time_to_start_stopped) { time_to_start_stopped = now; } @@ -215,7 +211,7 @@ class CrosswalkModule : public SceneModuleInterface planner_param.timeout_set_for_no_intention_to_walk, distance_to_crosswalk); const bool intent_to_cross = (now - *time_to_start_stopped).seconds() < timeout_no_intention_to_walk; - if (is_ego_yielding && !intent_to_cross && is_object_away_from_path) { + if (!intent_to_cross && is_object_away_from_path) { collision_state = CollisionState::IGNORE; return; } @@ -223,8 +219,30 @@ class CrosswalkModule : public SceneModuleInterface time_to_start_stopped = std::nullopt; } + if (is_object_stopped && collision_state == CollisionState::IGNORE) { + return; + } + // Compare time to collision and vehicle if (collision_point) { + auto isVehicleType = [](const uint8_t label) { + return label == ObjectClassification::MOTORCYCLE || + label == ObjectClassification::BICYCLE; + }; + if ( + isVehicleType(classification) && ego_crosswalk_passage_direction && + collision_point->crosswalk_passage_direction) { + double direction_diff = std::abs(std::fmod( + collision_point->crosswalk_passage_direction.value() - + ego_crosswalk_passage_direction.value(), + M_PI_2)); + direction_diff = std::min(direction_diff, M_PI_2 - direction_diff); + if (direction_diff < planner_param.vehicle_object_cross_angle_threshold) { + collision_state = CollisionState::IGNORE; + return; + } + } + // Check if ego will pass first const double ego_pass_first_additional_margin = collision_state == CollisionState::EGO_PASS_FIRST @@ -269,7 +287,8 @@ class CrosswalkModule : public SceneModuleInterface const rclcpp::Time & now, const bool is_ego_yielding, const bool has_traffic_light, const std::optional & collision_point, const uint8_t classification, const PlannerParam & planner_param, const lanelet::BasicPolygon2d & crosswalk_polygon, - const Polygon2d & attention_area) + const Polygon2d & attention_area, + const std::optional & ego_crosswalk_passage_direction) { // update current uuids current_uuids_.push_back(uuid); @@ -293,7 +312,7 @@ class CrosswalkModule : public SceneModuleInterface // update object state objects.at(uuid).transitState( now, position, vel, is_ego_yielding, collision_point, planner_param, crosswalk_polygon, - is_object_away_from_path); + is_object_away_from_path, ego_crosswalk_passage_direction); objects.at(uuid).collision_point = collision_point; objects.at(uuid).position = position; objects.at(uuid).classification = classification; @@ -335,7 +354,10 @@ class CrosswalkModule : public SceneModuleInterface rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; @@ -457,7 +479,8 @@ class CrosswalkModule : public SceneModuleInterface const int64_t module_id_; - rclcpp::Publisher::SharedPtr collision_info_pub_; + rclcpp::Publisher::SharedPtr + collision_info_pub_; lanelet::ConstLanelet crosswalk_; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..7a875327d4dde --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "crosswalk", "autoware::behavior_velocity_planner::CrosswalkModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CHANGELOG.rst index fc13807261e4f..15342f4fdc1f4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CHANGELOG.rst @@ -2,6 +2,67 @@ Changelog for package autoware_behavior_velocity_detection_area_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* feat(behavior_velocity_detection_area): use base class without RTC (`#9802 `_) + * feat(behavior_velocity_detection_area): use base class without RTC + * fix + * fix + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt index 3aff4a524ffdd..53eafaffbba6c 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/CMakeLists.txt @@ -14,6 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml index b49b43794685c..9294795274066 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/config/detection_area.param.yaml @@ -8,5 +8,4 @@ state_clear_time: 2.0 hold_stop_margin_distance: 0.0 distance_to_judge_over_stop_line: 0.5 - enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval suppress_pass_judge_when_stopping: false diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml index ff91cf40a32a6..aff8d6238f4fb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_detection_area_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_detection_area_module package Kyoichi Sugahara @@ -17,6 +17,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp index 3f39696ff6bcc..f231d7adc3326 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.cpp @@ -34,8 +34,7 @@ using autoware::universe_utils::getOrDeclareParameter; using lanelet::autoware::DetectionArea; DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterfaceWithRTC( - node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) +: SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(DetectionAreaModuleManager::getModuleName()); planner_param_.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); @@ -65,11 +64,8 @@ void DetectionAreaModuleManager::launchNewModules( if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *detection_area_with_lane_id.first, planner_param_, - logger_.get_child("detection_area_module"), clock_)); - generateUUID(module_id); - updateRTCStatus( - getUUID(module_id), true, State::WAITING_FOR_EXECUTION, - std::numeric_limits::lowest(), path.header.stamp); + logger_.get_child("detection_area_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp index 7aa60cbbaa18b..4b34ac4a45298 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC +class DetectionAreaModuleManager : public SceneModuleManagerInterface<> { public: explicit DetectionAreaModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp index 61b3b185999d8..8641ce19e4ae5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.cpp @@ -36,15 +36,17 @@ DetectionAreaModule::DetectionAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) @@ -105,12 +107,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) modified_stop_line_seg_idx = current_seg_idx; } - setDistance(stop_dist); - // Check state - setSafe(detection_area::can_clear_stop_state( - last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time)); - if (isActivated()) { + const bool is_safe = detection_area::can_clear_stop_state( + last_obstacle_found_time_, clock_->now(), planner_param_.state_clear_time); + if (is_safe) { last_obstacle_found_time_ = {}; if (!planner_param_.suppress_pass_judge_when_stopping || !is_stopped) { state_ = State::GO; @@ -139,7 +139,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) dead_line_seg_idx); if (dist_from_ego_to_dead_line < 0.0) { RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line"); - setSafe(true); return true; } } @@ -152,7 +151,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) if ( state_ != State::STOP && dist_from_ego_to_stop < -planner_param_.distance_to_judge_over_stop_line) { - setSafe(true); return true; } @@ -169,7 +167,6 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) RCLCPP_WARN_THROTTLE( logger_, *clock_, std::chrono::milliseconds(1000).count(), "[detection_area] vehicle is over stop border"); - setSafe(true); return true; } } @@ -183,9 +180,10 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); } return true; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp index 9224cf4624687..42cc0461cd59d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/src/scene.hpp @@ -68,7 +68,10 @@ class DetectionAreaModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..f84d22debea8e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_detection_area_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "detection_area", "autoware::behavior_velocity_planner::DetectionAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CHANGELOG.rst index 013fd3b9b54cb..390c1c065d632 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/CHANGELOG.rst @@ -2,6 +2,70 @@ Changelog for package autoware_behavior_velocity_intersection_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(intersection): add wall marker for too late detect objects (`#10006 `_) +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* fix(autoware_behavior_velocity_intersection_module): fix bugprone-branch-clone (`#9702 `_) + fix: bugprone-error +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs (`#9744 `_) + * feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs + * fix + --------- +* feat(behavior_velocity_planner): remove unnecessary tier4_api_msgs (`#9692 `_) +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml index b056f71614da3..644bcbe7a3de7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_intersection_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_intersection_module package Mamoru Sobue @@ -19,7 +19,10 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py index 1eb6ae1ffafc1..c1bed003ea3a9 100755 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/scripts/ttc.py @@ -22,13 +22,13 @@ import time from PIL import Image +from autoware_internal_debug_msgs.msg import Float64MultiArrayStamped import imageio import matplotlib import matplotlib.pyplot as plt import numpy as np import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float64MultiArrayStamped matplotlib.use("TKAgg") diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp index e50bc041cbc89..2c9a2c30b11b8 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -446,6 +446,14 @@ autoware::motion_utils::VirtualWalls IntersectionModule::createVirtualWalls() wall.pose = debug_data_.absence_traffic_light_creep_wall.value(); virtual_walls.push_back(wall); } + if (debug_data_.too_late_stop_wall_pose) { + wall.style = autoware::motion_utils::VirtualWallType::pass; + wall.text = "intersection"; + wall.detail = "too late to stop"; + wall.ns = "intersection" + std::to_string(module_id_) + "_"; + wall.pose = debug_data_.too_late_stop_wall_pose.value(); + virtual_walls.push_back(wall); + } return virtual_walls; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp index 39ed8e80412a6..d6f7dfbac92d4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -338,7 +338,8 @@ void IntersectionModuleManager::launchNewModules( } 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_); + has_traffic_light, node_, logger_.get_child("intersection_module"), clock_, time_keeper_, + planning_factor_interface_); generateUUID(module_id); /* set RTC status as non_occluded status initially */ const UUID uuid = getUUID(new_module->getModuleId()); @@ -353,14 +354,14 @@ void IntersectionModuleManager::launchNewModules( } } -std::function &)> +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 [lane_set](const std::shared_ptr & scene_module) { + return [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) { @@ -526,7 +527,8 @@ void MergeFromPrivateModuleManager::launchNewModules( 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_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } else { @@ -540,7 +542,8 @@ void MergeFromPrivateModuleManager::launchNewModules( 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_)); + logger_.get_child("merge_from_private_road_module"), clock_, time_keeper_, + planning_factor_interface_)); continue; } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp index 8189cbe7bc5d7..c6f76d7c39640 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -21,10 +21,10 @@ #include #include #include +#include #include #include -#include #include #include @@ -48,8 +48,8 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - 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; @@ -64,7 +64,7 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC tl_observation_pub_; }; -class MergeFromPrivateModuleManager : public SceneModuleManagerInterface +class MergeFromPrivateModuleManager : public SceneModuleManagerInterface<> { public: explicit MergeFromPrivateModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 6f7841ebb0bbb..b4e1f313a6936 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -18,7 +18,6 @@ #include // for toGeomPoly #include -#include #include #include // for toPolygon2d #include @@ -45,15 +44,16 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using autoware::motion_utils::VelocityFactorInterface; - IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), lane_id_(lane_id), associative_ids_(associative_ids), @@ -61,8 +61,6 @@ IntersectionModule::IntersectionModule( has_traffic_light_(has_traffic_light), occlusion_uuid_(autoware::universe_utils::generateUUID()) { - velocity_factor_.init(PlanningBehavior::INTERSECTION); - { collision_state_machine_.setMarginTime( planner_param_.collision_detection.collision_detection_hold_time); @@ -88,10 +86,11 @@ IntersectionModule::IntersectionModule( static_occlusion_timeout_state_machine_.setState(StateMachine::State::STOP); } - ego_ttc_pub_ = node.create_publisher( + ego_ttc_pub_ = node.create_publisher( "~/debug/intersection/ego_ttc", 1); - object_ttc_pub_ = node.create_publisher( - "~/debug/intersection/object_ttc", 1); + object_ttc_pub_ = + node.create_publisher( + "~/debug/intersection/object_ttc", 1); } bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path) @@ -231,7 +230,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // calculate the expected vehicle speed and obtain the spatiotemporal profile of ego to the // exit of intersection // ========================================================================================== - tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = calcIntersectionPassingTime(*path, is_prioritized, intersection_stoplines, &ego_ttc_time_array); @@ -240,7 +239,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat // passed each pass judge line for the first time, save current collision status for late // diagnosis // ========================================================================================== - tier4_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped object_ttc_time_array; updateObjectInfoManagerCollision( path_lanelets, time_distance_array, traffic_prioritized_level, safely_passed_1st_judge_line, safely_passed_2nd_judge_line, &object_ttc_time_array); @@ -274,6 +273,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat 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); + debug_data_.too_late_stop_wall_pose = path->points.at(default_stopline_idx).point.pose; return OverPassJudge{safety_diag, evasive_diag}; } return OverPassJudge{ @@ -353,9 +353,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < 0.0); const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - state_machine.setState(StateMachine::State::GO); - } else if (is_stopped_duration && approached_dist_stopline) { + if (over_stopline || (is_stopped_duration && approached_dist_stopline)) { state_machine.setState(StateMachine::State::GO); } return state_machine.getState() == StateMachine::State::GO; @@ -366,12 +364,7 @@ DecisionResult IntersectionModule::modifyPathVelocityDetail(PathWithLaneId * pat (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); const bool is_stopped = planner_data_->isVehicleStopped(duration); - if (over_stopline) { - return true; - } else if (is_stopped && approached_dist_stopline) { - return true; - } - return false; + return over_stopline || (is_stopped && approached_dist_stopline); }; const auto occlusion_wo_tl_pass_judge_line_idx = @@ -711,7 +704,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -726,7 +719,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -740,7 +733,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, - [[maybe_unused]] VelocityFactorInterface * velocity_factor, + [[maybe_unused]] planning_factor_interface::PlanningFactorInterface * planning_factor_interface, [[maybe_unused]] IntersectionModule::DebugData * debug_data) { return; @@ -752,7 +745,8 @@ void reactRTCApprovalByDecisionResult( const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -766,9 +760,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(pure StuckStop)"); } } if (!rtc_occlusion_approved && decision_result.occlusion_stopline_idx) { @@ -777,9 +774,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(occlusion_stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(occlusion_stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_stopline_idx).point.pose, + path->points.at(occlusion_stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(StuckStop with occlusion)"); } } return; @@ -791,7 +792,8 @@ void reactRTCApprovalByDecisionResult( const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -805,9 +807,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Yield Stuck)"); } } return; @@ -819,7 +824,8 @@ void reactRTCApprovalByDecisionResult( const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -831,9 +837,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop)"); } } if (!rtc_occlusion_approved) { @@ -842,9 +851,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -855,7 +867,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -868,9 +881,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_first_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with collision)"); } } if (!rtc_occlusion_approved) { @@ -887,9 +903,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FirstWaitBeforeOcclusion with occlusion)"); } } return; @@ -900,7 +919,8 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - tier4_planning_msgs::msg::PathWithLaneId * path, VelocityFactorInterface * velocity_factor, + tier4_planning_msgs::msg::PathWithLaneId * path, + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -926,9 +946,13 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(occlusion_peeking_stopline).point.pose, VelocityFactor::UNKNOWN); + path->points.at(occlusion_peeking_stopline).point.pose, + path->points.at(occlusion_peeking_stopline).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion)"); } } if (!rtc_default_approved) { @@ -937,9 +961,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(PeekingToOcclusion while stopping for collision)"); } } return; @@ -951,7 +978,8 @@ void reactRTCApprovalByDecisionResult( const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -963,9 +991,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } if (!rtc_occlusion_approved) { @@ -978,9 +1009,12 @@ void reactRTCApprovalByDecisionResult( debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(CollisionStop with occlusion)"); } } return; @@ -992,7 +1026,8 @@ void reactRTCApprovalByDecisionResult( const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1004,9 +1039,13 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(Occlusion without traffic light, collision detected)"); } } if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { @@ -1015,9 +1054,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Occlusion without traffic light)"); } } if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { @@ -1038,7 +1080,8 @@ void reactRTCApprovalByDecisionResult( 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, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1049,9 +1092,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for collision)"); } } if (!rtc_occlusion_approved) { @@ -1060,9 +1106,12 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(Safe, but RTC interrupted for occlusion)"); } } return; @@ -1074,7 +1123,8 @@ void reactRTCApprovalByDecisionResult( const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, - VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) + planning_factor_interface::PlanningFactorInterface * planning_factor_interface, + IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), @@ -1086,9 +1136,12 @@ void reactRTCApprovalByDecisionResult( debug_data->collision_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, "intersection(FullyPrioritized, collision detected)"); } } if (!rtc_occlusion_approved) { @@ -1097,9 +1150,13 @@ void reactRTCApprovalByDecisionResult( debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { - velocity_factor->set( + planning_factor_interface->add( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); + path->points.at(stopline_idx).point.pose, path->points.at(stopline_idx).point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, + "intersection(FullyPrioritized, RTC for occlusion is interrupting)"); } } return; @@ -1113,7 +1170,7 @@ void IntersectionModule::reactRTCApproval( VisitorSwitch{[&](const auto & decision) { reactRTCApprovalByDecisionResult( activated_, occlusion_activated_, decision, planner_param_, baselink2front, path, - &velocity_factor_, &debug_data_); + planning_factor_interface_.get(), &debug_data_); }}, decision_result); return; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b718dd84d33af..afba73ad45922 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,12 +22,12 @@ #include "object_manager.hpp" #include "result.hpp" -#include #include +#include #include #include -#include +#include #include #include @@ -46,7 +46,7 @@ namespace autoware::behavior_velocity_planner { -class IntersectionModule : public SceneModuleInterface +class IntersectionModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -214,6 +214,7 @@ class IntersectionModule : public SceneModuleInterface bool passed_first_pass_judge{false}; bool passed_second_pass_judge{false}; std::optional absence_traffic_light_creep_wall{std::nullopt}; + std::optional too_late_stop_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; @@ -304,7 +305,10 @@ class IntersectionModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, rclcpp::Node & node, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); /** *********************************************************** @@ -748,7 +752,7 @@ class IntersectionModule : public SceneModuleInterface 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); + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, @@ -809,13 +813,15 @@ class IntersectionModule : public SceneModuleInterface TimeDistanceArray calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const; /** @} */ mutable DebugData debug_data_; mutable InternalDebugData internal_debug_data_{}; - rclcpp::Publisher::SharedPtr ego_ttc_pub_; - rclcpp::Publisher::SharedPtr object_ttc_pub_; + rclcpp::Publisher::SharedPtr + ego_ttc_pub_; + rclcpp::Publisher::SharedPtr + object_ttc_pub_; }; } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 5a66bec15bab1..5103cd6cc46e4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -152,7 +152,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( 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, - tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array) + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array) { const auto & intersection_lanelets = intersection_lanelets_.value(); @@ -815,7 +815,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const IntersectionStopLines & intersection_stoplines, - tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const + autoware_internal_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { const double intersection_velocity = planner_param_.collision_detection.velocity_profile.default_velocity; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 3dfdcc36c0cff..d12b58a63e531 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -41,12 +41,14 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), associative_ids_(associative_ids) { - velocity_factor_.init(PlanningBehavior::MERGE); planner_param_ = planner_param; state_machine_.setState(StateMachine::State::STOP); } @@ -153,8 +155,10 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path) /* get stop point and stop factor */ const auto & stop_pose = path->points.at(stopline_idx).point.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "merge_from_private"); const double signed_arc_dist_to_stop_point = autoware::motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index dc8bf1a96a7a2..4b9c0a9c5547e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -61,7 +61,10 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, const PlannerParam & planner_param, const std::set & associative_ids, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); /** * @brief plan go-stop velocity at traffic crossing with collision check between reference path diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..b515107e0ae8e --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "intersection", "autoware::behavior_velocity_planner::IntersectionModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/CHANGELOG.rst index 3ad226a0a0f71..fc7e5f832e69b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/CHANGELOG.rst @@ -2,6 +2,52 @@ Changelog for package autoware_behavior_velocity_no_drivable_lane_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml index 3ac9bab0c2fdf..190583f583981 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_no_drivable_lane_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_no_drivable_lane_module package Ahmed Ebrahim diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp index 9cb1153a8edd2..517568c93bd35 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -51,7 +51,8 @@ void NoDrivableLaneModuleManager::launchNewModules( } registerModule(std::make_shared( - module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_)); + module_id, lane_id, planner_param_, logger_.get_child("no_drivable_lane_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp index 545af1576c6a8..bea068f5b9579 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class NoDrivableLaneModuleManager : public SceneModuleManagerInterface +class NoDrivableLaneModuleManager : public SceneModuleManagerInterface<> { public: explicit NoDrivableLaneModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp index 8365251904b18..bbca1bf2d6481 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -21,20 +21,24 @@ #include #include +#include + namespace autoware::behavior_velocity_planner { using autoware::universe_utils::createPoint; NoDrivableLaneModule::NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), planner_param_(planner_param), debug_data_(), state_(State::INIT) { - velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path) @@ -162,8 +166,10 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = op_stop_pose.value(); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -210,8 +216,10 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state(PathWithLaneId * // Get stop point and stop factor { const auto & stop_pose = autoware::universe_utils::getPose(path->points.at(0)); - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto & virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); @@ -248,8 +256,10 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path) // Get stop point and stop factor { const auto & stop_pose = ego_pos_on_path.pose; - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::STOPPED); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); const auto virtual_wall_pose = autoware::motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp index d8c5e3e0425d1..5490a37c7ec79 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -55,7 +55,10 @@ class NoDrivableLaneModule : public SceneModuleInterface NoDrivableLaneModule( const int64_t module_id, const int64_t lane_id, const PlannerParam & planner_param, - const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CHANGELOG.rst index 177028582aceb..eb26b4132216d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CHANGELOG.rst @@ -2,6 +2,62 @@ Changelog for package autoware_behavior_velocity_no_stopping_area_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt index b710924410549..e6b362271afb3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/CMakeLists.txt @@ -14,6 +14,7 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) endif() diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml index 88fafeb5b90dc..b9af47f6729c9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_no_stopping_area_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_no_stopping_area_module package Kosuke Takeuchi @@ -17,7 +17,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_interpolation autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp index 9d66aa6672d36..55b6c0dffd1a4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -60,7 +60,7 @@ void NoStoppingAreaModuleManager::launchNewModules( // 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_)); + clock_, time_keeper_, planning_factor_interface_)); generateUUID(module_id); updateRTCStatus( getUUID(module_id), true, State::WAITING_FOR_EXECUTION, @@ -69,16 +69,17 @@ void NoStoppingAreaModuleManager::launchNewModules( } } -std::function &)> +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; - }; + 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 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp index 7009e94612580..523cbba291632 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -41,8 +41,8 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoStoppingAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 3769aed71a1ec..14f4f7b62c1f7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -40,14 +41,16 @@ NoStoppingAreaModule::NoStoppingAreaModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterfaceWithRTC(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param), debug_data_() { - velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -141,9 +144,11 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path) // Create StopReason { - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_point->second, - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_point->second, stop_point->second, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0, + 0.0 /*shift distance*/, ""); } } else if (state_machine_.getState() == StateMachine::State::GO) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 51f3a0d261ebd..036b6e30509c4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -17,9 +17,9 @@ #include "utils.hpp" -#include #include #include +#include #include #include @@ -31,7 +31,7 @@ namespace autoware::behavior_velocity_planner { -class NoStoppingAreaModule : public SceneModuleInterface +class NoStoppingAreaModule : public SceneModuleInterfaceWithRTC { public: struct PlannerParam @@ -57,7 +57,10 @@ class NoStoppingAreaModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..875e757cbfcec --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_no_stopping_area_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "no_stopping_area", "autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/CHANGELOG.rst index e74a81a6a971a..3d17b244e890a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/CHANGELOG.rst @@ -2,6 +2,56 @@ Changelog for package autoware_behavior_velocity_occlusion_spot_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix(autoware_behavior_velocity_occlusion_spot_module): fix bugprone-macro-parentheses (`#9712 `_) + * fix: bugprone-error + * fix: fmt + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml index 66cb33192fec0..c4b26150a6670 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_occlusion_spot_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_occlusion_spot_module package Taiki Tanaka diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index c4dace9d49244..d28cff55a7a8d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -123,8 +123,8 @@ void OcclusionSpotModuleManager::launchNewModules( // general if (!isModuleRegistered(module_id_)) { registerModule(std::make_shared( - module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), - clock_)); + module_id_, planner_data_, planner_param_, logger_.get_child("occlusion_spot_module"), clock_, + time_keeper_, planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 32b1818214952..08c1516dea67a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -39,7 +39,7 @@ namespace autoware::behavior_velocity_planner { -class OcclusionSpotModuleManager : public SceneModuleManagerInterface +class OcclusionSpotModuleManager : public SceneModuleManagerInterface<> { public: explicit OcclusionSpotModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 522e83390b0f1..935b1ec125024 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -36,7 +36,7 @@ // turn on only when debugging. #define DEBUG_PRINT(enable, n, x) \ if (enable) { \ - const std::string time_msg = n + std::to_string(x); \ + const std::string time_msg = (n) + std::to_string(x); \ RCLCPP_INFO_STREAM_THROTTLE(logger_, *clock_, 3000, time_msg); \ } @@ -64,11 +64,13 @@ namespace utils = occlusion_spot_utils; OcclusionSpotModule::OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), param_(planner_param) + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + param_(planner_param) { - velocity_factor_.init(PlanningBehavior::UNKNOWN); - if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; //! occupancy grid limitation( 100 * 100 ) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 35c409a9c459b..c744cdcb22b84 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -48,7 +48,10 @@ class OcclusionSpotModule : public SceneModuleInterface OcclusionSpotModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); /** * @brief plan occlusion spot velocity at unknown area in occupancy grid diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CHANGELOG.rst index 43eafef522dde..5bcc3d250f51e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CHANGELOG.rst @@ -2,6 +2,31 @@ Changelog for package autoware_behavior_velocity_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(behavior_velocity_planner): remove virtual traffic light dependency from behavior_velocity_planner and behavior_velocity_planner_common (`#9746 `_) + * feat: remove-virtual-traffic-light-dependency-from-plugin-manager + * build passed + * fix test + * fix test + * fix + * fix typo + --------- +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner): independent of plugin packages (`#9760 `_) +* feat(behavior_velocity_planner): remove unnecessary tier4_api_msgs (`#9692 `_) +* Contributors: Fumiya Watanabe, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt index 37a02b844dfe9..a520b02a43c05 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/CMakeLists.txt @@ -2,20 +2,13 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_planner) find_package(autoware_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -rosidl_generate_interfaces( - ${PROJECT_NAME} - "srv/LoadPlugin.srv" - "srv/UnloadPlugin.srv" - DEPENDENCIES -) autoware_package() ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/node.cpp src/planner_manager.cpp + src/test_utils.cpp ) rclcpp_components_register_node(${PROJECT_NAME}_lib @@ -23,18 +16,9 @@ rclcpp_components_register_node(${PROJECT_NAME}_lib EXECUTABLE ${PROJECT_NAME}_node ) -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(${PROJECT_NAME}_lib - ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") -endif() - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/src/test_node_interface.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp similarity index 83% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp index e260f582aae60..472538406c382 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/node.hpp @@ -12,27 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NODE_HPP_ -#define NODE_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ +#include "autoware/behavior_velocity_planner/planner_manager.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" -#include "planner_manager.hpp" #include #include -#include -#include #include +#include #include #include #include #include #include #include -#include -#include #include #include #include @@ -47,8 +44,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 tier4_planning_msgs::msg::VelocityLimit; @@ -86,10 +81,6 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware::universe_utils::InterProcessPollingSubscriber< - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> - sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; - autoware::universe_utils::InterProcessPollingSubscriber sub_occupancy_grid_{this, "~/input/occupancy_grid"}; @@ -127,13 +118,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; - rclcpp::Service::SharedPtr srv_load_plugin_; - rclcpp::Service::SharedPtr srv_unload_plugin_; + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; void onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - const UnloadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); void onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response); // mutex for planner_data_ std::mutex mutex_; @@ -152,4 +144,4 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node }; } // namespace autoware::behavior_velocity_planner -#endif // NODE_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__NODE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp similarity index 91% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp index fddd658cef06e..9bd423ecfef21 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/planner_manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNER_MANAGER_HPP_ -#define PLANNER_MANAGER_HPP_ +#ifndef AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ #include #include @@ -57,4 +57,4 @@ class BehaviorVelocityPlannerManager }; } // namespace autoware::behavior_velocity_planner -#endif // PLANNER_MANAGER_HPP_ +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__PLANNER_MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.hpp new file mode 100644 index 0000000000000..f34182e77e6f1 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/include/autoware/behavior_velocity_planner/test_utils.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 AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ + +#include "autoware/behavior_velocity_planner/node.hpp" + +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; +using autoware::planning_test_manager::PlanningInterfaceTestManager; + +struct PluginInfo +{ + std::string module_name; // e.g. crosswalk + std::string plugin_name; // e.g. autoware::behavior_velocity_planner::CrosswalkModulePlugin +}; + +std::shared_ptr generateTestManager(); + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec); + +void publishMandatoryTopics( + std::shared_ptr test_manager, + std::shared_ptr test_target_node); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER__TEST_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml index 8ba8272712364..d68937727773f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_planner - 0.40.0 + 0.41.0 The autoware_behavior_velocity_planner package Mamoru Sobue @@ -32,9 +32,8 @@ autoware_cmake eigen3_cmake_module - rosidl_default_generators - autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils @@ -56,33 +55,14 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs - tier4_v2x_msgs visualization_msgs - rosidl_default_runtime - ament_cmake_ros ament_lint_auto - autoware_behavior_velocity_blind_spot_module - autoware_behavior_velocity_crosswalk_module - autoware_behavior_velocity_detection_area_module - autoware_behavior_velocity_intersection_module - autoware_behavior_velocity_no_drivable_lane_module - autoware_behavior_velocity_no_stopping_area_module - autoware_behavior_velocity_occlusion_spot_module - autoware_behavior_velocity_run_out_module - autoware_behavior_velocity_speed_bump_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 - rosidl_interface_packages - ament_cmake diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp index d78bc883e6b35..443bfebe2a3eb 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "node.hpp" +#include "autoware/behavior_velocity_planner/node.hpp" #include #include @@ -70,9 +70,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1)); - srv_load_plugin_ = create_service( + srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); - srv_unload_plugin_ = create_service( + srv_unload_plugin_ = create_service( "~/service/unload_plugin", std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); @@ -112,19 +112,19 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } void BehaviorVelocityPlannerNode::onLoadPlugin( - const LoadPlugin::Request::SharedPtr request, - [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.launchScenePlugin(*this, request->plugin_name); + planner_manager_.launchScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onUnloadPlugin( - const UnloadPlugin::Request::SharedPtr request, - [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) + const autoware_internal_debug_msgs::srv::String::Request::SharedPtr request, + [[maybe_unused]] const autoware_internal_debug_msgs::srv::String::Response::SharedPtr response) { std::unique_lock lk(mutex_); - planner_manager_.removeScenePlugin(*this, request->plugin_name); + planner_manager_.removeScenePlugin(*this, request->data); } void BehaviorVelocityPlannerNode::onParam() @@ -271,9 +271,6 @@ bool BehaviorVelocityPlannerNode::processData(rclcpp::Clock clock) planner_data_.route_handler_ = std::make_shared(*map_data); } - // optional data - getData(planner_data_.virtual_traffic_light_states, sub_virtual_traffic_light_states_); - // planner_data_.external_velocity_limit is std::optional type variable. const auto external_velocity_limit = sub_external_velocity_limit_.takeData(); if (external_velocity_limit) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp index 4820c340058ff..45ee83260d53a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planner_manager.hpp" +#include "autoware/behavior_velocity_planner/planner_manager.hpp" #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp new file mode 100644 index 0000000000000..8b9f39e97d22b --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/src/test_utils.cpp @@ -0,0 +1,119 @@ +// 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_velocity_planner/test_utils.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + + // set subscriber with topic name: behavior_velocity_planner → test_node_ + test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); + + // set behavior_velocity_planner node's input topic name(this topic is changed to test node) + test_manager->setPathWithLaneIdTopicName( + "behavior_velocity_planner_node/input/path_with_lane_id"); + + test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); + + return test_manager; +} + +std::shared_ptr generateNode( + const std::vector & plugin_info_vec) +{ + auto node_options = rclcpp::NodeOptions{}; + + const auto autoware_test_utils_dir = + ament_index_cpp::get_package_share_directory("autoware_test_utils"); + const auto behavior_velocity_planner_common_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); + const auto behavior_velocity_planner_dir = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); + const auto velocity_smoother_dir = + ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); + + const auto get_behavior_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_behavior_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + + std::vector plugin_names; + for (const auto & plugin_info : plugin_info_vec) { + plugin_names.emplace_back(plugin_info.plugin_name); + } + + std::vector params; + params.emplace_back("launch_modules", plugin_names); + params.emplace_back("is_simulation", false); + node_options.parameter_overrides(params); + + auto yaml_files = std::vector{ + autoware_test_utils_dir + "/config/test_common.param.yaml", + autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", + autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", + velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", + velocity_smoother_dir + "/config/Analytical.param.yaml", + behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", + behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml"}; + for (const auto & plugin_info : plugin_info_vec) { + yaml_files.push_back(get_behavior_velocity_module_config(plugin_info.module_name)); + } + + autoware::test_utils::updateNodeOptions(node_options, yaml_files); + + // TODO(Takagi, Isamu): set launch_modules + // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light + // TODO(Kyoichi Sugahara) set to true launch_occlusion_spot + // TODO(Kyoichi Sugahara) set to true launch_run_out + // TODO(Kyoichi Sugahara) set to true launch_speed_bump + + 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->publishTF(test_target_node, "/tf"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishPredictedObjects( + test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); + test_manager->publishPointCloud( + test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); + test_manager->publishOdometry( + test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); + test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); + test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); + test_manager->publishTrafficSignals( + test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); + test_manager->publishMaxVelocity( + test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); + test_manager->publishOccupancyGrid( + test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp deleted file mode 100644 index fe79450d0def8..0000000000000 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ /dev/null @@ -1,179 +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 "node.hpp" - -#include -#include -#include - -#include - -#include -#include -#include -#include - -using autoware::behavior_velocity_planner::BehaviorVelocityPlannerNode; -using autoware::planning_test_manager::PlanningInterfaceTestManager; - -std::shared_ptr generateTestManager() -{ - auto test_manager = std::make_shared(); - - // set subscriber with topic name: behavior_velocity_planner → test_node_ - test_manager->setPathSubscriber("behavior_velocity_planner_node/output/path"); - - // set behavior_velocity_planner node's input topic name(this topic is changed to test node) - test_manager->setPathWithLaneIdTopicName( - "behavior_velocity_planner_node/input/path_with_lane_id"); - - test_manager->setInitialPoseTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->setOdometryTopicName("behavior_velocity_planner_node/input/vehicle_odometry"); - - return test_manager; -} - -std::shared_ptr generateNode() -{ - auto node_options = rclcpp::NodeOptions{}; - - const auto autoware_test_utils_dir = - ament_index_cpp::get_package_share_directory("autoware_test_utils"); - const auto behavior_velocity_planner_common_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner_common"); - const auto behavior_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_planner"); - const auto velocity_smoother_dir = - ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - - const auto get_behavior_velocity_module_config = [](const std::string & module) { - const auto package_name = "autoware_behavior_velocity_" + module + "_module"; - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return package_path + "/config/" + module + ".param.yaml"; - }; - - std::vector module_names; - 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("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); - params.emplace_back("is_simulation", false); - node_options.parameter_overrides(params); - - autoware::test_utils::updateNodeOptions( - node_options, - {autoware_test_utils_dir + "/config/test_common.param.yaml", - autoware_test_utils_dir + "/config/test_nearest_search.param.yaml", - autoware_test_utils_dir + "/config/test_vehicle_info.param.yaml", - velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", - velocity_smoother_dir + "/config/Analytical.param.yaml", - behavior_velocity_planner_common_dir + "/config/behavior_velocity_planner_common.param.yaml", - behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - 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_drivable_lane")}); - - // TODO(Takagi, Isamu): set launch_modules - // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light - // TODO(Kyoichi Sugahara) set to true launch_occlusion_spot - // TODO(Kyoichi Sugahara) set to true launch_run_out - // TODO(Kyoichi Sugahara) set to true launch_speed_bump - - 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->publishTF(test_target_node, "/tf"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishPredictedObjects( - test_target_node, "behavior_velocity_planner_node/input/dynamic_objects"); - test_manager->publishPointCloud( - test_target_node, "behavior_velocity_planner_node/input/no_ground_pointcloud"); - test_manager->publishOdometry( - test_target_node, "behavior_velocity_planner_node/input/vehicle_odometry"); - test_manager->publishAcceleration(test_target_node, "behavior_velocity_planner_node/input/accel"); - test_manager->publishMap(test_target_node, "behavior_velocity_planner_node/input/vector_map"); - test_manager->publishTrafficSignals( - test_target_node, "behavior_velocity_planner_node/input/traffic_signals"); - test_manager->publishMaxVelocity( - test_target_node, "behavior_velocity_planner_node/input/external_velocity_limit_mps"); - test_manager->publishVirtualTrafficLightState( - test_target_node, "behavior_velocity_planner_node/input/virtual_traffic_light_states"); - test_manager->publishOccupancyGrid( - test_target_node, "behavior_velocity_planner_node/input/occupancy_grid"); -} - -TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) -{ - rclcpp::init(0, nullptr); - auto test_manager = generateTestManager(); - auto test_target_node = generateNode(); - - publishMandatoryTopics(test_manager, test_target_node); - - // test with nominal path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - // test with empty path_with_lane_id - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(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->testWithNominalPathWithLaneId(test_target_node)); - - // make sure behavior_path_planner is running - EXPECT_GE(test_manager->getReceivedTopicNum(), 1); - - ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); - - rclcpp::shutdown(); -} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp new file mode 100644 index 0000000000000..6e232318c1711 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/test/test_node_interface.cpp @@ -0,0 +1,61 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware/behavior_velocity_planner/test_utils.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CHANGELOG.rst index efc6fd8e18cb6..c229807e51a5d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/CHANGELOG.rst @@ -2,6 +2,66 @@ Changelog for package autoware_behavior_velocity_planner_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix(behavior_velocity_planner_common): fix unregister process (`#9913 `_) +* feat(behavior_velocity_planner): remove virtual traffic light dependency from behavior_velocity_planner and behavior_velocity_planner_common (`#9746 `_) + * feat: remove-virtual-traffic-light-dependency-from-plugin-manager + * build passed + * fix test + * fix test + * fix + * fix typo + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs (`#9744 `_) + * feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs + * fix + --------- +* feat(behavior_velocity_planner): remove unnecessary tier4_api_msgs (`#9692 `_) +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka, Yuki TAKAGI + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md index 2abbb83575af5..c4c8d97b4b390 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/README.md @@ -1,3 +1,3 @@ # Behavior Velocity Planner Common -This package provides common functions as a library, which are used in the `behavior_velocity_planner` node and modules. +This package provides a behavior velocity interface without RTC, and common functions as a library, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp index 309ba33a3498a..f8b37999cb6bf 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp @@ -29,10 +29,7 @@ #include #include #include -#include -#include #include -#include #include #include @@ -66,7 +63,6 @@ struct PlannerData std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; std::optional external_velocity_limit; - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; bool is_simulation = false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp index fadda66f12562..743fd0e31e387 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp @@ -16,22 +16,20 @@ #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 @@ -50,18 +48,14 @@ namespace autoware::behavior_velocity_planner { -using autoware::motion_utils::PlanningBehavior; -using autoware::motion_utils::VelocityFactor; using autoware::objects_of_interest_marker_interface::ColorName; using autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; -using autoware::rtc_interface::RTCInterface; using autoware::universe_utils::DebugPublisher; using autoware::universe_utils::getOrDeclareParameter; +using autoware::universe_utils::StopWatch; +using autoware_internal_debug_msgs::msg::Float64Stamped; using builtin_interfaces::msg::Time; -using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::PathWithLaneId; -using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::msg::State; using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest @@ -81,7 +75,10 @@ class SceneModuleInterface { public: explicit SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); virtual ~SceneModuleInterface() = default; virtual bool modifyPathVelocity(PathWithLaneId * path) = 0; @@ -96,58 +93,17 @@ class SceneModuleInterface planner_data_ = planner_data; } - std::optional getInfrastructureCommand() - { - return infrastructure_command_; - } - - void setInfrastructureCommand( - const std::optional & command) - { - infrastructure_command_ = command; - } - - void setTimeKeeper(const std::shared_ptr & time_keeper) - { - time_keeper_ = time_keeper; - } - - std::shared_ptr getTimeKeeper() { return time_keeper_; } - - 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_; - autoware::motion_utils::VelocityFactorInterface velocity_factor_; std::vector objects_of_interest_; mutable std::shared_ptr time_keeper_; - - void setSafe(const bool safe) - { - safe_ = safe; - if (!rtc_enabled_) { - syncActivation(); - } - } - void setDistance(const double distance) { distance_ = distance; } - void syncActivation() { setActivation(isSafe()); } + std::shared_ptr planning_factor_interface_; void setObjectsOfInterestData( const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, @@ -160,10 +116,36 @@ class SceneModuleInterface const std::vector & points) const; }; +template class SceneModuleManagerInterface { public: - SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name); + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) + : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) + { + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 1); + if (!node.has_parameter("is_publish_debug_path")) { + is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); + } else { + is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); + } + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } + pub_virtual_wall_ = node.create_publisher( + std::string("~/virtual_wall/") + module_name, 5); + planning_factor_interface_ = + std::make_shared(&node, module_name); + + processing_time_publisher_ = std::make_shared(&node, "~/debug"); + + pub_processing_time_detail_ = node.create_publisher( + "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); + + time_keeper_ = std::make_shared(pub_processing_time_detail_); + } virtual ~SceneModuleManagerInterface() = default; @@ -171,31 +153,95 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path) + { + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); + } virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } protected: - virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); + virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) + { + universe_utils::ScopedTimeTrack st( + "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); + StopWatch stop_watch; + stop_watch.tic("Total"); + visualization_msgs::msg::MarkerArray debug_marker_array; + + for (const auto & scene_module : scene_modules_) { + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path); + + // The velocity factor must be called after modifyPathVelocity. + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + + virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); + } + + planning_factor_interface_->publish(); + pub_debug_->publish(debug_marker_array); + if (is_publish_debug_path_) { + tier4_planning_msgs::msg::PathWithLaneId debug_path; + debug_path.header = path->header; + debug_path.points = path->points; + pub_debug_path_->publish(debug_path); + } + pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); + processing_time_publisher_->publish( + std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); + } virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual std::function &)> - getModuleExpiredFunction(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); + virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) + { + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } + } 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 registerModule(const std::shared_ptr & scene_module) + { + RCLCPP_DEBUG( + logger_, "register task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_modules_.insert(scene_module); + } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const + { + const auto & p = planner_data_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, + p->ego_nearest_yaw_threshold); + } - std::set> scene_modules_; + std::set> scene_modules_; std::set registered_module_id_set_; std::shared_ptr planner_data_; @@ -209,77 +255,28 @@ class SceneModuleManagerInterface rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; rclcpp::Publisher::SharedPtr pub_debug_path_; - rclcpp::Publisher::SharedPtr - pub_velocity_factor_; - rclcpp::Publisher::SharedPtr - pub_infrastructure_commands_; std::shared_ptr processing_time_publisher_; rclcpp::Publisher::SharedPtr pub_processing_time_detail_; std::shared_ptr time_keeper_; -}; - -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; - - static 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; - } + std::shared_ptr planning_factor_interface_; }; - +extern template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +extern template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner #endif // AUTOWARE__BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml index 77b26aac09161..a66faa2e0a303 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_planner_common - 0.40.0 + 0.41.0 The autoware_behavior_velocity_planner_common package Tomoya Kimura @@ -20,15 +20,16 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_map_msgs autoware_motion_utils autoware_objects_of_interest_marker_interface autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_route_handler - autoware_rtc_interface autoware_universe_utils autoware_vehicle_info_utils autoware_velocity_smoother @@ -44,9 +45,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_api_msgs tier4_planning_msgs - tier4_v2x_msgs visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index cdfde1ce51205..1849c05085ac4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -13,32 +13,27 @@ // limitations under the License. #include -#include #include -#include -#include #include #include #include #include -#include #include namespace autoware::behavior_velocity_planner { -using autoware::universe_utils::StopWatch; - SceneModuleInterface::SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) : module_id_(module_id), - activated_(false), - safe_(false), - distance_(std::numeric_limits::lowest()), logger_(logger), clock_(clock), - time_keeper_(std::shared_ptr()) + time_keeper_(time_keeper), + planning_factor_interface_(planning_factor_interface) { } @@ -50,222 +45,17 @@ size_t SceneModuleInterface::findEgoSegmentIndex( points, p->current_odometry->pose, p->ego_nearest_dist_threshold); } -SceneModuleManagerInterface::SceneModuleManagerInterface( - rclcpp::Node & node, [[maybe_unused]] const char * module_name) -: node_(node), clock_(node.get_clock()), logger_(node.get_logger()) -{ - const auto ns = std::string("~/debug/") + module_name; - pub_debug_ = node.create_publisher(ns, 1); - if (!node.has_parameter("is_publish_debug_path")) { - is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); - } else { - is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); - } - if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( - std::string("~/debug/path_with_lane_id/") + module_name, 1); - } - pub_virtual_wall_ = node.create_publisher( - std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); - pub_infrastructure_commands_ = - node.create_publisher( - "~/output/infrastructure_commands", 1); - - processing_time_publisher_ = std::make_shared(&node, "~/debug"); - - pub_processing_time_detail_ = node.create_publisher( - "~/debug/processing_time_detail_ms/" + std::string(module_name), 1); - - time_keeper_ = std::make_shared(pub_processing_time_detail_); -} - -size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const -{ - const auto & p = planner_data_; - return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); -} - -void SceneModuleManagerInterface::updateSceneModuleInstances( +template SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name); +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - planner_data_ = planner_data; - - launchNewModules(path); - deleteExpiredModules(path); -} - -void SceneModuleManagerInterface::modifyPathVelocity( - tier4_planning_msgs::msg::PathWithLaneId * path) -{ - universe_utils::ScopedTimeTrack st( - "SceneModuleManagerInterface::modifyPathVelocity", *time_keeper_); - StopWatch stop_watch; - stop_watch.tic("Total"); - visualization_msgs::msg::MarkerArray debug_marker_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; - infrastructure_command_array.stamp = clock_->now(); - - for (const auto & scene_module : scene_modules_) { - scene_module->resetVelocityFactor(); - scene_module->setPlannerData(planner_data_); - scene_module->modifyPathVelocity(path); - - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - - if (const auto command = scene_module->getInfrastructureCommand()) { - infrastructure_command_array.commands.push_back(*command); - } - - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { - debug_marker_array.markers.push_back(marker); - } - - virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); - } - - pub_velocity_factor_->publish(velocity_factor_array); - pub_infrastructure_commands_->publish(infrastructure_command_array); - pub_debug_->publish(debug_marker_array); - if (is_publish_debug_path_) { - tier4_planning_msgs::msg::PathWithLaneId debug_path; - debug_path.header = path->header; - debug_path.points = path->points; - pub_debug_path_->publish(debug_path); - } - pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); - processing_time_publisher_->publish( - std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); -} - -void SceneModuleManagerInterface::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - const auto isModuleExpired = getModuleExpiredFunction(path); - - auto itr = scene_modules_.begin(); - while (itr != scene_modules_.end()) { - if (isModuleExpired(*itr)) { - itr = scene_modules_.erase(itr); - } else { - itr++; - } - } -} - -void SceneModuleManagerInterface::registerModule( - const std::shared_ptr & scene_module) -{ - RCLCPP_DEBUG( - logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); - registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_module->setTimeKeeper(time_keeper_); - scene_modules_.insert(scene_module); -} - -SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc) -: SceneModuleManagerInterface(node, module_name), - rtc_interface_(&node, module_name, enable_rtc), - objects_of_interest_marker_interface_(&node, module_name) -{ -} - -void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) -{ - setActivation(); - modifyPathVelocity(path); - sendRTC(path->header.stamp); - publishObjectsOfInterestMarker(); -} - -void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) -{ - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - const auto state = !scene_module->isActivated() && scene_module->isSafe() - ? State::WAITING_FOR_EXECUTION - : State::RUNNING; - updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); - } - publishRTCStatus(stamp); -} - -void SceneModuleManagerInterfaceWithRTC::setActivation() -{ - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - scene_module->setActivation(rtc_interface_.isActivated(uuid)); - scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); - } -} - -UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const -{ - if (map_uuid_.count(module_id) == 0) { - const UUID uuid; - return uuid; - } - return map_uuid_.at(module_id); -} - -void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) -{ - map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); -} - -void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) -{ - const auto result = map_uuid_.erase(module_id); - if (result == 0) { - RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); - } -} - -void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() -{ - for (const auto & scene_module : scene_modules_) { - const auto objects = scene_module->getObjectsOfInterestData(); - for (const auto & obj : objects) { - objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); - } - scene_module->clearObjectsOfInterestData(); - } - - objects_of_interest_marker_interface_.publishMarkerArray(); -} - -void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - const auto isModuleExpired = getModuleExpiredFunction(path); - - auto itr = scene_modules_.begin(); - while (itr != scene_modules_.end()) { - if (isModuleExpired(*itr)) { - const UUID uuid = getUUID((*itr)->getModuleId()); - updateRTCStatus( - uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), - clock_->now()); - removeUUID((*itr)->getModuleId()); - registered_module_id_set_.erase((*itr)->getModuleId()); - itr = scene_modules_.erase(itr); - } else { - itr++; - } - } -} - + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CHANGELOG.rst new file mode 100644 index 0000000000000..08974b37c362a --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CHANGELOG.rst @@ -0,0 +1,94 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_behavior_velocity_rtc_interface +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + +0.40.0 (2025-01-06) +------------------- + +0.39.0 (2024-11-25) +------------------- + +0.38.0 (2024-11-11) +------------------- diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt new file mode 100644 index 0000000000000..2bd9b9b8d20f0 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_behavior_velocity_rtc_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_interface_with_rtc.cpp +) + +ament_auto_package(INSTALL_TO_SHARE) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md new file mode 100644 index 0000000000000..79b5a4e3d7b95 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/README.md @@ -0,0 +1,3 @@ +# Behavior Velocity RTC Interface + +This package provides a behavior velocity interface with RTC, which are used in the `behavior_velocity_planner` node and modules. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp new file mode 100644 index 0000000000000..0cf1e343fb646 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp @@ -0,0 +1,157 @@ +// 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_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ + +#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::rtc_interface::RTCInterface; +using autoware::universe_utils::getOrDeclareParameter; +using builtin_interfaces::msg::Time; +using tier4_planning_msgs::msg::PathWithLaneId; +using tier4_rtc_msgs::msg::Module; +using tier4_rtc_msgs::msg::State; +using unique_identifier_msgs::msg::UUID; + +class SceneModuleInterfaceWithRTC : public SceneModuleInterface +{ +public: + explicit SceneModuleInterfaceWithRTC( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); + virtual ~SceneModuleInterfaceWithRTC() = default; + + 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_; } + +protected: + bool activated_; + bool safe_; + bool rtc_enabled_; + double distance_; + + void setSafe(const bool safe) + { + safe_ = safe; + if (!rtc_enabled_) { + syncActivation(); + } + } + void setDistance(const double distance) { distance_ = distance; } + void syncActivation() { setActivation(isSafe()); } +}; + +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; + + static 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; + } +}; + +extern template size_t +SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +extern template void +SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +extern template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +extern template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_RTC_INTERFACE__SCENE_MODULE_INTERFACE_WITH_RTC_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml new file mode 100644 index 0000000000000..22935a111ffb2 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/package.xml @@ -0,0 +1,39 @@ + + + + autoware_behavior_velocity_rtc_interface + 0.41.0 + The autoware_behavior_velocity_rtc_interface package + + Mamoru Sobue + Takayuki Murooka + Tomoya Kimura + Shumpei Wakabayashi + Takagi, Isamu + Fumiya Watanabe + + Apache License 2.0 + + Takayuki Murooka + + ament_cmake_auto + autoware_cmake + + autoware_behavior_velocity_planner_common + autoware_motion_utils + autoware_planning_factor_interface + autoware_planning_msgs + autoware_rtc_interface + autoware_universe_utils + rclcpp + rclcpp_components + tier4_planning_msgs + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp new file mode 100644 index 0000000000000..34c7a30e73acf --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/src/scene_module_interface_with_rtc.cpp @@ -0,0 +1,143 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ + +SceneModuleInterfaceWithRTC::SceneModuleInterfaceWithRTC( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), + activated_(false), + safe_(false), + distance_(std::numeric_limits::lowest()) +{ +} + +SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc) +: SceneModuleManagerInterface(node, module_name), + rtc_interface_(&node, module_name, enable_rtc), + objects_of_interest_marker_interface_(&node, module_name) +{ +} + +void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) +{ + setActivation(); + modifyPathVelocity(path); + sendRTC(path->header.stamp); + publishObjectsOfInterestMarker(); +} + +void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + const auto state = !scene_module->isActivated() && scene_module->isSafe() + ? State::WAITING_FOR_EXECUTION + : State::RUNNING; + updateRTCStatus(uuid, scene_module->isSafe(), state, scene_module->getDistance(), stamp); + } + publishRTCStatus(stamp); +} + +void SceneModuleManagerInterfaceWithRTC::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + scene_module->setActivation(rtc_interface_.isActivated(uuid)); + scene_module->setRTCEnabled(rtc_interface_.isRTCEnabled(uuid)); + } +} + +UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const +{ + if (map_uuid_.count(module_id) == 0) { + const UUID uuid; + return uuid; + } + return map_uuid_.at(module_id); +} + +void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) +{ + map_uuid_.insert({module_id, autoware::universe_utils::generateUUID()}); +} + +void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) +{ + const auto result = map_uuid_.erase(module_id); + if (result == 0) { + RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); + } +} + +void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() +{ + for (const auto & scene_module : scene_modules_) { + const auto objects = scene_module->getObjectsOfInterestData(); + for (const auto & obj : objects) { + objects_of_interest_marker_interface_.insertObjectData(obj.pose, obj.shape, obj.color); + } + scene_module->clearObjectsOfInterestData(); + } + + objects_of_interest_marker_interface_.publishMarkerArray(); +} + +void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( + const tier4_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + auto itr = scene_modules_.begin(); + while (itr != scene_modules_.end()) { + if (isModuleExpired(*itr)) { + const UUID uuid = getUUID((*itr)->getModuleId()); + updateRTCStatus( + uuid, (*itr)->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), + clock_->now()); + removeUUID((*itr)->getModuleId()); + registered_module_id_set_.erase((*itr)->getModuleId()); + itr = scene_modules_.erase(itr); + } else { + itr++; + } + } +} + +template size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const; +template void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const tier4_planning_msgs::msg::PathWithLaneId & path); +template void SceneModuleManagerInterface::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path); +template void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module); +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CHANGELOG.rst index eaf93783ae47a..8edfbd4d2216d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CHANGELOG.rst @@ -2,6 +2,64 @@ Changelog for package autoware_behavior_velocity_run_out_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs (`#9744 `_) + * feat(behavior_velocity_planner): use XXXStamped in autoware_internal_debug_msgs + * fix + --------- +* fix(autoware_behavior_velocity_run_out_module): fix bugprone-branch-clone (`#9715 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt index da2f4ce33ff60..a2f35ada11b5e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/CMakeLists.txt @@ -17,10 +17,11 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - tests/test_dynamic_obstacle.cpp - tests/test_path_utils.cpp - tests/test_utils.cpp - tests/test_state_machine.cpp + test/test_dynamic_obstacle.cpp + test/test_path_utils.cpp + test/test_utils.cpp + test/test_state_machine.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} autoware_behavior_velocity_run_out_module diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml index 90e1d32198854..761ebeb94419a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_run_out_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_run_out_module package Tomohito Ando @@ -20,7 +20,9 @@ eigen3_cmake_module autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_motion_utils autoware_object_recognition_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp index 2823648e184d3..3f4b9f330e2d0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -316,7 +316,7 @@ void RunOutDebug::setAccelReason(const AccelReason & accel_reason) void RunOutDebug::publishDebugValue() { // publish debug values - tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; debug_msg.stamp = node_.now(); for (const auto & v : debug_values_.getValues()) { debug_msg.data.push_back(v); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp index 3e913f57f30c0..1ffa826c4d1d1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -18,17 +18,17 @@ #include -#include -#include +#include +#include #include #include #include namespace autoware::behavior_velocity_planner { +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Int32Stamped; using sensor_msgs::msg::PointCloud2; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Int32Stamped; class DebugValues { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp index 4a16f263d0a54..4af7882461643 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -156,7 +156,8 @@ void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathW if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, planner_data_, planner_param_, logger_.get_child("run_out_module"), - std::move(dynamic_obstacle_creator_), debug_ptr_, clock_)); + std::move(dynamic_obstacle_creator_), debug_ptr_, clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp index 131ba32f32100..068ed81015fb1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -25,7 +25,7 @@ namespace autoware::behavior_velocity_planner { -class RunOutModuleManager : public SceneModuleManagerInterface +class RunOutModuleManager : public SceneModuleManagerInterface<> { public: explicit RunOutModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp index a32a65a0d8909..a300e8f5f9284 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -45,15 +45,16 @@ RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), planner_param_(planner_param), dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), debug_ptr_(debug_ptr), state_machine_(std::make_unique(planner_param.approaching.state)) { - velocity_factor_.init(PlanningBehavior::RUN_OUT); - if (planner_param.run_out.use_partition_lanelet) { const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); @@ -770,9 +771,10 @@ bool RunOutModule::insertStopPoint( stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); - velocity_factor_.set( - path.points, planner_data_->current_odometry->pose, stop_point.value(), VelocityFactor::UNKNOWN, - "run_out"); + planning_factor_interface_->add( + path.points, planner_data_->current_odometry->pose, stop_point.value(), stop_point.value(), + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift_distance*/, "run_out_stop"); return true; } @@ -811,7 +813,7 @@ void RunOutModule::insertVelocityForState( // insert velocity for each state switch (state) { - case State::GO: { + case State::GO: { // NOLINT insertStoppingVelocity(target_obstacle, current_pose, current_vel, current_acc, output_path); break; } @@ -876,8 +878,11 @@ void RunOutModule::insertApproachingVelocity( return; } - velocity_factor_.set( - output_path.points, current_pose, stop_point.value(), VelocityFactor::UNKNOWN, "run_out"); + planning_factor_interface_->add( + output_path.points, planner_data_->current_odometry->pose, stop_point.value(), + stop_point.value(), tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}, true /*is_driving_forward*/, 0.0 /*velocity*/, + 0.0 /*shift_distance*/, "run_out_approaching_velocity"); // debug debug_ptr_->pushStopPose(autoware::universe_utils::calcOffsetPose( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp index 3673a215bb749..1879d724f98b3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -32,10 +32,10 @@ namespace autoware::behavior_velocity_planner { +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; -using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; @@ -47,7 +47,10 @@ class RunOutModule : public SceneModuleInterface const int64_t module_id, const std::shared_ptr & planner_data, const PlannerParam & planner_param, const rclcpp::Logger logger, std::unique_ptr dynamic_obstacle_creator, - const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock); + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp index 1f948cc7faaa1..c48e4f867fac0 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -21,10 +21,10 @@ #include #include +#include #include #include #include -#include #include #include @@ -39,11 +39,11 @@ using autoware::universe_utils::LineString2d; using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; -using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; struct CommonParam diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_dynamic_obstacle.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_dynamic_obstacle.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..75bf59751ed44 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "run_out", "autoware::behavior_velocity_planner::RunOutModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_path_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_path_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_state_machine.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_state_machine.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/tests/test_utils.cpp rename to planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/test/test_utils.cpp diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/CHANGELOG.rst index f0a53b8d7124d..5c2978b1bfbc9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/CHANGELOG.rst @@ -2,6 +2,48 @@ Changelog for package autoware_behavior_velocity_speed_bump_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml index 4940460ba52c1..5519b54162062 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_speed_bump_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_speed_bump_module package Tomoya Kimura diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp index 4677d49b78f4b..a69e33e9e0391 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.cpp @@ -63,7 +63,7 @@ void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pa if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, lane_id, *speed_bump_with_lane_id.first, planner_param_, - logger_.get_child("speed_bump_module"), clock_)); + logger_.get_child("speed_bump_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp index 950bb8471cc22..f98db8b88b7a9 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/manager.hpp @@ -29,7 +29,7 @@ namespace autoware::behavior_velocity_planner { -class SpeedBumpModuleManager : public SceneModuleManagerInterface +class SpeedBumpModuleManager : public SceneModuleManagerInterface<> { public: explicit SpeedBumpModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp index 54ea807f3268b..a5099eedce027 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.cpp @@ -21,6 +21,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -33,8 +34,11 @@ using geometry_msgs::msg::Point32; SpeedBumpModule::SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), lane_id_(lane_id), speed_bump_reg_elem_(std::move(speed_bump_reg_elem)), diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp index 176a01d5112c5..5b908f90bf9c2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_speed_bump_module/src/scene.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -54,7 +55,10 @@ class SpeedBumpModule : public SceneModuleInterface SpeedBumpModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::SpeedBump & speed_bump_reg_elem, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CHANGELOG.rst index c24c67ff81108..ce67ba65749df 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CHANGELOG.rst @@ -2,6 +2,69 @@ Changelog for package autoware_behavior_velocity_stop_line_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix: remove unnecessary parameters (`#9935 `_) +* fix(planning): corrects typo in svg (`#9814 `_) +* fix(planning): corrects typos (`#9840 `_) + * fix(planning): corrects typos + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Atto Armoo, Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt index f4528f0d13cf4..a187b4cda9459 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/CMakeLists.txt @@ -14,6 +14,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_scene.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md index f373afc2351bf..963d75e5a8492 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/README.md @@ -1,33 +1,32 @@ -## Stop Line +# Stop Line -### Role +## Role -This module plans velocity so that the vehicle can stop right before stop lines and restart driving after stopped. +This module plans the vehicle's velocity to ensure it stops just before stop lines and can resume movement after stopping. ![stop line](docs/stop_line.svg) -### Activation Timing +## Activation Timing This module is activated when there is a stop line in a target lane. -### Module Parameters +## Module Parameters -| Parameter | Type | Description | -| -------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_margin` | double | a margin that the vehicle tries to stop before stop_line | -| `stop_duration_sec` | double | [s] time parameter for the ego vehicle to stop in front of a stop line | -| `hold_stop_margin_distance` | double | [m] parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | -| `use_initialization_stop_state` | bool | A flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | -| `show_stop_line_collision_check` | bool | A flag to determine whether to show the debug information of collision check with a stop line | +| Parameter | Type | Description | +| ------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_margin` | double | Margin that the vehicle tries to stop in before stop_line | +| `stop_duration_sec` | double | [s] Time parameter for the ego vehicle to stop before stop line | +| `hold_stop_margin_distance` | double | [m] Parameter for restart prevention (See Algorithm section). Also, when the ego vehicle is within this distance from a stop line, the ego state becomes STOPPED from APPROACHING | +| `use_initialization_stop_state` | bool | Flag to determine whether to return to the approaching state when the vehicle moves away from a stop line. | -### Inner-workings / Algorithms +## Inner-workings / Algorithms - Gets a stop line from map information. -- insert a stop point on the path from the stop line defined in the map and the ego vehicle length. +- Inserts a stop point on the path from the stop line defined in the map and the ego vehicle length. - Sets velocities of the path after the stop point to 0[m/s]. -- Release the inserted stop velocity when the vehicle stops at the stop point for `stop_duration_sec` seconds. +- Releases the inserted stop velocity when the vehicle halts at the stop point for `stop_duration_sec` seconds. -#### Flowchart +### Flowchart ```plantuml @startuml @@ -85,15 +84,15 @@ Then, we can get `offset segment` and `offset from segment start`. ![find_offset_segment](docs/./find_offset_segment.drawio.svg) -After that, we can calculate a offset point from `offset segment` and `offset`. This will be `stop_pose`. +After that, we can calculate an offset point from `offset segment` and `offset`. This will be `stop_pose`. ![calculate_stop_pose](docs/./calculate_stop_pose.drawio.svg) -#### Restart prevention +### Restart Prevention -If it needs X meters (e.g. 0.5 meters) to stop once the vehicle starts moving due to the poor vehicle control performance, the vehicle goes over the stopping position that should be strictly observed when the vehicle starts to moving in order to approach the near stop point (e.g. 0.3 meters away). +If the vehicle requires X meters (e.g. 0.5 meters) to stop once it starts moving due to poor vehicle control performance, it may overshoot the stopping position, which must be strictly observed. This happens when the vehicle begins to move in order to approach a nearby stop point (e.g. 0.3 meters away). -This module has parameter `hold_stop_margin_distance` in order to prevent from these redundant restart. If the vehicle is stopped within `hold_stop_margin_distance` meters from stop point of the module (\_front_to_stop_line < hold_stop_margin_distance), the module judges that the vehicle has already stopped for the module's stop point and plans to keep stopping current position even if the vehicle is stopped due to other factors. +This module includes the parameter `hold_stop_margin_distance` to prevent redundant restarts in such a situation. If the vehicle is stopped within `hold_stop_margin_distance` meters of the stop point (\_front_to_stop_line < hold_stop_margin_distance), the module determines that the vehicle has already stopped at the stop point and will maintain the current stopping position, even if the vehicle has come to a stop due to other factors.
![example](docs/restart_prevention.svg){width=1000} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg index 182c941907f57..6cd47ffc14a56 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg @@ -233,7 +233,7 @@

= node(i) + node(i+1)
- = all segment has two points + = all segments have two points
diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml index fd4b9f83ae998..61508e0ffd9dc 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_stop_line_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_stop_line_module package Yukinari Hisaki @@ -20,6 +20,7 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp index b305a1d2aa404..e289779df34b6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -81,7 +81,7 @@ void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::Pat if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( module_id, stop_line_with_lane_id.first, planner_param_, - logger_.get_child("stop_line_module"), clock_)); + logger_.get_child("stop_line_module"), clock_, time_keeper_, planning_factor_interface_)); } } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp index bef8a5eef4ac0..c746e2bf6a314 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { using StopLineWithLaneId = std::pair; -class StopLineModuleManager : public SceneModuleManagerInterface +class StopLineModuleManager : public SceneModuleManagerInterface<> { public: explicit StopLineModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 2a9c5dab1ebd9..2115a0a7eca6e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -20,6 +20,7 @@ #include +#include #include #include @@ -28,14 +29,16 @@ namespace autoware::behavior_velocity_planner StopLineModule::StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), stop_line_(std::move(stop_line)), planner_param_(planner_param), state_(State::APPROACH), debug_data_() { - velocity_factor_.init(PlanningBehavior::STOP_SIGN); } bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) @@ -59,7 +62,12 @@ bool StopLineModule::modifyPathVelocity(PathWithLaneId * path) path->points = trajectory->restore(); - updateVelocityFactor(&velocity_factor_, state_, *stop_point - ego_s); + // TODO(soblin): PlanningFactorInterface use trajectory class + planning_factor_interface_->add( + path->points, trajectory->compute(*stop_point).point.pose, + planner_data_->current_odometry->pose, planner_data_->current_odometry->pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, "stopline"); updateStateAndStoppedTime( &state_, &stopped_time_, clock_->now(), *stop_point - ego_s, planner_data_->isVehicleStopped()); @@ -149,24 +157,6 @@ void StopLineModule::updateStateAndStoppedTime( } } -void StopLineModule::updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point) -{ - switch (state) { - case State::APPROACH: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::APPROACHING); - break; - } - case State::STOPPED: { - velocity_factor->set(distance_to_stop_point, VelocityFactor::STOPPED); - break; - } - case State::START: - break; - } -} - void StopLineModule::updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 9ac1463da8618..d2c999a377ab4 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -18,7 +18,6 @@ #include "autoware/behavior_velocity_planner_common/scene_module_interface.hpp" #include "autoware/behavior_velocity_planner_common/utilization/util.hpp" -#include "autoware/motion_utils/factor/velocity_factor_interface.hpp" #include "autoware/trajectory/path_point_with_lane_id.hpp" #include @@ -30,6 +29,7 @@ #include +#include #include #include @@ -68,7 +68,10 @@ class StopLineModule : public SceneModuleInterface StopLineModule( const int64_t module_id, lanelet::ConstLineString3d stop_line, const PlannerParam & planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; @@ -95,10 +98,6 @@ class StopLineModule : public SceneModuleInterface State * state, std::optional * stopped_time, const rclcpp::Time & now, const double & distance_to_stop_point, const bool & is_vehicle_stopped) const; - static void updateVelocityFactor( - autoware::motion_utils::VelocityFactorInterface * velocity_factor, const State & state, - const double & distance_to_stop_point); - void updateDebugData( DebugData * debug_data, const geometry_msgs::msg::Pose & stop_pose, const State & state) const; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..c6d6ff638cfbb --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "stop_line", "autoware::behavior_velocity_planner::StopLineModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp index 1eafb71eb403c..2b176a64c597d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_stop_line_module/test/test_scene.cpp @@ -74,7 +74,10 @@ class StopLineModuleTest : public ::testing::Test clock_ = std::make_shared(); module_ = std::make_shared( - 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_); + 1, stop_line_, planner_param_, rclcpp::get_logger("test_logger"), clock_, + std::make_shared(), + std::make_shared( + node_.get(), "test_stopline")); module_->setPlannerData(planner_data_); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/CHANGELOG.rst index 4fb540814a9f1..42d5dde3b6433 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/CHANGELOG.rst @@ -2,6 +2,48 @@ Changelog for package autoware_behavior_velocity_template_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml index e808e7758bd65..8ece81460081f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_template_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_template_module package Daniel Sanchez diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp index 4325cc1d5aaf9..a004f5b823138 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.cpp @@ -42,8 +42,9 @@ void TemplateModuleManager::launchNewModules( { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { - registerModule( - std::make_shared(module_id, logger_.get_child(getModuleName()), clock_)); + registerModule(std::make_shared( + module_id, logger_.get_child(getModuleName()), clock_, time_keeper_, + planning_factor_interface_)); } } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp index a7a0d83be6368..c5b9293fcdcc5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/manager.hpp @@ -38,7 +38,7 @@ namespace autoware::behavior_velocity_planner * @param node A reference to the ROS node. */ class TemplateModuleManager -: public autoware::behavior_velocity_planner::SceneModuleManagerInterface +: public autoware::behavior_velocity_planner::SceneModuleManagerInterface<> { public: explicit TemplateModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp index 3ce8502baaf63..7dc8ca9b2e43b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.cpp @@ -19,14 +19,18 @@ #include +#include #include namespace autoware::behavior_velocity_planner { TemplateModule::TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock) + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface) { } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp index 70cd5cb1235e7..d204e589f0416 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_template_module/src/scene.hpp @@ -18,9 +18,9 @@ #include #include +#include #include #include - namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -29,7 +29,10 @@ class TemplateModule : public SceneModuleInterface { public: TemplateModule( - const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); /** * @brief Modify the velocity of path points. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CHANGELOG.rst index 669882a37bbce..b294a6f36588f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CHANGELOG.rst @@ -2,6 +2,62 @@ Changelog for package autoware_behavior_velocity_traffic_light_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt index 6370dd5e6c21d..02ed192459970 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/CMakeLists.txt @@ -15,6 +15,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_utils.cpp + test/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml index 082973d9431e5..518d3275a28d3 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_traffic_light_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_traffic_light_module package Satoshi Ota @@ -19,7 +19,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common + autoware_behavior_velocity_rtc_interface autoware_lanelet2_extension autoware_motion_utils autoware_perception_msgs diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp index b6747724ba6f7..1d1ee7fc996b2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.cpp @@ -53,24 +53,13 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat autoware_perception_msgs::msg::TrafficLightGroup tl_state; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { std::shared_ptr traffic_light_scene_module( std::dynamic_pointer_cast(scene_module)); - traffic_light_scene_module->resetVelocityFactor(); traffic_light_scene_module->setPlannerData(planner_data_); traffic_light_scene_module->modifyPathVelocity(path); - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = traffic_light_scene_module->getVelocityFactor(); - if (velocity_factor.behavior != PlanningBehavior::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < nearest_ref_stop_path_point_index_) { @@ -88,7 +77,6 @@ void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::Pat virtual_wall_marker_creator_.add_virtual_walls( traffic_light_scene_module->createVirtualWalls()); } - pub_velocity_factor_->publish(velocity_factor_array); pub_debug_->publish(debug_marker_array); pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); pub_tl_state_->publish(tl_state); @@ -114,7 +102,8 @@ void TrafficLightModuleManager::launchNewModules( if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) { registerModule(std::make_shared( lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, - logger_.get_child("traffic_light_module"), clock_)); + logger_.get_child("traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); generateUUID(lane_id); updateRTCStatus( getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), @@ -123,7 +112,7 @@ void TrafficLightModuleManager::launchNewModules( } } -std::function &)> +std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { @@ -131,7 +120,7 @@ TrafficLightModuleManager::getModuleExpiredFunction( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); return [this, lanelet_id_set]( - [[maybe_unused]] const std::shared_ptr & scene_module) { + [[maybe_unused]] const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { if (isModuleRegisteredFromExistingAssociatedModule(id)) { return false; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp index eb8ef53ddb604..5ac32d1107880 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/manager.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -42,8 +42,8 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; + std::function &)> + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) override; void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp index 5eb0d5aa5f267..3a8692e9c53dd 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.cpp @@ -43,8 +43,11 @@ namespace autoware::behavior_velocity_planner TrafficLightModule::TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(lane_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterfaceWithRTC(lane_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), @@ -52,7 +55,6 @@ TrafficLightModule::TrafficLightModule( debug_data_(), is_prev_state_stop_(false) { - velocity_factor_.init(PlanningBehavior::TRAFFIC_SIGNAL); planner_param_ = planner_param; } @@ -291,9 +293,11 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( size_t insert_index = insert_target_point_idx; planning_utils::insertVelocity(modified_path, target_point_with_lane_id, 0.0, insert_index); - velocity_factor_.set( + planning_factor_interface_->add( modified_path.points, planner_data_->current_odometry->pose, - target_point_with_lane_id.point.pose, VelocityFactor::UNKNOWN); + target_point_with_lane_id.point.pose, target_point_with_lane_id.point.pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); return modified_path; } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp index 8221bb3740273..71f0855a4af6f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/src/scene.hpp @@ -23,8 +23,8 @@ #define EIGEN_MPL2_ONLY #include #include -#include #include +#include #include #include @@ -33,7 +33,7 @@ namespace autoware::behavior_velocity_planner { -class TrafficLightModule : public SceneModuleInterface +class TrafficLightModule : public SceneModuleInterfaceWithRTC { public: using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; @@ -69,7 +69,10 @@ class TrafficLightModule : public SceneModuleInterface TrafficLightModule( const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e24c2dab5dab9 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_traffic_light_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "traffic_light", "autoware::behavior_velocity_planner::TrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/CHANGELOG.rst index 731c1967b5a1d..ca86c893f97f2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/CHANGELOG.rst @@ -2,6 +2,74 @@ Changelog for package autoware_behavior_velocity_virtual_traffic_light_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_planning_test_manager): remove dependency of VirtualTrafficLightState and ExpandStopRange (`#9953 `_) + * feat(autoware_planning_test_manager): remove dependency of virtual traffic light + * modify obstacle_stop test code + --------- +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(behavior_velocity_planner)!: remove velocity_factor completely (`#9943 `_) + * feat(behavior_velocity_planner)!: remove velocity_factor completely + * minimize diff + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(behavior_velocity_planner): remove virtual traffic light dependency from behavior_velocity_planner and behavior_velocity_planner_common (`#9746 `_) + * feat: remove-virtual-traffic-light-dependency-from-plugin-manager + * build passed + * fix test + * fix test + * fix + * fix typo + --------- +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 6d3bc68242d7c..3a1e0d9580ded 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_virtual_traffic_light_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_virtual_traffic_light_module package Kosuke Takeuchi @@ -18,6 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index a51cf9211c21f..dade98dfc1133 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -21,6 +21,8 @@ #include #include +#include + #include #include @@ -53,6 +55,14 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node p.check_timeout_after_stop_line = getOrDeclareParameter(node, ns + ".check_timeout_after_stop_line"); } + + sub_virtual_traffic_light_states_ = autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>:: + create_subscription(&node, "~/input/virtual_traffic_light_states"); + + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); } void VirtualTrafficLightModuleManager::launchNewModules( @@ -84,22 +94,49 @@ void VirtualTrafficLightModuleManager::launchNewModules( ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) { registerModule(std::make_shared( module_id, lane_id, *m.first, m.second, planner_param_, - logger_.get_child("virtual_traffic_light_module"), clock_)); + logger_.get_child("virtual_traffic_light_module"), clock_, time_keeper_, + planning_factor_interface_)); } } } -std::function &)> +std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [id_set](const std::shared_ptr & scene_module) { + return [id_set](const std::shared_ptr & scene_module) { return id_set.count(scene_module->getModuleId()) == 0; }; } + +void VirtualTrafficLightModuleManager::modifyPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId * path) +{ + // NOTE: virtual traffic light specific implementation + // Since the argument of modifyPathVelocity cannot be changed, the specific information + // of virtual traffic light states is set here. + const auto virtual_traffic_light_states = sub_virtual_traffic_light_states_->takeData(); + for (const auto & scene_module : scene_modules_) { + scene_module->setVirtualTrafficLightStates(virtual_traffic_light_states); + } + + SceneModuleManagerInterface::modifyPathVelocity(path); + + // NOTE: virtual traffic light specific implementation + // publish infrastructure_command_array + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + for (const auto & scene_module : scene_modules_) { + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + } + pub_infrastructure_commands_->publish(infrastructure_command_array); +} } // namespace autoware::behavior_velocity_planner #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index ba5a4b23a6fe0..aecef0851d8ab 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -20,16 +20,20 @@ #include #include #include +#include #include #include +#include +#include #include #include namespace autoware::behavior_velocity_planner { -class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface +class VirtualTrafficLightModuleManager +: public SceneModuleManagerInterface { public: explicit VirtualTrafficLightModuleManager(rclcpp::Node & node); @@ -38,10 +42,20 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface private: VirtualTrafficLightModule::PlannerParam planner_param_; + + void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - std::function &)> getModuleExpiredFunction( + std::function &)> getModuleExpiredFunction( const tier4_planning_msgs::msg::PathWithLaneId & path) override; + + autoware::universe_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray>::SharedPtr + sub_virtual_traffic_light_states_; + + rclcpp::Publisher::SharedPtr + pub_infrastructure_commands_; }; class VirtualTrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 206fb14b6d41c..a42e3520ea3c1 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -38,15 +39,16 @@ VirtualTrafficLightModule::VirtualTrafficLightModule( const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), lane_id_(lane_id), reg_elem_(reg_elem), lane_(lane), planner_param_(planner_param) { - velocity_factor_.init(PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT); - // Get map data const auto instrument = reg_elem_.getVirtualTrafficLight(); const auto instrument_bottom_line = toAutowarePoints(instrument); @@ -333,12 +335,12 @@ bool VirtualTrafficLightModule::isNearAnyEndLine(const size_t end_line_idx) std::optional VirtualTrafficLightModule::findCorrespondingState() { - // No message - if (!planner_data_->virtual_traffic_light_states) { + // Note: This variable is set by virtual traffic light's manager. + if (!virtual_traffic_light_states_) { return {}; } - for (const auto & state : planner_data_->virtual_traffic_light_states->states) { + for (const auto & state : virtual_traffic_light_states_->states) { if (state.id == map_data_.instrument_id) { return state; } @@ -417,9 +419,10 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN, - command_.type); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_stop_line = @@ -450,11 +453,32 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( } // Set StopReason - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0, 0.0 /*shift distance*/, ""); // Set data for visualization module_data_.stop_head_pose_at_end_line = calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); } + +std::optional +VirtualTrafficLightModule::getInfrastructureCommand() const +{ + return infrastructure_command_; +} + +void VirtualTrafficLightModule::setInfrastructureCommand( + const std::optional & command) +{ + infrastructure_command_ = command; +} + +void VirtualTrafficLightModule::setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states) +{ + virtual_traffic_light_states_ = virtual_traffic_light_states; +} } // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index b031ba5b2bb2b..24e77cfaea157 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -24,9 +25,13 @@ #include #include +#include +#include + #include #include +#include #include #include @@ -77,29 +82,38 @@ class VirtualTrafficLightModule : public SceneModuleInterface const int64_t module_id, const int64_t lane_id, const lanelet::autoware::VirtualTrafficLight & reg_elem, lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; autoware::motion_utils::VirtualWalls createVirtualWalls() override; + std::optional getInfrastructureCommand() const; + void setInfrastructureCommand( + const std::optional & command); + + void setVirtualTrafficLightStates( + const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr + virtual_traffic_light_states); + private: const int64_t lane_id_; const lanelet::autoware::VirtualTrafficLight & reg_elem_; const lanelet::ConstLanelet lane_; const PlannerParam planner_param_; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states_; State state_{State::NONE}; tier4_v2x_msgs::msg::InfrastructureCommand command_; + std::optional infrastructure_command_; MapData map_data_; ModuleData module_data_; void updateInfrastructureCommand(); - void setVelocityFactor( - const geometry_msgs::msg::Pose & stop_pose, - autoware_adapi_v1_msgs::msg::VelocityFactor * velocity_factor); - std::optional getPathIndexOfFirstEndLine(); bool isBeforeStartLine(const size_t end_line_idx); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..0e22413c0c8ec --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_virtual_traffic_light_module/test/test_node_interface.cpp @@ -0,0 +1,89 @@ +// 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 +#include + +#include + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; + +void publishVirtualTrafficLightState( + std::shared_ptr test_manager, rclcpp::Node::SharedPtr target_node) +{ + auto test_node = test_manager->getTestNode(); + const auto pub_virtual_traffic_light = + test_manager->getTestNode()->create_publisher( + "behavior_velocity_planner_node/input/virtual_traffic_light_states", 1); + pub_virtual_traffic_light->publish(VirtualTrafficLightStateArray{}); + autoware::test_utils::spinSomeNodes(test_node, target_node, 3); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + publishVirtualTrafficLightState(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "virtual_traffic_light", + "autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + publishVirtualTrafficLightState(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CHANGELOG.rst b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CHANGELOG.rst index 40a8bcf82e7b9..d4f846dea7a5d 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CHANGELOG.rst +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CHANGELOG.rst @@ -2,6 +2,58 @@ Changelog for package autoware_behavior_velocity_walkway_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(behavior_velocity_modules): add node test (`#9790 `_) + * feat(behavior_velocity_crosswalk): add node test + * fix + * feat(behavior_velocity_xxx_module): add node test + * fix + * fix + * fix + * fix + * change directory tests -> test + --------- +* refactor(behavior_velocity_planner_common): add behavior_velocity_rtc_interface and move RTC-related implementation (`#9799 `_) + * split into planer_common and rtc_interface + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp + Co-authored-by: Mamoru Sobue + * Update planning/behavior_velocity_planner/autoware_behavior_velocity_rtc_interface/include/autoware/behavior_velocity_rtc_interface/scene_module_interface_with_rtc.hpp + Co-authored-by: Mamoru Sobue + * fix + --------- + Co-authored-by: Mamoru Sobue +* Contributors: Fumiya Watanabe, Mamoru Sobue, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt index 11504d9c8999c..8b11fdce7283e 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/CMakeLists.txt @@ -11,4 +11,14 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene_walkway.cpp ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + ${TEST_SOURCES} + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml index 5f1aea22855a4..4bda6bbe5826b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/package.xml @@ -2,7 +2,7 @@ autoware_behavior_velocity_walkway_module - 0.40.0 + 0.41.0 The autoware_behavior_velocity_walkway_module package Satoshi Ota @@ -18,6 +18,7 @@ autoware_cmake autoware_behavior_velocity_crosswalk_module + autoware_behavior_velocity_planner autoware_behavior_velocity_planner_common autoware_lanelet2_extension autoware_motion_utils @@ -31,6 +32,7 @@ rclcpp visualization_msgs + ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp index 0f1a7509043b5..bfa8a96531090 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -61,7 +61,8 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); + lanelet.id(), lanelet_map_ptr, p, use_regulatory_element, logger, clock_, time_keeper_, + planning_factor_interface_)); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp index 85d4495914823..b323d6d201795 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -35,7 +35,7 @@ namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; -class WalkwayModuleManager : public SceneModuleManagerInterface +class WalkwayModuleManager : public SceneModuleManagerInterface<> { public: explicit WalkwayModuleManager(rclcpp::Node & node); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 6a7505930a334..91ed11a7467d5 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -18,6 +18,7 @@ #include #include +#include #include namespace autoware::behavior_velocity_planner @@ -32,15 +33,16 @@ using autoware::universe_utils::getPose; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface) +: SceneModuleInterface(module_id, logger, clock, time_keeper, planning_factor_interface), module_id_(module_id), state_(State::APPROACH), planner_param_(planner_param), use_regulatory_element_(use_regulatory_element) { - velocity_factor_.init(PlanningBehavior::SIDEWALK); - if (use_regulatory_element_) { const auto reg_elem_ptr = std::dynamic_pointer_cast( lanelet_map_ptr->regulatoryElementLayer.get(module_id)); @@ -119,9 +121,10 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path) } /* get stop point and stop factor */ - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose.value(), - VelocityFactor::UNKNOWN); + planning_factor_interface_->add( + path->points, planner_data_->current_odometry->pose, stop_pose.value(), stop_pose.value(), + tier4_planning_msgs::msg::PlanningFactor::STOP, tier4_planning_msgs::msg::SafetyFactorArray{}, + true /*is_driving_forward*/, 0.0 /*velocity*/, 0.0 /*shift distance*/, "walkway_stop"); // use arc length to identify if ego vehicle is in front of walkway stop or not. const double signed_arc_dist_to_stop_point = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index df54eafd11cc2..3e06bf2d878c6 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -44,7 +44,10 @@ class WalkwayModule : public SceneModuleInterface WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, const bool use_regulatory_element, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock, + const std::shared_ptr time_keeper, + const std::shared_ptr + planning_factor_interface); bool modifyPathVelocity(PathWithLaneId * path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp new file mode 100644 index 0000000000000..e0b7fa5c31965 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_walkway_module/test/test_node_interface.cpp @@ -0,0 +1,64 @@ +// 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 + +#include + +#include +#include +#include +#include + +namespace autoware::behavior_velocity_planner +{ +TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionPathWithLaneID) +{ + rclcpp::init(0, nullptr); + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode({}); + + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test with nominal path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + // test with empty path_with_lane_id + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithAbnormalPathWithLaneId(test_target_node)); + rclcpp::shutdown(); +} + +TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) +{ + rclcpp::init(0, nullptr); + + const auto plugin_info_vec = {autoware::behavior_velocity_planner::PluginInfo{ + "walkway", "autoware::behavior_velocity_planner::WalkwayModulePlugin"}}; + + auto test_manager = autoware::behavior_velocity_planner::generateTestManager(); + auto test_target_node = autoware::behavior_velocity_planner::generateNode(plugin_info_vec); + autoware::behavior_velocity_planner::publishMandatoryTopics(test_manager, test_target_node); + + // test for normal trajectory + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPathWithLaneId(test_target_node)); + + // make sure behavior_path_planner is running + EXPECT_GE(test_manager->getReceivedTopicNum(), 1); + + ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testOffTrackFromPathWithLaneId(test_target_node)); + + rclcpp::shutdown(); +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CHANGELOG.rst b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CHANGELOG.rst index 118b555408997..3d20f11b239ad 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CHANGELOG.rst +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package autoware_motion_velocity_dynamic_obstacle_stop_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(motion_velocity_planner)!: add _universe suffix to autoware_motion_velocity_planner_common and autoware_motion_velocity_planner_node (`#9942 `_) +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(motion_velocity_planner): introduce Object/Pointcloud structure in PlannerData (`#9812 `_) + * feat: new object/pointcloud struct in motion velocity planner + * update planner_data + * modify modules + * fix + --------- +* feat(motion_velocity_planner): remove unnecessary tier4_planning_msgs dependency (`#9757 `_) + * feat(motion_velocity_planner): remove unnecessary tier4_planning_msgs dependency + * fix + --------- +* feat(motion_velocity_planner): use Float64Stamped in autoware_internal_debug_msgs (`#9745 `_) +* Contributors: Fumiya Watanabe, Mamoru Sobue, Ryohsuke Mitsudome, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt index 896db96fd573d..4d055e9fff223 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(autoware_motion_velocity_dynamic_obstacle_stop_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml index eef7b6af1f9af..2e94f1d32b8b3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/package.xml @@ -2,7 +2,7 @@ autoware_motion_velocity_dynamic_obstacle_stop_module - 0.40.0 + 0.41.0 dynamic_obstacle_stop module to stop ego when in the immediate path of a dynamic object Maxime Clement @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake autoware_motion_utils - autoware_motion_velocity_planner_common + autoware_motion_velocity_planner_common_universe autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -24,7 +24,6 @@ pluginlib rclcpp tf2 - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp index 12cb59ac46195..c7a103e0269ca 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.cpp @@ -43,7 +43,10 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo module_name_ = module_name; logger_ = node.get_logger().get_child(ns_); clock_ = node.get_clock(); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + planning_factor_interface_ = + std::make_unique( + &node, "dynamic_obstacle_stop"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -51,8 +54,9 @@ void DynamicObstacleStopModule::init(rclcpp::Node & node, const std::string & mo node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); using autoware::universe_utils::getOrDeclareParameter; auto & p = params_; @@ -130,7 +134,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( ? 0.0 : params_.hysteresis; const auto dynamic_obstacles = dynamic_obstacle_stop::filter_predicted_objects( - planner_data->predicted_objects, ego_data, params_, hysteresis); + planner_data->objects, ego_data, params_, hysteresis); const auto preprocessing_duration_us = stopwatch.toc("preprocessing"); @@ -159,15 +163,9 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( debug_data_.stop_pose = stop_pose; if (stop_pose) { result.stop_points.push_back(stop_pose->position); - const auto stop_pose_reached = - planner_data->current_odometry.twist.twist.linear.x < 1e-3 && - autoware::universe_utils::calcDistance2d(ego_data.pose, *stop_pose) < 1e-3; - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *stop_pose, - stop_pose_reached ? autoware::motion_utils::VelocityFactor::STOPPED - : autoware::motion_utils::VelocityFactor::APPROACHING, - "dynamic_obstacle_stop"); - result.velocity_factor = velocity_factor_interface_.get(); + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *stop_pose, PlanningFactor::STOP, + SafetyFactorArray{}); create_virtual_walls(); } } @@ -190,7 +188,7 @@ VelocityPlanningResult DynamicObstacleStopModule::plan( processing_times["collisions"] = collisions_duration_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp index 1843d26a22a29..a678e7657a6c3 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/dynamic_obstacle_stop_module.hpp @@ -19,8 +19,8 @@ #include "types.hpp" #include -#include -#include +#include +#include #include #include @@ -33,6 +33,7 @@ class DynamicObstacleStopModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index 2ab3c09e64edd..0726fdba02de0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -96,20 +96,22 @@ bool is_unavoidable( }; std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { std::vector filtered_objects; - for (const auto & object : objects.objects) { - const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >= - params.minimum_object_velocity; + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; + const auto is_not_too_slow = + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x >= + params.minimum_object_velocity; if ( - is_vehicle(object) && is_not_too_slow && - is_in_range(object, ego_data.trajectory, params, hysteresis) && - is_not_too_close(object, ego_data, params.ego_longitudinal_offset) && + is_vehicle(predicted_object) && is_not_too_slow && + is_in_range(predicted_object, ego_data.trajectory, params, hysteresis) && + is_not_too_close(predicted_object, ego_data, params.ego_longitudinal_offset) && (!params.ignore_unavoidable_collisions || - !is_unavoidable(object, ego_data.pose, ego_data.earliest_stop_pose, params))) - filtered_objects.push_back(object); + !is_unavoidable(predicted_object, ego_data.pose, ego_data.earliest_stop_pose, params))) + filtered_objects.push_back(predicted_object); } return filtered_objects; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index d20d6354066c6..33abb19074099 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -17,6 +17,8 @@ #include "types.hpp" +#include + #include #include @@ -74,7 +76,7 @@ bool is_unavoidable( /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects std::vector filter_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, + const std::vector & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace autoware::motion_velocity_planner::dynamic_obstacle_stop diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp index 3fa179903c23f..419b5d9e46bd1 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_dynamic_obstacle_stop_module/test/test_object_filtering.cpp @@ -14,6 +14,7 @@ #include "../src/object_filtering.hpp" +#include #include #include @@ -26,6 +27,8 @@ #include #include +#include + TEST(TestObjectFiltering, isVehicle) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::is_vehicle; @@ -202,8 +205,7 @@ TEST(TestObjectFiltering, isUnavoidable) TEST(TestObjectFiltering, filterPredictedObjects) { using autoware::motion_velocity_planner::dynamic_obstacle_stop::filter_predicted_objects; - autoware_perception_msgs::msg::PredictedObjects objects; - autoware_perception_msgs::msg::PredictedObject object; + std::vector objects; autoware::motion_velocity_planner::dynamic_obstacle_stop::EgoData ego_data; autoware::motion_velocity_planner::dynamic_obstacle_stop::PlannerParam params; double hysteresis{}; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CHANGELOG.rst b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CHANGELOG.rst index c1310671d02fa..21cf1519e61d2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CHANGELOG.rst +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CHANGELOG.rst @@ -2,6 +2,33 @@ Changelog for package autoware_motion_velocity_obstacle_velocity_limiter_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(motion_velocity_planner)!: add _universe suffix to autoware_motion_velocity_planner_common and autoware_motion_velocity_planner_node (`#9942 `_) +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* fix(autoware_motion_velocity_obstacle_velocity_limiter_module): remove cppcheck suppressions (`#9843 `_) +* feat(motion_velocity_planner): introduce Object/Pointcloud structure in PlannerData (`#9812 `_) + * feat: new object/pointcloud struct in motion velocity planner + * update planner_data + * modify modules + * fix + --------- +* fix(autoware_motion_velocity_obstacle_velocity_limiter_module): fix bugprone-exception-escape (`#9779 `_) + * fix: bugprone-error + * fix: cppcheck + * fix: cpplint + --------- +* feat(motion_velocity_planner): remove unnecessary tier4_planning_msgs dependency (`#9757 `_) + * feat(motion_velocity_planner): remove unnecessary tier4_planning_msgs dependency + * fix + --------- +* feat(motion_velocity_planner): use Float64Stamped in autoware_internal_debug_msgs (`#9745 `_) +* Contributors: Fumiya Watanabe, Mamoru Sobue, Ryohsuke Mitsudome, Ryuta Kambe, Takayuki Murooka, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CMakeLists.txt index 6ff39738137b3..0bd6bf6c5decc 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(autoware_motion_velocity_obstacle_velocity_limiter_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp index 79b817c51169c..c8aa9895a0451 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/benchmarks/collision_checker_benchmark.cpp @@ -16,6 +16,7 @@ #include "../src/types.hpp" #include +#include #include #include @@ -55,45 +56,51 @@ polygon_t random_polygon() int main() { - Obstacles obstacles; - std::vector polygons; - polygons.reserve(100); - for (auto i = 0; i < 100; ++i) { - polygons.push_back(random_polygon()); - } - for (auto nb_lines = 1lu; nb_lines < 1000lu; nb_lines += 10) { - obstacles.lines.push_back(random_line()); - obstacles.points.clear(); - for (auto nb_points = 1lu; nb_points < 1000lu; nb_points += 10) { - obstacles.points.push_back(random_point()); - const auto rtt_constr_start = std::chrono::system_clock::now(); - CollisionChecker rtree_collision_checker(obstacles, 0, 0); - const auto rtt_constr_end = std::chrono::system_clock::now(); - const auto naive_constr_start = std::chrono::system_clock::now(); - CollisionChecker naive_collision_checker(obstacles, nb_points + 1, nb_lines * 100); - const auto naive_constr_end = std::chrono::system_clock::now(); - const auto rtt_check_start = std::chrono::system_clock::now(); - for (const auto & polygon : polygons) - // cppcheck-suppress unreadVariable - const auto rtree_result = rtree_collision_checker.intersections(polygon); - const auto rtt_check_end = std::chrono::system_clock::now(); - const auto naive_check_start = std::chrono::system_clock::now(); - for (const auto & polygon : polygons) - // cppcheck-suppress unreadVariable - const auto naive_result = naive_collision_checker.intersections(polygon); - const auto naive_check_end = std::chrono::system_clock::now(); - const auto rtt_constr_time = - std::chrono::duration_cast(rtt_constr_end - rtt_constr_start); - const auto naive_constr_time = - std::chrono::duration_cast(naive_constr_end - naive_constr_start); - const auto rtt_check_time = - std::chrono::duration_cast(rtt_check_end - rtt_check_start); - const auto naive_check_time = - std::chrono::duration_cast(naive_check_end - naive_check_start); - std::printf( - "%lu, %lu, %ld, %ld, %ld, %ld\n", nb_lines, nb_points, rtt_constr_time.count(), - rtt_check_time.count(), naive_constr_time.count(), naive_check_time.count()); + try { + Obstacles obstacles; + std::vector polygons; + polygons.reserve(100); + for (auto i = 0; i < 100; ++i) { + polygons.push_back(random_polygon()); + } + for (auto nb_lines = 1lu; nb_lines < 1000lu; nb_lines += 10) { + obstacles.lines.push_back(random_line()); + obstacles.points.clear(); + for (auto nb_points = 1lu; nb_points < 1000lu; nb_points += 10) { + obstacles.points.push_back(random_point()); + const auto rtt_constr_start = std::chrono::system_clock::now(); + CollisionChecker rtree_collision_checker(obstacles, 0, 0); + const auto rtt_constr_end = std::chrono::system_clock::now(); + const auto naive_constr_start = std::chrono::system_clock::now(); + CollisionChecker naive_collision_checker(obstacles, nb_points + 1, nb_lines * 100); + const auto naive_constr_end = std::chrono::system_clock::now(); + const auto rtt_check_start = std::chrono::system_clock::now(); + for (const auto & polygon : polygons) + const auto rtree_result = rtree_collision_checker.intersections(polygon); + const auto rtt_check_end = std::chrono::system_clock::now(); + const auto naive_check_start = std::chrono::system_clock::now(); + for (const auto & polygon : polygons) + const auto naive_result = naive_collision_checker.intersections(polygon); + const auto naive_check_end = std::chrono::system_clock::now(); + const auto rtt_constr_time = + std::chrono::duration_cast(rtt_constr_end - rtt_constr_start); + const auto naive_constr_time = std::chrono::duration_cast( + naive_constr_end - naive_constr_start); + const auto rtt_check_time = + std::chrono::duration_cast(rtt_check_end - rtt_check_start); + const auto naive_check_time = + std::chrono::duration_cast(naive_check_end - naive_check_start); + std::printf( + "%lu, %lu, %ld, %ld, %ld, %ld\n", nb_lines, nb_points, rtt_constr_time.count(), + rtt_check_time.count(), naive_constr_time.count(), naive_check_time.count()); + } } + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return {}; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return {}; } return 0; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml index 68e2ead5a5ec7..3b19c27807a81 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/package.xml @@ -1,7 +1,7 @@ autoware_motion_velocity_obstacle_velocity_limiter_module - 0.40.0 + 0.41.0 Package to adjust velocities of a trajectory in order for the ride to feel safe Maxime CLEMENT @@ -15,7 +15,7 @@ autoware_grid_map_utils autoware_lanelet2_extension autoware_motion_utils - autoware_motion_velocity_planner_common + autoware_motion_velocity_planner_common_universe autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager @@ -33,7 +33,6 @@ sensor_msgs tf2_eigen tf2_geometry_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp index e501b756af6a1..9be7f52bca99a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.cpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp index d7d2de63b33f8..ad746fb2a7b7e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter.hpp @@ -19,7 +19,7 @@ #include "parameters.hpp" #include "types.hpp" -#include +#include #include #include @@ -56,7 +56,7 @@ void calculateSteeringAngles(TrajectoryPoints & trajectory, const double wheel_b /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const double buffer, + const std::vector & dynamic_obstacles, const double buffer, const double min_vel); /// @brief create footprint polygons from projection lines diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp index 3fa3ec7cbf782..eb89c027a3b48 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.cpp @@ -48,7 +48,6 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string projection_params_ = obstacle_velocity_limiter::ProjectionParameters(node); obstacle_params_ = obstacle_velocity_limiter::ObstacleParameters(node); velocity_params_ = obstacle_velocity_limiter::VelocityParameters(node); - velocity_factor_interface_.init(autoware::motion_utils::PlanningBehavior::ROUTE_OBSTACLE); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -56,8 +55,9 @@ void ObstacleVelocityLimiterModule::init(rclcpp::Node & node, const std::string node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); const auto vehicle_info = vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); @@ -169,7 +169,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( const auto preprocessing_us = stopwatch.toc("preprocessing"); stopwatch.tic("obstacles"); obstacle_masks.negative_masks = obstacle_velocity_limiter::createPolygonMasks( - planner_data->predicted_objects, obstacle_params_.dynamic_obstacles_buffer, + planner_data->objects, obstacle_params_.dynamic_obstacles_buffer, obstacle_params_.dynamic_obstacles_min_vel); if (obstacle_params_.ignore_on_path) obstacle_masks.negative_masks.push_back(obstacle_velocity_limiter::createTrajectoryFootprint( @@ -188,8 +188,8 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( obstacle_masks.positive_mask = obstacle_velocity_limiter::createEnvelopePolygon(footprint_polygons); obstacle_velocity_limiter::addSensorObstacles( - obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud, obstacle_masks, - obstacle_params_); + obstacles, planner_data->occupancy_grid, planner_data->no_ground_pointcloud.pointcloud, + obstacle_masks, obstacle_params_); } const auto obstacles_us = stopwatch.toc("obstacles"); autoware::motion_utils::VirtualWalls virtual_walls; @@ -236,7 +236,7 @@ VelocityPlanningResult ObstacleVelocityLimiterModule::plan( processing_times["slowdowns"] = slowdowns_us / 1000; processing_times["Total"] = total_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp index 65e24c2dff349..54f9c5c12e59e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacle_velocity_limiter_module.hpp @@ -17,8 +17,8 @@ #include "parameters.hpp" -#include -#include +#include +#include #include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp index 47215ee5cb0c4..187f028dc5969 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.cpp @@ -25,6 +25,7 @@ #include #include +#include namespace autoware::motion_velocity_planner::obstacle_velocity_limiter { @@ -58,17 +59,18 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity) + const std::vector & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; - for (const auto & object : objects.objects) { + for (const auto & object : objects) { + const auto & predicted_object = object.predicted_object; const double obj_vel_norm = std::hypot( - object.kinematics.initial_twist_with_covariance.twist.linear.x, - object.kinematics.initial_twist_with_covariance.twist.linear.y); + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x, + predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y); if (min_velocity <= obj_vel_norm) { polygons.push_back(createObjectPolygon( - object.kinematics.initial_pose_with_covariance.pose, object.shape.dimensions, buffer)); + predicted_object.kinematics.initial_pose_with_covariance.pose, + predicted_object.shape.dimensions, buffer)); } } return polygons; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp index 7e4704ba6818c..aff99140744e4 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/src/obstacles.hpp @@ -18,6 +18,7 @@ #include "parameters.hpp" #include "types.hpp" +#include #include #include @@ -163,8 +164,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, - const double min_velocity); + const std::vector & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object /// @param[out] obstacles Obstacles object in which to add the sensor obstacles diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp index ac07c62f62cf7..926b730dcc3dd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_obstacle_velocity_limiter_module/test/test_collision_distance.cpp @@ -15,6 +15,7 @@ #include "../src/distance.hpp" #include "../src/obstacles.hpp" #include "../src/types.hpp" +#include "autoware/motion_velocity_planner_common_universe/planner_data.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include @@ -27,6 +28,7 @@ #include #include +#include const auto point_in_polygon = [](const auto x, const auto y, const auto & polygon) { return std::find_if(polygon.outer().begin(), polygon.outer().end(), [=](const auto & pt) { @@ -387,11 +389,12 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { + using autoware::motion_velocity_planner::PlannerData; using autoware::motion_velocity_planner::obstacle_velocity_limiter::createObjectPolygons; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; - PredictedObjects objects; + std::vector objects; auto polygons = createObjectPolygons(objects, 0.0, 0.0); EXPECT_TRUE(polygons.empty()); @@ -404,7 +407,7 @@ TEST(TestCollisionDistance, createObjPolygons) object1.kinematics.initial_twist_with_covariance.twist.linear.x = 0.0; object1.shape.dimensions.x = 1.0; object1.shape.dimensions.y = 1.0; - objects.objects.push_back(object1); + objects.push_back(PlannerData::Object(object1)); polygons = createObjectPolygons(objects, 0.0, 1.0); EXPECT_TRUE(polygons.empty()); @@ -431,7 +434,7 @@ TEST(TestCollisionDistance, createObjPolygons) object2.kinematics.initial_twist_with_covariance.twist.linear.x = 2.0; object2.shape.dimensions.x = 2.0; object2.shape.dimensions.y = 1.0; - objects.objects.push_back(object2); + objects.push_back(PlannerData::Object(object2)); polygons = createObjectPolygons(objects, 0.0, 2.0); ASSERT_EQ(polygons.size(), 1ul); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CHANGELOG.rst b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CHANGELOG.rst index 4fe77d0532fce..e0bfb818bbe94 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CHANGELOG.rst +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CHANGELOG.rst @@ -2,6 +2,51 @@ Changelog for package autoware_motion_velocity_out_of_lane_module ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(motion_velocity_planner)!: add _universe suffix to autoware_motion_velocity_planner_common and autoware_motion_velocity_planner_node (`#9942 `_) +* chore(planning): move package directory for planning factor interface (`#9948 `_) + * chore: add new package for planning factor interface + * chore(surround_obstacle_checker): update include file + * chore(obstacle_stop_planner): update include file + * chore(obstacle_cruise_planner): update include file + * chore(motion_velocity_planner): update include file + * chore(bpp): update include file + * chore(bvp-common): update include file + * chore(blind_spot): update include file + * chore(crosswalk): update include file + * chore(detection_area): update include file + * chore(intersection): update include file + * chore(no_drivable_area): update include file + * chore(no_stopping_area): update include file + * chore(occlusion_spot): update include file + * chore(run_out): update include file + * chore(speed_bump): update include file + * chore(stop_line): update include file + * chore(template_module): update include file + * chore(traffic_light): update include file + * chore(vtl): update include file + * chore(walkway): update include file + * chore(motion_utils): remove factor interface + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(motion_velocity_planner): introduce Object/Pointcloud structure in PlannerData (`#9812 `_) + * feat: new object/pointcloud struct in motion velocity planner + * update planner_data + * modify modules + * fix + --------- +* feat(motion_velocity_planner): remove unnecessary tier4_planning_msgs dependency (`#9757 `_) + * feat(motion_velocity_planner): remove unnecessary tier4_planning_msgs dependency + * fix + --------- +* feat(motion_velocity_planner): use Float64Stamped in autoware_internal_debug_msgs (`#9745 `_) +* Contributors: Fumiya Watanabe, Mamoru Sobue, Ryohsuke Mitsudome, Satoshi OTA, Takayuki Murooka + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt index b96ca3017a031..f6301974552be 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/CMakeLists.txt @@ -3,7 +3,7 @@ project(autoware_motion_velocity_out_of_lane_module) find_package(autoware_cmake REQUIRED) autoware_package() -pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node plugins.xml) +pluginlib_export_plugin_description_file(autoware_motion_velocity_planner_node_universe plugins.xml) ament_auto_add_library(${PROJECT_NAME} SHARED DIRECTORY src diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 857716819d8cc..d9d2209edc6ef 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -2,7 +2,7 @@ autoware_motion_velocity_out_of_lane_module - 0.40.0 + 0.41.0 The motion_velocity_out_of_lane_module package Maxime Clement @@ -17,7 +17,7 @@ autoware_cmake autoware_motion_utils - autoware_motion_velocity_planner_common + autoware_motion_velocity_planner_common_universe autoware_perception_msgs autoware_planning_msgs autoware_route_handler @@ -29,7 +29,6 @@ pluginlib rclcpp tf2 - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index bd757a551515e..fccde78e1a9f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index f9ba7f4af9877..a6c8368c20571 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -106,23 +106,26 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { autoware_perception_msgs::msg::PredictedObjects filtered_objects; - filtered_objects.header = planner_data.predicted_objects.header; - for (const auto & object : planner_data.predicted_objects.objects) { + filtered_objects.header = planner_data.predicted_objects_header; + for (const auto & object : planner_data.objects) { + const auto & predicted_object = object.predicted_object; const auto is_pedestrian = - std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - }) != object.classification.end(); + std::find_if( + predicted_object.classification.begin(), predicted_object.classification.end(), + [](const auto & c) { + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + }) != predicted_object.classification.end(); if (is_pedestrian) continue; const auto is_coming_from_behind = motion_utils::calcSignedArcLength( ego_data.trajectory_points, 0UL, - object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; + predicted_object.kinematics.initial_pose_with_covariance.pose.position) < 0.0; if (params.objects_ignore_behind_ego && is_coming_from_behind) { continue; } - auto filtered_object = object; + auto filtered_object = predicted_object; const auto is_invalid_predicted_path = [&](const auto & predicted_path) { const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); @@ -130,10 +133,10 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const auto lat_offset_to_current_ego = std::abs(motion_utils::calcLateralOffset(no_overlap_path, ego_data.pose.position)); const auto is_crossing_ego = - lat_offset_to_current_ego <= - object.shape.dimensions.y / 2.0 + std::max( - params.left_offset + params.extra_left_offset, - params.right_offset + params.extra_right_offset); + lat_offset_to_current_ego <= predicted_object.shape.dimensions.y / 2.0 + + std::max( + params.left_offset + params.extra_left_offset, + params.right_offset + params.extra_right_offset); return is_low_confidence || is_crossing_ego; }; auto & predicted_paths = filtered_object.kinematics.predicted_paths; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 3b2ae11718810..d59c8eae59427 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp index f0e0360ef1c15..a670b828aa828 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_collisions.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index 63ca1b5784fe8..814af37aaa800 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include #include @@ -58,7 +58,10 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) logger_ = node.get_logger(); clock_ = node.get_clock(); init_parameters(node); - velocity_factor_interface_.init(motion_utils::PlanningBehavior::ROUTE_OBSTACLE); + + planning_factor_interface_ = + std::make_unique( + &node, "out_of_lane"); debug_publisher_ = node.create_publisher("~/" + ns_ + "/debug_markers", 1); @@ -66,8 +69,9 @@ void OutOfLaneModule::init(rclcpp::Node & node, const std::string & module_name) node.create_publisher("~/" + ns_ + "/virtual_walls", 1); processing_diag_publisher_ = std::make_shared( &node, "~/debug/" + ns_ + "/processing_time_ms_diag"); - processing_time_publisher_ = node.create_publisher( - "~/debug/" + ns_ + "/processing_time_ms", 1); + processing_time_publisher_ = + node.create_publisher( + "~/debug/" + ns_ + "/processing_time_ms", 1); } void OutOfLaneModule::init_parameters(rclcpp::Node & node) { @@ -309,15 +313,9 @@ VelocityPlanningResult OutOfLaneModule::plan( slowdown_pose->position, slowdown_pose->position, slowdown_velocity); } - const auto is_approaching = - motion_utils::calcSignedArcLength( - ego_trajectory_points, ego_data.pose.position, slowdown_pose->position) > 0.1 && - planner_data->current_odometry.twist.twist.linear.x > 0.1; - const auto status = is_approaching ? motion_utils::VelocityFactor::APPROACHING - : motion_utils::VelocityFactor::STOPPED; - velocity_factor_interface_.set( - ego_trajectory_points, ego_data.pose, *slowdown_pose, status, "out_of_lane"); - result.velocity_factor = velocity_factor_interface_.get(); + planning_factor_interface_->add( + ego_trajectory_points, ego_data.pose, *slowdown_pose, PlanningFactor::SLOW_DOWN, + SafetyFactorArray{}); virtual_wall_marker_creator.add_virtual_walls( out_of_lane::debug::create_virtual_walls(*slowdown_pose, slowdown_velocity == 0.0, params_)); virtual_wall_publisher_->publish(virtual_wall_marker_creator.create_markers(clock_->now())); @@ -349,7 +347,7 @@ VelocityPlanningResult OutOfLaneModule::plan( processing_times["publish_markers"] = pub_markers_us / 1000; processing_times["Total"] = total_time_us / 1000; processing_diag_publisher_->publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = clock_->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index c39c6e17101c5..b6eb03f5f0469 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -18,14 +18,12 @@ #include "types.hpp" #include -#include -#include +#include +#include #include #include #include -#include -#include #include @@ -40,6 +38,7 @@ class OutOfLaneModule : public PluginModuleInterface { public: void init(rclcpp::Node & node, const std::string & module_name) override; + void publish_planning_factor() override { planning_factor_interface_->publish(); }; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( const std::vector & ego_trajectory_points, diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CHANGELOG.rst b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/CHANGELOG.rst similarity index 94% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CHANGELOG.rst rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/CHANGELOG.rst index 50db2374baedc..10917e51fe348 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CHANGELOG.rst +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_motion_velocity_planner_common -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_motion_velocity_planner_common_universe +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(motion_velocity_planner)!: add _universe suffix to autoware_motion_velocity_planner_common and autoware_motion_velocity_planner_node (`#9942 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome 0.40.0 (2024-12-12) ------------------- diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/CMakeLists.txt similarity index 89% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/CMakeLists.txt index ffc06aa8cc2f8..70098f42a4939 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_motion_velocity_planner_common) +project(autoware_motion_velocity_planner_common_universe) find_package(autoware_cmake REQUIRED) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/collision_checker.hpp similarity index 91% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/collision_checker.hpp index bf471c62af969..3b135225c01cf 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/collision_checker.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/collision_checker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ -#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__COLLISION_CHECKER_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__COLLISION_CHECKER_HPP_ #include @@ -66,4 +66,4 @@ class CollisionChecker }; } // namespace autoware::motion_velocity_planner -#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__COLLISION_CHECKER_HPP_ +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__COLLISION_CHECKER_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp similarity index 63% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp index a96587c4465d6..4d4917f23496c 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/planner_data.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__PLANNER_DATA_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__PLANNER_DATA_HPP_ #include -#include +#include #include #include #include @@ -31,10 +31,6 @@ #include #include #include -#include -#include -#include -#include #include #include @@ -43,6 +39,7 @@ #include #include #include +#include namespace autoware::motion_velocity_planner { @@ -58,11 +55,72 @@ struct PlannerData { } + struct Object + { + public: + Object() = default; + explicit Object(const autoware_perception_msgs::msg::PredictedObject & arg_predicted_object) + : predicted_object(arg_predicted_object) + { + } + + autoware_perception_msgs::msg::PredictedObject predicted_object; + // double get_lon_vel_relative_to_traj() + // { + // if (!lon_vel_relative_to_traj) { + // lon_vel_relative_to_traj = 0.0; + // } + // return *lon_vel_relative_to_traj; + // } + // double get_lat_vel_relative_to_traj() + // { + // if (!lat_vel_relative_to_traj) { + // lat_vel_relative_to_traj = 0.0; + // } + // return *lat_vel_relative_to_traj; + // } + + private: + // TODO(murooka) implement the following variables and their functions. + // std::optional dist_to_traj_poly{std::nullopt}; + // std::optional dist_to_traj_lateral{std::nullopt}; + // std::optional dist_from_ego_longitudinal{std::nullopt}; + // std::optional lon_vel_relative_to_traj{std::nullopt}; + // std::optional lat_vel_relative_to_traj{std::nullopt}; + }; + + struct Pointcloud + { + public: + Pointcloud() = default; + explicit Pointcloud(const pcl::PointCloud & arg_pointcloud) + : pointcloud(arg_pointcloud) + { + } + + pcl::PointCloud pointcloud; + + private: + // NOTE: clustered result will be added. + }; + + void process_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & predicted_objects) + { + predicted_objects_header = predicted_objects.header; + + objects.clear(); + for (const auto & predicted_object : predicted_objects.objects) { + objects.push_back(Object(predicted_object)); + } + } + // msgs from callbacks that are used for data-ready nav_msgs::msg::Odometry current_odometry; geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration; - autoware_perception_msgs::msg::PredictedObjects predicted_objects; - pcl::PointCloud no_ground_pointcloud; + std_msgs::msg::Header predicted_objects_header; + std::vector objects; + Pointcloud no_ground_pointcloud; nav_msgs::msg::OccupancyGrid occupancy_grid; std::shared_ptr route_handler; @@ -75,8 +133,6 @@ struct PlannerData // last observed infomation for UNKNOWN std::map traffic_light_id_map_raw_; std::map traffic_light_id_map_last_observed_; - std::optional external_velocity_limit; - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states; // velocity smoother std::shared_ptr velocity_smoother_; @@ -110,4 +166,4 @@ struct PlannerData }; } // namespace autoware::motion_velocity_planner -#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__PLANNER_DATA_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp similarity index 70% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp index 9bd662af697ea..a01b39646e6e2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/plugin_module_interface.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ -#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__PLUGIN_MODULE_INTERFACE_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__PLUGIN_MODULE_INTERFACE_HPP_ #include "planner_data.hpp" #include "velocity_planning_result.hpp" -#include +#include #include #include +#include #include -#include #include #include @@ -32,6 +32,9 @@ namespace autoware::motion_velocity_planner { +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::SafetyFactorArray; + class PluginModuleInterface { public: @@ -42,15 +45,20 @@ class PluginModuleInterface const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; - autoware::motion_utils::VelocityFactorInterface velocity_factor_interface_; + virtual void publish_planning_factor() {} rclcpp::Logger logger_ = rclcpp::get_logger(""); rclcpp::Publisher::SharedPtr debug_publisher_; rclcpp::Publisher::SharedPtr virtual_wall_publisher_; std::shared_ptr processing_diag_publisher_; - rclcpp::Publisher::SharedPtr processing_time_publisher_; + rclcpp::Publisher::SharedPtr + processing_time_publisher_; autoware::motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator{}; + +protected: + std::unique_ptr + planning_factor_interface_; }; } // namespace autoware::motion_velocity_planner -#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__PLUGIN_MODULE_INTERFACE_HPP_ +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__PLUGIN_MODULE_INTERFACE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp similarity index 78% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp index 1288884a37979..d3dfa7a270a16 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/velocity_planning_result.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/include/autoware/motion_velocity_planner_common_universe/velocity_planning_result.hpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ -#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ +#ifndef AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__VELOCITY_PLANNING_RESULT_HPP_ +#define AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__VELOCITY_PLANNING_RESULT_HPP_ #include -#include #include #include @@ -43,8 +42,7 @@ struct VelocityPlanningResult { std::vector stop_points{}; std::vector slowdown_intervals{}; - std::optional velocity_factor{}; }; } // namespace autoware::motion_velocity_planner -#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON__VELOCITY_PLANNING_RESULT_HPP_ +#endif // AUTOWARE__MOTION_VELOCITY_PLANNER_COMMON_UNIVERSE__VELOCITY_PLANNING_RESULT_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml similarity index 89% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml index 3f0c027a5c986..91d569dc3f8cc 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/package.xml @@ -1,8 +1,8 @@ - autoware_motion_velocity_planner_common - 0.40.0 + autoware_motion_velocity_planner_common_universe + 0.41.0 Common functions and interfaces for motion_velocity_planner modules Maxime Clement @@ -17,6 +17,7 @@ eigen3_cmake_module autoware_behavior_velocity_planner_common + autoware_internal_debug_msgs autoware_motion_utils autoware_perception_msgs autoware_planning_msgs @@ -27,8 +28,6 @@ geometry_msgs libboost-dev rclcpp - tier4_debug_msgs - tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/collision_checker.cpp similarity index 97% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/collision_checker.cpp index f16c23efcb14c..f2f930a9adf73 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/collision_checker.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/src/collision_checker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_velocity_planner_common/collision_checker.hpp" +#include "autoware/motion_velocity_planner_common_universe/collision_checker.hpp" #include #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/test/test_collision_checker.cpp similarity index 98% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/test/test_collision_checker.cpp index d2a4a4560a430..79d8519189b21 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/test/test_collision_checker.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common_universe/test/test_collision_checker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware/motion_velocity_planner_common/collision_checker.hpp" +#include "autoware/motion_velocity_planner_common_universe/collision_checker.hpp" #include diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/LoadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv deleted file mode 100644 index 7b9f08ab0ded8..0000000000000 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/srv/UnloadPlugin.srv +++ /dev/null @@ -1,3 +0,0 @@ -string plugin_name ---- -bool success diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CHANGELOG.rst b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/CHANGELOG.rst similarity index 95% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CHANGELOG.rst rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/CHANGELOG.rst index 4a6dc3095391f..93a648302f213 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CHANGELOG.rst +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_motion_velocity_planner_node -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_motion_velocity_planner_node_universe +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(motion_velocity_planner)!: add _universe suffix to autoware_motion_velocity_planner_common and autoware_motion_velocity_planner_node (`#9942 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome 0.40.0 (2024-12-12) ------------------- diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/CMakeLists.txt similarity index 95% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/CMakeLists.txt index 0af4da6fd4426..cd9403ec73fe7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/CMakeLists.txt +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_motion_velocity_planner_node) +project(autoware_motion_velocity_planner_node_universe) find_package(autoware_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/README.md similarity index 50% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/README.md index 494446828e134..ef09e856eb72b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/README.md @@ -17,31 +17,30 @@ This means that to stop before a wall, a stop point is inserted in the trajector ## Input topics -| Name | Type | Description | -| -------------------------------------- | ----------------------------------------------------- | ----------------------------- | -| `~/input/trajectory` | autoware_planning_msgs::msg::Trajectory | input trajectory | -| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | -| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | -| `~/input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | -| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | -| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | +| Name | Type | Description | +| ------------------------------ | ----------------------------------------------------- | ----------------------------- | +| `~/input/trajectory` | autoware_planning_msgs::msg::Trajectory | input trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | +| `~/input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | ## Output topics -| Name | Type | Description | -| --------------------------- | ------------------------------------------------- | -------------------------------------------------- | -| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | -| `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | +| Name | Type | Description | +| ----------------------------------------- | ---------------------------------------------- | -------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/planning_factors/` | tier4_planning_msgs::msg::PlanningFactorsArray | factors causing change in the ego velocity profile | ## Services -| Name | Type | Description | -| ------------------------- | -------------------------------------------------------- | ---------------------------- | -| `~/service/load_plugin` | autoware_motion_velocity_planner_node::srv::LoadPlugin | To request loading a plugin | -| `~/service/unload_plugin` | autoware_motion_velocity_planner_node::srv::UnloadPlugin | To request unloaded a plugin | +| Name | Type | Description | +| ------------------------- | ----------------------------------------------------------------- | ---------------------------- | +| `~/service/load_plugin` | autoware_motion_velocity_planner_node_universe::srv::LoadPlugin | To request loading a plugin | +| `~/service/unload_plugin` | autoware_motion_velocity_planner_node_universe::srv::UnloadPlugin | To request unloaded a plugin | ## Node parameters diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml similarity index 100% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/config/motion_velocity_planner.param.yaml rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/config/motion_velocity_planner.param.yaml diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/docs/MotionVelocityPlanner-InternalInterface.drawio.svg similarity index 100% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/MotionVelocityPlanner-InternalInterface.drawio.svg rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/docs/MotionVelocityPlanner-InternalInterface.drawio.svg diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/docs/set_stop_velocity.drawio.svg similarity index 100% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/docs/set_stop_velocity.drawio.svg rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/docs/set_stop_velocity.drawio.svg diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/launch/motion_velocity_planner.launch.xml similarity index 84% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/launch/motion_velocity_planner.launch.xml index 3732d86ef380a..ed831ef367de6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/launch/motion_velocity_planner.launch.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/launch/motion_velocity_planner.launch.xml @@ -13,9 +13,9 @@ - + - + @@ -24,7 +24,6 @@ - diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/package.xml similarity index 88% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/package.xml index 186140cba6e3c..7d9652219b656 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/package.xml @@ -1,8 +1,8 @@ - autoware_motion_velocity_planner_node - 0.40.0 + autoware_motion_velocity_planner_node_universe + 0.41.0 Node of the motion_velocity_planner Maxime Clement @@ -18,10 +18,12 @@ rosidl_default_generators + autoware_internal_debug_msgs autoware_map_msgs autoware_motion_utils - autoware_motion_velocity_planner_common + autoware_motion_velocity_planner_common_universe autoware_perception_msgs + autoware_planning_factor_interface autoware_planning_msgs autoware_universe_utils autoware_velocity_smoother @@ -37,9 +39,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs tier4_metric_msgs - tier4_planning_msgs visualization_msgs rosidl_default_runtime diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json similarity index 100% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/schema/motion_velocity_planner.schema.json rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/schema/motion_velocity_planner.schema.json diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp similarity index 93% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp index 96865fec1c197..8f5aa761573b7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.cpp @@ -82,11 +82,9 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Publishers trajectory_pub_ = this->create_publisher("~/output/trajectory", 1); - velocity_factor_publisher_ = - this->create_publisher( - "~/output/velocity_factors", 1); processing_time_publisher_ = - this->create_publisher("~/debug/processing_time_ms", 1); + this->create_publisher( + "~/debug/processing_time_ms", 1); debug_viz_pub_ = this->create_publisher("~/debug/markers", 1); metrics_pub_ = this->create_publisher("~/metrics", 1); @@ -160,13 +158,14 @@ bool MotionVelocityPlannerNode::update_planner_data( const auto predicted_objects_ptr = sub_predicted_objects_.takeData(); if (check_with_log(predicted_objects_ptr, "Waiting for predicted objects")) - planner_data_.predicted_objects = *predicted_objects_ptr; + planner_data_.process_predicted_objects(*predicted_objects_ptr); processing_times["update_planner_data.pred_obj"] = sw.toc(true); const auto no_ground_pointcloud_ptr = sub_no_ground_pointcloud_.takeData(); if (check_with_log(no_ground_pointcloud_ptr, "Waiting for pointcloud")) { const auto no_ground_pointcloud = process_no_ground_pointcloud(no_ground_pointcloud_ptr); - if (no_ground_pointcloud) planner_data_.no_ground_pointcloud = *no_ground_pointcloud; + if (no_ground_pointcloud) + planner_data_.no_ground_pointcloud = PlannerData::Pointcloud(*no_ground_pointcloud); } processing_times["update_planner_data.pcd"] = sw.toc(true); @@ -185,9 +184,6 @@ bool MotionVelocityPlannerNode::update_planner_data( // optional data const auto traffic_signals_ptr = sub_traffic_signals_.takeData(); if (traffic_signals_ptr) process_traffic_signals(traffic_signals_ptr); - const auto virtual_traffic_light_states_ptr = sub_virtual_traffic_light_states_.takeData(); - if (virtual_traffic_light_states_ptr) - planner_data_.virtual_traffic_light_states = *virtual_traffic_light_states_ptr; processing_times["update_planner_data.traffic_lights"] = sw.toc(true); return is_ready; @@ -302,7 +298,7 @@ void MotionVelocityPlannerNode::on_trajectory( trajectory_pub_, output_trajectory_msg.header.stamp); processing_times["Total"] = stop_watch.toc("Total"); processing_diag_publisher_.publish(processing_times); - tier4_debug_msgs::msg::Float64Stamped processing_time_msg; + autoware_internal_debug_msgs::msg::Float64Stamped processing_time_msg; processing_time_msg.stamp = get_clock()->now(); processing_time_msg.data = processing_times["Total"]; processing_time_publisher_->publish(processing_time_msg); @@ -359,7 +355,6 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry.pose.pose; const double v0 = planner_data.current_odometry.twist.twist.linear.x; const double a0 = planner_data.current_acceleration.accel.accel.linear.x; - const auto & external_v_limit = planner_data.external_velocity_limit; const auto & smoother = planner_data.velocity_smoother_; const auto traj_lateral_acc_filtered = @@ -384,10 +379,6 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories, false)) { RCLCPP_ERROR(get_logger(), "failed to smooth"); } - if (external_v_limit) { - autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - 0LU, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); - } return traj_smoothed; } @@ -428,20 +419,13 @@ autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_traj resampled_trajectory, std::make_shared(planner_data_)); processing_times["plan_velocities"] = stop_watch.toc("plan_velocities"); - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factors; - velocity_factors.header.frame_id = "map"; - velocity_factors.header.stamp = get_clock()->now(); - for (const auto & planning_result : planning_results) { for (const auto & stop_point : planning_result.stop_points) insert_stop(output_trajectory_msg, stop_point); for (const auto & slowdown_interval : planning_result.slowdown_intervals) insert_slowdown(output_trajectory_msg, slowdown_interval); - if (planning_result.velocity_factor) - velocity_factors.factors.push_back(*planning_result.velocity_factor); } - velocity_factor_publisher_->publish(velocity_factors); return output_trajectory_msg; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp similarity index 88% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp index 757be518e018a..826c740d85b76 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/node.hpp @@ -17,14 +17,15 @@ #include "planner_manager.hpp" -#include +#include #include #include #include -#include -#include +#include +#include #include +#include #include #include #include @@ -32,7 +33,6 @@ #include #include #include -#include #include #include @@ -47,8 +47,8 @@ namespace autoware::motion_velocity_planner { using autoware_map_msgs::msg::LaneletMapBin; -using autoware_motion_velocity_planner_node::srv::LoadPlugin; -using autoware_motion_velocity_planner_node::srv::UnloadPlugin; +using autoware_motion_velocity_planner_node_universe::srv::LoadPlugin; +using autoware_motion_velocity_planner_node_universe::srv::UnloadPlugin; using autoware_planning_msgs::msg::Trajectory; using TrajectoryPoints = std::vector; @@ -80,9 +80,6 @@ class MotionVelocityPlannerNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber< autoware_perception_msgs::msg::TrafficLightGroupArray> sub_traffic_signals_{this, "~/input/traffic_signals"}; - autoware::universe_utils::InterProcessPollingSubscriber< - tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> - sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; rclcpp::Subscription::SharedPtr sub_lanelet_map_; void on_trajectory( @@ -96,11 +93,10 @@ class MotionVelocityPlannerNode : public rclcpp::Node // publishers rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - rclcpp::Publisher::SharedPtr - velocity_factor_publisher_; autoware::universe_utils::ProcessingTimePublisher processing_diag_publisher_{ this, "~/debug/processing_time_ms_diag"}; - rclcpp::Publisher::SharedPtr processing_time_publisher_; + rclcpp::Publisher::SharedPtr + processing_time_publisher_; autoware::universe_utils::PublishedTimePublisher published_time_publisher_{this}; rclcpp::Publisher::SharedPtr metrics_pub_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp similarity index 97% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp index b08798cbef772..6d889343641a9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.cpp @@ -25,7 +25,7 @@ namespace autoware::motion_velocity_planner MotionVelocityPlannerManager::MotionVelocityPlannerManager() : plugin_loader_( - "autoware_motion_velocity_planner_node", + "autoware_motion_velocity_planner_node_universe", "autoware::motion_velocity_planner::PluginModuleInterface") { } @@ -102,6 +102,8 @@ std::vector MotionVelocityPlannerManager::plan_velocitie VelocityPlanningResult res = plugin->plan(ego_trajectory_points, planner_data); results.push_back(res); + plugin->publish_planning_factor(); + if (res.stop_points.size() > 0) { const auto stop_decision_metric = make_decision_metric(plugin->get_module_name(), "stop"); metrics_.push_back(stop_decision_metric); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp similarity index 93% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp index 0e3bd77180f6e..48f6a246c6456 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/src/planner_manager.hpp @@ -15,8 +15,8 @@ #ifndef PLANNER_MANAGER_HPP_ #define PLANNER_MANAGER_HPP_ -#include -#include +#include +#include #include #include diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/srv/LoadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/LoadPlugin.srv rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/srv/LoadPlugin.srv diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/srv/UnloadPlugin.srv similarity index 100% rename from planning/behavior_velocity_planner/autoware_behavior_velocity_planner/srv/UnloadPlugin.srv rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/srv/UnloadPlugin.srv diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp similarity index 97% rename from planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp rename to planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp index 44ff7820c1566..b4a95dc020ffd 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/test/src/test_node_interface.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node_universe/test/src/test_node_interface.cpp @@ -50,7 +50,7 @@ std::shared_ptr generateNode() const auto autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); const auto motion_velocity_planner_dir = - ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node"); + ament_index_cpp::get_package_share_directory("autoware_motion_velocity_planner_node_universe"); const auto velocity_smoother_dir = ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); @@ -101,8 +101,6 @@ void publishMandatoryTopics( test_manager->publishMap(test_target_node, "motion_velocity_planner_node/input/vector_map"); test_manager->publishTrafficSignals( test_target_node, "motion_velocity_planner_node/input/traffic_signals"); - test_manager->publishVirtualTrafficLightState( - test_target_node, "motion_velocity_planner_node/input/virtual_traffic_light_states"); test_manager->publishOccupancyGrid( test_target_node, "motion_velocity_planner_node/input/occupancy_grid"); } diff --git a/planning/sampling_based_planner/autoware_bezier_sampler/CHANGELOG.rst b/planning/sampling_based_planner/autoware_bezier_sampler/CHANGELOG.rst index fa7e4a3bb0808..5b202eea4e110 100644 --- a/planning/sampling_based_planner/autoware_bezier_sampler/CHANGELOG.rst +++ b/planning/sampling_based_planner/autoware_bezier_sampler/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_bezier_sampler ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(goal_planner): add bezier based pull over planner (`#9642 `_) +* Contributors: Fumiya Watanabe, Mamoru Sobue + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp index a832ef1f5cd39..7c0db035d29e4 100644 --- a/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp @@ -40,6 +40,9 @@ struct SamplingParameters std::vector sample( const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, const SamplingParameters & params); +Bezier sample( + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, + const double initial_velocity, const double final_velocity, const double acceleration); /// @brief generate a Bezier curve for the given states, velocities, and accelerations Bezier generate( const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, diff --git a/planning/sampling_based_planner/autoware_bezier_sampler/package.xml b/planning/sampling_based_planner/autoware_bezier_sampler/package.xml index 249d64d770af5..665666a1ba7bd 100644 --- a/planning/sampling_based_planner/autoware_bezier_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_bezier_sampler/package.xml @@ -1,7 +1,7 @@ autoware_bezier_sampler - 0.40.0 + 0.41.0 Package to sample trajectories using Bézier curves Maxime CLEMENT Maxime CLEMENT diff --git a/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp index c36b8f88c8486..637cb0482e033 100644 --- a/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp @@ -58,6 +58,33 @@ std::vector sample( } return curves; } +Bezier sample( + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, + const double initial_velocity, const double final_velocity, const double acceleration) +{ + const double distance_initial_to_final = std::sqrt( + (initial.pose.x() - final.pose.x()) * (initial.pose.x() - final.pose.x()) + + (initial.pose.y() - final.pose.y()) * (initial.pose.y() - final.pose.y())); + // Tangent vectors + const Eigen::Vector2d initial_tangent_unit(std::cos(initial.heading), std::sin(initial.heading)); + const Eigen::Vector2d final_tangent_unit(std::cos(final.heading), std::sin(final.heading)); + // Unit vectors + const Eigen::Vector2d initial_normal_unit = initial_tangent_unit.unitOrthogonal(); + const Eigen::Vector2d final_normal_unit = final_tangent_unit.unitOrthogonal(); + const double initial_tangent_length = initial_velocity * distance_initial_to_final; + const double final_tangent_length = final_velocity * distance_initial_to_final; + const double acceleration_length = acceleration * distance_initial_to_final; + const Eigen::Vector2d initial_acceleration = + acceleration_length * initial_tangent_unit + + initial.curvature * initial_tangent_length * initial_tangent_length * initial_normal_unit; + const Eigen::Vector2d final_acceleration = + acceleration_length * final_tangent_unit + + final.curvature * final_tangent_length * final_tangent_length * final_normal_unit; + return generate( + {initial.pose.x(), initial.pose.y()}, {final.pose.x(), final.pose.y()}, + initial_tangent_unit * initial_tangent_length, initial_acceleration, + final_tangent_unit * final_tangent_length, final_acceleration); +} Bezier generate( const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration, diff --git a/planning/sampling_based_planner/autoware_frenet_planner/CHANGELOG.rst b/planning/sampling_based_planner/autoware_frenet_planner/CHANGELOG.rst index eff4108b3fc03..ead6bb6f73a88 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/CHANGELOG.rst +++ b/planning/sampling_based_planner/autoware_frenet_planner/CHANGELOG.rst @@ -2,6 +2,35 @@ Changelog for package autoware_frenet_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(lane_change): using frenet planner to generate lane change path when ego near terminal (`#9767 `_) + * frenet planner + * minor refactoring + * adding parameter + * Add diff th param + * limit curvature for prepare segment + * minor refactoring + * print average curvature + * refactor + * filter the path directly + * fix some conflicts + * include curvature smoothing + * document + * fix image folder + * image size + * doxygen + * add debug for state + * use sign function instead + * rename argument + * readme + * fix failed test due to empty value + * add additional note + * fix conflict + --------- +* Contributors: Fumiya Watanabe, Zulfaqar Azmi + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/sampling_based_planner/autoware_frenet_planner/package.xml b/planning/sampling_based_planner/autoware_frenet_planner/package.xml index cd5febc3267db..8130f47a8aab3 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/package.xml +++ b/planning/sampling_based_planner/autoware_frenet_planner/package.xml @@ -1,7 +1,7 @@ autoware_frenet_planner - 0.40.0 + 0.41.0 The autoware_frenet_planner package for trajectory generation in Frenet frame Maxime CLEMENT Maxime CLEMENT diff --git a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index cab4acfbb8556..54d4bc2e90714 100644 --- a/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -197,6 +197,15 @@ void calculateCartesian( trajectory.longitudinal_accelerations.push_back(0.0); trajectory.lateral_accelerations.push_back(0.0); } + for (auto i = 0UL; i < trajectory.points.size(); ++i) { + geometry_msgs::msg::Pose pose; + pose.position.x = trajectory.points[i].x(); + pose.position.y = trajectory.points[i].y(); + pose.position.z = 0.0; + pose.orientation = + autoware::universe_utils::createQuaternionFromRPY(0.0, 0.0, trajectory.yaws[i]); + trajectory.poses.push_back(pose); + } } } diff --git a/planning/sampling_based_planner/autoware_path_sampler/CHANGELOG.rst b/planning/sampling_based_planner/autoware_path_sampler/CHANGELOG.rst index a50f2a0ad3aed..1dad475fea0c7 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/CHANGELOG.rst +++ b/planning/sampling_based_planner/autoware_path_sampler/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package autoware_path_sampler ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(sampling_based_planner)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in sampling_based_planner (`#9916 `_) +* feat(motion_planning): use StringStamped in autoware_internal_debug_msgs (`#9742 `_) + feat(motion planning): use StringStamped in autoware_internal_debug_msgs +* Contributors: Fumiya Watanabe, Takayuki Murooka, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp index 95d833c4df47d..7398e28d18b28 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ #define AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ +#include "autoware_internal_debug_msgs/msg/string_stamped.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -25,7 +26,6 @@ #include "geometry_msgs/msg/twist.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" -#include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace autoware::path_sampler @@ -45,7 +45,7 @@ using nav_msgs::msg::Odometry; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug -using tier4_debug_msgs::msg::StringStamped; +using autoware_internal_debug_msgs::msg::StringStamped; } // namespace autoware::path_sampler #endif // AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ diff --git a/planning/sampling_based_planner/autoware_path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml index 142eafdfa4bfe..9efd0907c2727 100644 --- a/planning/sampling_based_planner/autoware_path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -2,7 +2,7 @@ autoware_path_sampler - 0.40.0 + 0.41.0 Package for the sampling-based path planner Maxime CLEMENT Apache License 2.0 @@ -15,6 +15,7 @@ autoware_bezier_sampler autoware_frenet_planner + autoware_internal_debug_msgs autoware_interpolation autoware_motion_utils autoware_perception_msgs @@ -27,7 +28,6 @@ rclcpp_components std_msgs tf2_ros - tier4_debug_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/sampling_based_planner/autoware_sampler_common/CHANGELOG.rst b/planning/sampling_based_planner/autoware_sampler_common/CHANGELOG.rst index 7e0eaaf4e7376..261ab51ebb10e 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/CHANGELOG.rst +++ b/planning/sampling_based_planner/autoware_sampler_common/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_sampler_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/planning/sampling_based_planner/autoware_sampler_common/package.xml b/planning/sampling_based_planner/autoware_sampler_common/package.xml index eb08c8125d3ee..a190c56969368 100644 --- a/planning/sampling_based_planner/autoware_sampler_common/package.xml +++ b/planning/sampling_based_planner/autoware_sampler_common/package.xml @@ -1,7 +1,7 @@ autoware_sampler_common - 0.40.0 + 0.41.0 Common classes and functions for sampling based planners Maxime CLEMENT Maxime CLEMENT diff --git a/sensing/autoware_cuda_utils/CHANGELOG.rst b/sensing/autoware_cuda_utils/CHANGELOG.rst index 92a99bc8d3267..bd0461582a882 100644 --- a/sensing/autoware_cuda_utils/CHANGELOG.rst +++ b/sensing/autoware_cuda_utils/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_cuda_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/sensing/autoware_cuda_utils/package.xml b/sensing/autoware_cuda_utils/package.xml index 32a29133201ec..1e0014357be0b 100644 --- a/sensing/autoware_cuda_utils/package.xml +++ b/sensing/autoware_cuda_utils/package.xml @@ -1,7 +1,7 @@ autoware_cuda_utils - 0.40.0 + 0.41.0 cuda utility library Daisuke Nishimatsu diff --git a/sensing/autoware_gnss_poser/CHANGELOG.rst b/sensing/autoware_gnss_poser/CHANGELOG.rst index 8195195c9b15c..d97f28dbe618b 100644 --- a/sensing/autoware_gnss_poser/CHANGELOG.rst +++ b/sensing/autoware_gnss_poser/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_gnss_poser ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* Contributors: Fumiya Watanabe, Ryohsuke Mitsudome + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp index e02c78319681b..d4ba9b994cbcb 100644 --- a/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp +++ b/sensing/autoware_gnss_poser/include/autoware/gnss_poser/gnss_poser_node.hpp @@ -14,7 +14,7 @@ #ifndef AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ #define AUTOWARE__GNSS_POSER__GNSS_POSER_NODE_HPP_ -#include +#include #include #include @@ -47,7 +47,7 @@ class GNSSPoser : public rclcpp::Node explicit GNSSPoser(const rclcpp::NodeOptions & node_options); private: - using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo; + using MapProjectorInfo = autoware::component_interface_specs_universe::map::MapProjectorInfo; void callback_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); void callback_nav_sat_fix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); diff --git a/sensing/autoware_gnss_poser/package.xml b/sensing/autoware_gnss_poser/package.xml index 6414ccbabca3f..50f9a32156f85 100644 --- a/sensing/autoware_gnss_poser/package.xml +++ b/sensing/autoware_gnss_poser/package.xml @@ -2,7 +2,7 @@ autoware_gnss_poser - 0.40.0 + 0.41.0 The ROS 2 autoware_gnss_poser package Yamato Ando Masahiro Sakamoto @@ -19,7 +19,7 @@ libboost-dev - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils autoware_geography_utils autoware_sensing_msgs diff --git a/sensing/autoware_image_diagnostics/CHANGELOG.rst b/sensing/autoware_image_diagnostics/CHANGELOG.rst index 1486e459e4754..900bdce1670cd 100644 --- a/sensing/autoware_image_diagnostics/CHANGELOG.rst +++ b/sensing/autoware_image_diagnostics/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_image_diagnostics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_image_diagnostics): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_image_diagnostics (`#9918 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files sensing/autoware_image_diagnostics +* fix(autoware_image_diagnostics): fix bugprone-branch-clone (`#9723 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/sensing/autoware_image_diagnostics/README.md b/sensing/autoware_image_diagnostics/README.md index 03858088564b3..42fcbdc8dc612 100644 --- a/sensing/autoware_image_diagnostics/README.md +++ b/sensing/autoware_image_diagnostics/README.md @@ -28,13 +28,13 @@ After all image's blocks state are evaluated, the whole image status is summariz ### Output -| Name | Type | Description | -| ----------------------------------- | --------------------------------------- | ------------------------------------- | -| `image_diag/debug/gray_image` | `sensor_msgs::msg::Image` | gray image | -| `image_diag/debug/dft_image` | `sensor_msgs::msg::Image` | discrete Fourier transformation image | -| `image_diag/debug/diag_block_image` | `sensor_msgs::msg::Image` | each block state colorization | -| `image_diag/image_state_diag` | `tier4_debug_msgs::msg::Int32Stamped` | image diagnostics status value | -| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | +| Name | Type | Description | +| ----------------------------------- | ------------------------------------------------- | ------------------------------------- | +| `image_diag/debug/gray_image` | `sensor_msgs::msg::Image` | gray image | +| `image_diag/debug/dft_image` | `sensor_msgs::msg::Image` | discrete Fourier transformation image | +| `image_diag/debug/diag_block_image` | `sensor_msgs::msg::Image` | each block state colorization | +| `image_diag/image_state_diag` | `autoware_internal_debug_msgs::msg::Int32Stamped` | image diagnostics status value | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | diagnostics | ## Parameters diff --git a/sensing/autoware_image_diagnostics/package.xml b/sensing/autoware_image_diagnostics/package.xml index 2e4556de2b92b..91edff5213f2a 100644 --- a/sensing/autoware_image_diagnostics/package.xml +++ b/sensing/autoware_image_diagnostics/package.xml @@ -2,7 +2,7 @@ autoware_image_diagnostics - 0.40.0 + 0.41.0 The autoware_image_diagnostics package Dai Nguyen Yoshi Ri @@ -12,6 +12,7 @@ ament_cmake_auto + autoware_internal_debug_msgs autoware_universe_utils cv_bridge diagnostic_updater @@ -19,7 +20,6 @@ rclcpp rclcpp_components sensor_msgs - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp index 681e9ae70a89c..57e53aee46328 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.cpp @@ -33,7 +33,7 @@ ImageDiagNode::ImageDiagNode(const rclcpp::NodeOptions & node_options) dft_image_pub_ = image_transport::create_publisher(this, "image_diag/debug/dft_image"); gray_image_pub_ = image_transport::create_publisher(this, "image_diag/debug/gray_image"); - image_state_pub_ = create_publisher( + image_state_pub_ = create_publisher( "image_diag/image_state_diag", rclcpp::SensorDataQoS()); updater_.setHardwareID("Image_Diagnostics"); @@ -171,11 +171,9 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i cv::rectangle( diag_block_image, cv::Point(x, y), cv::Point(x + block_size_h, y + block_size_v), state_color_map_["BLOCKAGE"], -1, cv::LINE_AA); - } else if (region_state_vec[j] == Image_State::LOW_VIS) { - cv::rectangle( - diag_block_image, cv::Point(x, y), cv::Point(x + block_size_h, y + block_size_v), - state_color_map_["BACKLIGHT"], -1, cv::LINE_AA); - } else if (region_state_vec[j] == Image_State::BACKLIGHT) { + } else if ( + region_state_vec[j] == Image_State::LOW_VIS || + region_state_vec[j] == Image_State::BACKLIGHT) { cv::rectangle( diag_block_image, cv::Point(x, y), cv::Point(x + block_size_h, y + block_size_v), state_color_map_["BACKLIGHT"], -1, cv::LINE_AA); @@ -227,7 +225,7 @@ void ImageDiagNode::ImageChecker(const sensor_msgs::msg::Image::ConstSharedPtr i } else { params_.diagnostic_status = 0; } - tier4_debug_msgs::msg::Int32Stamped image_state_out; + autoware_internal_debug_msgs::msg::Int32Stamped image_state_out; image_state_out.data = params_.diagnostic_status; image_state_pub_->publish(image_state_out); diff --git a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp index 325624062b90b..9383da33160f9 100644 --- a/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp +++ b/sensing/autoware_image_diagnostics/src/image_diagnostics_node.hpp @@ -21,10 +21,10 @@ #include #include +#include +#include +#include #include -#include -#include -#include #if __has_include() #include @@ -95,8 +95,9 @@ class ImageDiagNode : public rclcpp::Node image_transport::Publisher block_diag_image_pub_; image_transport::Publisher dft_image_pub_; image_transport::Publisher gray_image_pub_; - rclcpp::Publisher::SharedPtr average_pub_; - rclcpp::Publisher::SharedPtr image_state_pub_; + rclcpp::Publisher::SharedPtr + average_pub_; + rclcpp::Publisher::SharedPtr image_state_pub_; }; } // namespace autoware::image_diagnostics diff --git a/sensing/autoware_image_transport_decompressor/CHANGELOG.rst b/sensing/autoware_image_transport_decompressor/CHANGELOG.rst index 57968c49291c0..358abdb5ba7f5 100644 --- a/sensing/autoware_image_transport_decompressor/CHANGELOG.rst +++ b/sensing/autoware_image_transport_decompressor/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_image_transport_decompressor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_image_transport_decompressor): fix bugprone-branch-clone (`#9724 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/sensing/autoware_image_transport_decompressor/package.xml b/sensing/autoware_image_transport_decompressor/package.xml index 0ee5a60b2f46e..101090e2cfaf5 100644 --- a/sensing/autoware_image_transport_decompressor/package.xml +++ b/sensing/autoware_image_transport_decompressor/package.xml @@ -2,7 +2,7 @@ autoware_image_transport_decompressor - 0.40.0 + 0.41.0 The image_transport_decompressor package Yukihiro Saito Kenzo Lobos-Tsunekawa diff --git a/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp b/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp index b1971f892a352..045d1ea564fe6 100644 --- a/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp +++ b/sensing/autoware_image_transport_decompressor/src/image_transport_decompressor.cpp @@ -107,13 +107,12 @@ void ImageTransportDecompressor::onCompressedImage( } } else { std::string image_encoding; - if (encoding_ == std::string("default")) { - image_encoding = input_compressed_image_msg->format.substr(0, split_pos); - } else if (encoding_ == std::string("rgb8")) { + if (encoding_ == std::string("rgb8")) { image_encoding = "rgb8"; } else if (encoding_ == std::string("bgr8")) { image_encoding = "bgr8"; } else { + // default encoding image_encoding = input_compressed_image_msg->format.substr(0, split_pos); } diff --git a/sensing/autoware_imu_corrector/CHANGELOG.rst b/sensing/autoware_imu_corrector/CHANGELOG.rst index fe36ee24880e0..79a35f67a3daf 100644 --- a/sensing/autoware_imu_corrector/CHANGELOG.rst +++ b/sensing/autoware_imu_corrector/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package autoware_imu_corrector ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(imu_corrector): remove non-periodic publish to /diagnostics topic (`#9951 `_) + fix(imu_corrector): remove force_update() in timer callback + Co-authored-by: Takahisa.Ishikawa +* Contributors: Fumiya Watanabe, interimadd + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/sensing/autoware_imu_corrector/package.xml b/sensing/autoware_imu_corrector/package.xml index 94a2e3fe83071..c4938585b9275 100644 --- a/sensing/autoware_imu_corrector/package.xml +++ b/sensing/autoware_imu_corrector/package.xml @@ -2,7 +2,7 @@ autoware_imu_corrector - 0.40.0 + 0.41.0 The ROS 2 autoware_imu_corrector package Yamato Ando Taiki Yamada diff --git a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp index 8dcc024ffc2b4..17194ac1b0459 100644 --- a/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/autoware_imu_corrector/src/gyro_bias_estimator.cpp @@ -40,7 +40,6 @@ GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) { updater_.setHardwareID(get_name()); updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); - // diagnostic_updater is designed to be updated at the same rate as the timer updater_.setPeriod(diagnostics_updater_interval_sec_); gyro_bias_estimation_module_ = std::make_unique(); @@ -182,8 +181,6 @@ void GyroBiasEstimator::timer_callback() transform_vector3(gyro_bias_estimation_module_->get_bias_base_link(), *tf_base2imu_ptr); validate_gyro_bias(); - updater_.force_update(); - updater_.setPeriod(diagnostics_updater_interval_sec_); // to reset timer inside the updater } void GyroBiasEstimator::validate_gyro_bias() diff --git a/sensing/autoware_pcl_extensions/CHANGELOG.rst b/sensing/autoware_pcl_extensions/CHANGELOG.rst index 56c9f0bdff0dc..abf97d3a3ef80 100644 --- a/sensing/autoware_pcl_extensions/CHANGELOG.rst +++ b/sensing/autoware_pcl_extensions/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_pcl_extensions ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/sensing/autoware_pcl_extensions/package.xml b/sensing/autoware_pcl_extensions/package.xml index 840bd976b5b5f..61e76007e2927 100644 --- a/sensing/autoware_pcl_extensions/package.xml +++ b/sensing/autoware_pcl_extensions/package.xml @@ -2,7 +2,7 @@ autoware_pcl_extensions - 0.40.0 + 0.41.0 The autoware_pcl_extensions package Ryu Yamamoto Kenzo Lobos Tsunekawa diff --git a/sensing/autoware_pointcloud_preprocessor/CHANGELOG.rst b/sensing/autoware_pointcloud_preprocessor/CHANGELOG.rst index ad1ce2aaf8464..def8780dc20f8 100644 --- a/sensing/autoware_pointcloud_preprocessor/CHANGELOG.rst +++ b/sensing/autoware_pointcloud_preprocessor/CHANGELOG.rst @@ -2,6 +2,155 @@ Changelog for package autoware_pointcloud_preprocessor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_pointcloud_preprocessor): redesign concatenate and time sync node (`#8300 `_) + * chore: rebase main + * chore: solve conflicts + * chore: fix cpp check + * chore: add diagnostics readme + * chore: update figure + * chore: upload jitter.png and add old design link + * chore: add the link to the tool for analyzing timestamp + * fix: fix bug that timer didn't cancel + * chore: fix logic for logging + * Update sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/schema/cocatenate_and_time_sync_node.schema.json + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/schema/cocatenate_and_time_sync_node.schema.json + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * chore: remove distortion corrector related changes + * feat: add managed tf buffer + * chore: fix filename + * chore: add explanataion for maximum queue size + * chore: add explanation for timeout_sec + * chore: fix schema's explanation + * chore: fix description for twist and odom + * chore: remove license that are not used + * chore: change guard to prama once + * chore: default value change to string + * Update sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * style(pre-commit): autofix + * chore: clang-tidy style for static constexpr + * chore: remove unused vector header + * chore: fix naming concatenated_cloud_publisher + * chore: fix namimg diagnostic_updater\_ + * chore: specify parameter in comment + * chore: change RCLCPP_WARN to RCLCPP_WARN_STREAM_THROTTLE + * chore: add comment for cancelling timer + * chore: Simplify loop structure for topic-to-cloud mapping + * chore: fix spell errors + * chore: fix more spell error + * chore: rename mutex and lock + * chore: const reference for string parameter + * chore: add explaination for RclcppTimeHash\_ + * chore: change the concatenate node to parent node + * chore: clean processOdometry and processTwist + * chore: change twist shared pointer queue to twist queue + * chore: refactor compensate pointcloud to function + * chore: reallocate memory for concatenate_cloud_ptr + * chore: remove new to make shared + * chore: dis to distance + * chore: refacotr poitncloud_sub + * chore: return early return but throw runtime error + * chore: replace #define DEFAULT_SYNC_TOPIC_POSTFIX with member variable + * chore: fix spell error + * chore: remove redundant function call + * chore: replace conplex tuple to structure + * chore: use reference instead of a pointer to conveys node + * chore: fix camel to snake case + * chore: fix logic of publish synchronized pointcloud + * chore: fix cpp check + * chore: remove logging and throw error directly + * chore: fix clangd warnings + * chore: fix json schema + * chore: fix clangd warning + * chore: remove unused variable + * chore: fix launcher + * chore: fix clangd warning + * chore: ensure thread safety + * style(pre-commit): autofix + * chore: clean code + * chore: add parameters for handling rosbag replay in loops + * chore: fix diagonistic + * chore: reduce copy operation + * chore: reserve space for concatenated pointcloud + * chore: fix clangd error + * chore: fix pipeline latency + * chore: add debug mode + * chore: refactor convert_to_xyzirc_cloud function + * chore: fix json schema + * chore: fix logging output + * chore: fix the output order of the debug mode + * chore: fix pipeline latency output + * chore: clean code + * chore: set some parameters to false in testing + * chore: fix default value for schema + * chore: fix diagnostic msgs + * chore: fix parameter for sample ros bag + * chore: update readme + * chore: fix empty pointcloud + * chore: remove duplicated logic + * chore: fix logic for handling empty pointcloud + * chore: clean code + * chore: remove rosbag_replay parameter + * chore: remove nodelet cpp + * chore: clang tidy warning + * feat: add naive approach for unsynchronized pointclouds + * chore: add more explanations in docs for naive approach + * feat: refactor naive method and fix the multithreading issue + * chore: set parameter to naive + * chore: fix parameter + * chore: fix readme + * Update sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * Update sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + * style(pre-commit): autofix + * feat: remove mutually exclusive approaches + * chore: fix spell error + * chore: remove unused variable + * refactor: refactor collectorInfo to polymorphic + * chore: fix variable name + * chore: fix figure and diagnostic msg in readme + * chroe: refactor collectorinfo structure + * chore: revert wrong file changes + * chore: improve message + * chore: remove unused input topics + * chore: change to explicit check + * chore: tier4 debug msgs to autoware internal debug msgs + * chore: update documentation + --------- + Co-authored-by: Max Schmeller <6088931+mojomex@users.noreply.github.com> + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* feat(autoware_pointcloud_preprocessor): tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_pointcloud_preprocessor (`#9920 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files sensing/autoware_pointcloud_preprocessor +* fix(autoware_pointcloud_preprocessor): fix autoware pointcloud preprocessor docs (`#9765 `_) + * fix downsample and passthrough + * fix: fix blockage-diag docs that page is not shown + --------- +* fix(autoware_pointcloud_preprocessor): fix image display in distortion corrector (`#9761 `_) + fix: fix image display +* fix(autoware_pointcloud_preprocessor): remove unused function mask() (`#9751 `_) +* fix: enable to copy all information in pickup based pointcloud downsampler (`#9686 `_) + enable to copy all information in downsampler +* Contributors: Fumiya Watanabe, Ryuta Kambe, Vishal Chauhan, Yi-Hsiang Fang (Vivid), Yoshi Ri + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt index 56dab521d7dff..19c698b11bc5f 100644 --- a/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/autoware_pointcloud_preprocessor/CMakeLists.txt @@ -64,7 +64,10 @@ ament_target_dependencies(faster_voxel_grid_downsample_filter ) ament_auto_add_library(pointcloud_preprocessor_filter SHARED - src/concatenate_data/concatenate_and_time_sync_nodelet.cpp + src/concatenate_data/concatenate_and_time_sync_node.cpp + src/concatenate_data/combine_cloud_handler.cpp + src/concatenate_data/cloud_collector.cpp + src/concatenate_data/collector_matching_strategy.cpp src/concatenate_data/concatenate_pointclouds.cpp src/crop_box_filter/crop_box_filter_node.cpp src/time_synchronizer/time_synchronizer_node.cpp @@ -111,7 +114,7 @@ rclcpp_components_register_node(pointcloud_preprocessor_filter # ========== Concatenate and Sync data ========== rclcpp_components_register_node(pointcloud_preprocessor_filter PLUGIN "autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent" - EXECUTABLE concatenate_data_node) + EXECUTABLE concatenate_and_time_sync_node) # ========== CropBox ========== rclcpp_components_register_node(pointcloud_preprocessor_filter @@ -243,8 +246,17 @@ if(BUILD_TESTING) test/test_distortion_corrector_node.cpp ) + ament_add_gtest(test_concatenate_node_unit + test/test_concatenate_node_unit.cpp + ) + target_link_libraries(test_utilities pointcloud_preprocessor_filter) target_link_libraries(test_distortion_corrector_node pointcloud_preprocessor_filter) + target_link_libraries(test_concatenate_node_unit pointcloud_preprocessor_filter) + add_ros_test( + test/test_concatenate_node_component.py + TIMEOUT "50" + ) endif() diff --git a/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml b/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml new file mode 100644 index 0000000000000..56fe6643b9973 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/config/concatenate_and_time_sync_node.param.yaml @@ -0,0 +1,21 @@ +/**: + ros__parameters: + debug_mode: false + has_static_tf_only: false + rosbag_length: 10.0 + maximum_queue_size: 5 + timeout_sec: 0.2 + is_motion_compensated: true + publish_synchronized_pointcloud: true + keep_input_frame_in_synchronized_pointcloud: true + publish_previous_but_late_pointcloud: false + synchronized_pointcloud_postfix: pointcloud + input_twist_topic_type: twist + input_topics: [ + "/sensing/lidar/right/pointcloud_before_sync", + "/sensing/lidar/top/pointcloud_before_sync", + "/sensing/lidar/left/pointcloud_before_sync", + ] + output_frame: base_link + matching_strategy: + type: naive diff --git a/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md similarity index 64% rename from sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md rename to sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md index 911cda3823021..a9dcb8437ec67 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/blockage_diag.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/blockage-diag.md @@ -38,16 +38,16 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Output -| Name | Type | Description | -| :-------------------------------------------------------- | :-------------------------------------- | ------------------------------------------------------------------------------------------------ | -| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | -| `~/output/blockage_diag/debug/ground_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | -| `~/output/blockage_diag/debug/sky_blockage_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | -| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | -| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | -| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | -| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | -| `~/output/blockage_diag/debug/ground_dust_ratio` | `tier4_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | +| Name | Type | Description | +| :-------------------------------------------------------- | :-------------------------------------------------- | ------------------------------------------------------------------------------------------------ | +| `~/output/blockage_diag/debug/blockage_mask_image` | `sensor_msgs::msg::Image` | The mask image of detected blockage | +| `~/output/blockage_diag/debug/ground_blockage_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in ground region | +| `~/output/blockage_diag/debug/sky_blockage_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The area ratio of blockage region in sky region | +| `~/output/blockage_diag/debug/lidar_depth_map` | `sensor_msgs::msg::Image` | The depth map image of input point cloud | +| `~/output/blockage_diag/debug/single_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of detected dusty area in latest single frame | +| `~/output/blockage_diag/debug/multi_frame_dust_mask` | `sensor_msgs::msg::Image` | The mask image of continuous detected dusty area | +| `~/output/blockage_diag/debug/blockage_dust_merged_image` | `sensor_msgs::msg::Image` | The merged image of blockage detection(red) and multi frame dusty area detection(yellow) results | +| `~/output/blockage_diag/debug/ground_dust_ratio` | `autoware_internal_debug_msgs::msg::Float32Stamped` | The ratio of dusty area divided by area where ray usually returns from the ground. | ## Parameters diff --git a/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md b/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md index 08f7b92f88975..f640608cf91ec 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md @@ -1,75 +1,224 @@ -# concatenate_data +# concatenate_and_time_synchronize_node ## Purpose -Many self-driving cars combine multiple LiDARs to expand the sensing range. Therefore, a function to combine a plurality of point clouds is required. +The `concatenate_and_time_synchronize_node` is a node designed to combine and synchronize multiple point clouds into a single, unified point cloud. By integrating data from multiple LiDARs, this node significantly enhances the sensing range and coverage of autonomous vehicles, enabling more accurate perception of the surrounding environment. Synchronization ensures that point clouds are aligned temporally, reducing errors caused by mismatched timestamps. -To combine multiple sensor data with a similar timestamp, the [message_filters](https://github.com/ros2/message_filters) is often used in the ROS-based system, but this requires the assumption that all inputs can be received. Since safety must be strongly considered in autonomous driving, the point clouds concatenate node must be designed so that even if one sensor fails, the remaining sensor information can be output. +For example, consider a vehicle equipped with three LiDAR sensors mounted on the left, right, and top positions. Each LiDAR captures data from its respective field of view, as shown below: -## Inner-workings / Algorithms +| Left | Top | Right | +| :-----------------------------------------------: | :---------------------------------------------: | :-------------------------------------------------: | +| ![Concatenate Left](./image/concatenate_left.png) | ![Concatenate Top](./image/concatenate_top.png) | ![Concatenate Right](./image/concatenate_right.png) | -The figure below represents the reception time of each sensor data and how it is combined in the case. +After processing the data through the `concatenate_and_time_synchronize_node`, the outputs from all LiDARs are combined into a single comprehensive point cloud that provides a complete view of the environment: -![concatenate_data_timing_chart](./image/concatenate_data.drawio.svg) +![Full Scene View](./image/concatenate_all.png) + +This resulting point cloud allows autonomous systems to detect obstacles, map the environment, and navigate more effectively, leveraging the complementary fields of view from multiple LiDAR sensors. + +## Inner Workings / Algorithms + +![concatenate_algorithm](./image/concatenate_algorithm.drawio.svg) + +### Step 1: Match and Create Collector + +When a point cloud arrives, its timestamp is checked, and an offset is subtracted to get the reference timestamp. The node then checks if there is an existing collector with the same reference timestamp. If such a collector exists, the point cloud is added to it. If no such collector exists, a new collector is created with the reference timestamp. + +### Step 2: Trigger the Timer + +Once a collector is created, a timer for that collector starts counting down (this value is defined by `timeout_sec`). The collector begins to concatenate the point clouds either when all point clouds defined in `input_topics` have been collected or when the timer counts down to zero. + +### Step 3: Concatenate the Point Clouds + +The concatenation process involves merging multiple point clouds into a single, concatenated point cloud. The timestamp of the concatenated point cloud will be the earliest timestamp from the input point clouds. By setting the parameter `is_motion_compensated` to `true`, the node will consider the timestamps of the input point clouds and utilize the `twist` information from `geometry_msgs::msg::TwistWithCovarianceStamped` to compensate for motion, aligning the point cloud to the selected (earliest) timestamp. + +### Step 4: Publish the Point Cloud + +After concatenation, the concatenated point cloud is published, and the collector is deleted to free up resources. ## Inputs / Outputs ### Input -| Name | Type | Description | -| --------------- | ------------------------------------------------ | ----------------------------------------------------------------------------- | -| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | The vehicle odometry is used to interpolate the timestamp of each sensor data | +| Name | Type | Description | +| --------------- | ------------------------------------------------ | -------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | Twist information adjusts the point cloud scans based on vehicle motion, allowing LiDARs with different timestamps to be synchronized for concatenation. | +| `~/input/odom` | `nav_msgs::msg::Odometry` | Vehicle odometry adjusts the point cloud scans based on vehicle motion, allowing LiDARs with different timestamps to be synchronized for concatenation. | + +By setting the `input_twist_topic_type` parameter to `twist` or `odom`, the subscriber will subscribe to either `~/input/twist` or `~/input/odom`. If the user doesn't want to use the twist information or vehicle odometry to compensate for motion, set `is_motion_compensated` to `false`. ### Output | Name | Type | Description | | ----------------- | ------------------------------- | ------------------------- | -| `~/output/points` | `sensor_msgs::msg::Pointcloud2` | concatenated point clouds | +| `~/output/points` | `sensor_msgs::msg::Pointcloud2` | Concatenated point clouds | + +### Core Parameters -## Parameters +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json") }} -| Name | Type | Default Value | Description | -| -------------------- | ---------------- | ------------- | ------------------------------------------------------------------- | -| `input/points` | vector of string | [] | input topic names that type must be `sensor_msgs::msg::Pointcloud2` | -| `input_frame` | string | "" | input frame id | -| `output_frame` | string | "" | output frame id | -| `has_static_tf_only` | bool | false | flag to listen TF only once | -| `max_queue_size` | int | 5 | max queue size of input/output topics | +### Parameter Settings -### Core Parameters +If you didn't synchronize your LiDAR sensors, set the `type` parameter of `matching_strategy` to `naive` to concatenate the point clouds directly. However, if your LiDAR sensors are synchronized, set type to `advanced` and adjust the `lidar_timestamp_offsets` and `lidar_timestamp_noise_window` parameters accordingly. + +The three parameters, `timeout_sec`, `lidar_timestamp_offsets`, and `lidar_timestamp_noise_window`, are essential for efficiently collecting point clouds in the same collector and managing edge cases (point cloud drops or delays) effectively. + +#### timeout_sec + +When network issues occur or when point clouds experience delays in the previous processing pipeline, some point clouds may be delayed or dropped. To address this, the `timeout_sec` parameter is used. Once the timer is created, it will start counting down from `timeout_sec`. If the timer reaches zero, the collector will not wait for delayed or dropped point clouds but will concatenate the remaining point clouds in the collector directly. The figure below demonstrates how `timeout_sec` works with `concatenate_and_time_sync_node` when `timeout_sec` is set to `0.12` (120 ms). + +![concatenate_edge_case](./image/concatenate_edge_case.drawio.svg) + +#### lidar_timestamp_offsets + +Since different vehicles have varied designs for LiDAR scanning, the timestamps of each LiDAR may differ. Users need to know the offsets between each LiDAR and set the values in `lidar_timestamp_offsets`. + +To monitor the timestamps of each LiDAR, run the following command: + +```bash +ros2 topic echo "pointcloud_topic" --field header +``` + +The timestamps should increase steadily by approximately 100 ms, as per the Autoware default. You should see output like this: + +```bash +nanosec: 156260951 +nanosec: 257009560 +nanosec: 355444581 +``` + +This pattern indicates a LiDAR timestamp of 0.05. + +If there are three LiDARs (left, right, top), and the timestamps for the left, right, and top point clouds are `0.01`, `0.05`, and `0.09` seconds respectively, the parameters should be set as [0.0, 0.04, 0.08]. This reflects the timestamp differences between the current point cloud and the point cloud with the earliest timestamp. Note that the order of the `lidar_timestamp_offsets` corresponds to the order of the `input_topics`. + +The figure below demonstrates how `lidar_timestamp_offsets` works with `concatenate_and_time_sync_node`. + +![ideal_timestamp_offset](./image/ideal_timestamp_offset.drawio.svg) + +#### lidar_timestamp_noise_window + +Additionally, due to the mechanical design of LiDARs, there may be some jitter in the timestamps of each scan, as shown in the image below. For example, if the scan frequency is set to 10 Hz (scanning every 100 ms), the timestamps between each scan might not be exactly 100 ms apart. To handle this noise, the `lidar_timestamp_noise_window` parameter is provided. + +Users can use [this tool](https://github.com/tier4/timestamp_analyzer) to visualize the noise between each scan. + +![jitter](./image/jitter.png) + +From the example above, the noise ranges from 0 to 8 ms, so the user should set `lidar_timestamp_noise_window` to `0.008`. -| Name | Type | Default Value | Description | -| --------------------------------- | ---------------- | ------------- | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | -| `input_offset` | vector of double | [] | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers.
For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). | -| `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. | -| `input_twist_topic_type` | std::string | twist | Topic type for twist. Currently support `twist` or `odom`. | +The figure below demonstrates how `lidar_timestamp_noise_window` works with the `concatenate_and_time_sync_node`. If the green `X` is within the range of the red triangles, it indicates that the point cloud matches the reference timestamp of the collector. -## Actual Usage +![noise_timestamp_offset](./image/noise_timestamp_offset.drawio.svg) -For the example of actual usage of this node, please refer to the [preprocessor.launch.py](../launch/preprocessor.launch.py) file. +## Launch + +```bash +# The launch file will read the parameters from the concatenate_and_time_sync_node.param.yaml +ros2 launch autoware_pointcloud_preprocessor concatenate_and_time_sync_node.launch.xml +``` -### How to tuning timeout_sec and input_offset +## Test -The values in `timeout_sec` and `input_offset` are used in the timer_callback to control concatenation timings. +```bash +# build autoware_pointcloud_preprocessor +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-up-to autoware_pointcloud_preprocessor -- Assumptions - - when the timer runs out, we concatenate the pointclouds in the buffer - - when the first pointcloud comes to buffer, we reset the timer to `timeout_sec` - - when the second and later pointclouds comes to buffer, we reset the timer to `timeout_sec` - `input_offset` - - we assume all lidar has same frequency +# test autoware_pointcloud_preprocessor +colcon test --packages-select autoware_pointcloud_preprocessor --event-handlers console_cohesion+ +``` -| Name | Description | How to tune | -| -------------- | ---------------------------------------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `timeout_sec` | timeout sec for default timer | To avoid mis-concatenation, at least this value must be shorter than sampling time. | -| `input_offset` | timeout extension when a pointcloud comes to buffer. | The amount of waiting time will be `timeout_sec` - `input_offset`. So, you will need to set larger value for the last-coming pointcloud and smaller for fore-coming. | +## Debug and Diagnostics -### Node separation options for future +To verify whether the node has successfully concatenated the point clouds, the user can examine rqt or the `/diagnostics` topic using the following command: -Since the pointcloud concatenation has two process, "time synchronization" and "pointcloud concatenation", it is possible to separate these processes. +```bash +ros2 topic echo /diagnostics +``` -In the future, Nodes will be completely separated in order to achieve node loosely coupled nature, but currently both nodes can be selected for backward compatibility ([See this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)). +Below is an example output when the point clouds are concatenated successfully: -## Assumptions / Known limits +- Each point cloud has a value of `True`. +- The `cloud_concatenation_success` is `True`. +- The `level` value is `\0`. (diagnostic_msgs::msg::DiagnosticStatus::OK) -It is necessary to assume that the vehicle odometry value exists, the sensor data and odometry timestamp are correct, and the TF from `base_link` to `sensor_frame` is also correct. +```bash +header: + stamp: + sec: 1722492015 + nanosec: 848508777 + frame_id: '' +status: +- level: "\0" + name: 'concatenate_and_time_sync_node: concat_status' + message: Concatenated pointcloud is published and include all topics + hardware_id: concatenate_data_checker + values: + - key: concatenated_cloud_timestamp + value: '1718260240.159229994' + - key: reference_timestamp_min + value: '1718260240.149230003' + - key: reference_timestamp_max + value: '1718260240.169229984' + - key: /sensing/lidar/left/pointcloud_before_sync/timestamp + value: '1718260240.159229994' + - key: /sensing/lidar/left/pointcloud_before_sync/is_concatenated + value: 'True' + - key: /sensing/lidar/right/pointcloud_before_sync/timestamp + value: '1718260240.194104910' + - key: /sensing/lidar/right/pointcloud_before_sync/is_concatenated + value: 'True' + - key: /sensing/lidar/top/pointcloud_before_sync/timestamp + value: '1718260240.234578133' + - key: /sensing/lidar/top/pointcloud_before_sync/is_concatenated + value: 'True' + - key: cloud_concatenation_success + value: 'True' +``` + +Below is an example when point clouds fail to concatenate successfully. + +- Some point clouds might have values of `False`. +- The `cloud_concatenation_success` is `False`. +- The `level` value is `\x02`. (diagnostic_msgs::msg::DiagnosticStatus::ERROR) + +```bash +header: + stamp: + sec: 1722492663 + nanosec: 344942959 + frame_id: '' +status: +- level: "\x02" + name: 'concatenate_and_time_sync_node: concat_status' + message: Concatenated pointcloud is published but miss some topics + hardware_id: concatenate_data_checker + values: + - key: concatenated_cloud_timestamp + value: '1718260240.859827995' + - key: reference_timestamp_min + value: '1718260240.849828005' + - key: reference_timestamp_max + value: '1718260240.869827986' + - key: /sensing/lidar/left/pointcloud_before_sync/timestamp + value: '1718260240.859827995' + - key: /sensing/lidar/left/pointcloud_before_sync/is_concatenated + value: 'True' + - key: /sensing/lidar/right/pointcloud_before_sync/timestamp + value: '1718260240.895193815' + - key: /sensing/lidar/right/pointcloud_before_sync/is_concatenated + value: 'True' + - key: /sensing/lidar/top/pointcloud_before_sync/is_concatenated + value: 'False' + - key: cloud_concatenation_success + value: 'False' +``` + +## Node separation options + +There is also an option to separate the concatenate_and_time_sync_node into two nodes: one for `time synchronization` and another for `concatenate pointclouds` ([See this PR](https://github.com/autowarefoundation/autoware.universe/pull/3312)). + +Note that the `concatenate_pointclouds` and `time_synchronizer_nodelet` are using the [old design](https://github.com/autowarefoundation/autoware.universe/blob/9bb228fe5b7fa4c6edb47e4713c73489a02366e1/sensing/autoware_pointcloud_preprocessor/docs/concatenate-data.md) of the concatenate node. + +## Assumptions / Known Limits + +- If `is_motion_compensated` is set to `false`, the `concatenate_and_time_sync_node` will directly concatenate the point clouds without applying for motion compensation. This can save several milliseconds depending on the number of LiDARs being concatenated. Therefore, if the timestamp differences between point clouds are negligible, the user can set `is_motion_compensated` to `false` and omit the need for twist or odometry input for the node. +- As mentioned above, the user should clearly understand how their LiDAR's point cloud timestamps are managed to set the parameters correctly. If the user does not synchronize the point clouds, please set `matching_strategy.type` to `naive`. diff --git a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md index 75cdccc4453ba..032798e9db125 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/distortion-corrector.md @@ -55,16 +55,9 @@ ros2 launch autoware_pointcloud_preprocessor distortion_corrector.launch.xml - `hesai`: (x: 90 degrees, y: 0 degrees) - `others`: (x: 0 degrees, y: 90 degrees) and (x: 270 degrees, y: 0 degrees) - - - - - - - - - -
velodyne azimuth coordinatehesai azimuth coordinate

Velodyne azimuth coordinate

Hesai azimuth coordinate

+| ![Velodyne Azimuth Coordinate](./image/velodyne.drawio.png) | ![Hesai Azimuth Coordinate](./image/hesai.drawio.png) | +| :---------------------------------------------------------: | :---------------------------------------------------: | +| **Velodyne azimuth coordinate** | **Hesai azimuth coordinate** | ## References/External links diff --git a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md index 92ca1d3b3081c..f237a1871b5e5 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/downsample-filter.md @@ -48,7 +48,7 @@ These implementations inherit `autoware::pointcloud_preprocessor::Filter` class, ### Pickup Based Voxel Grid Downsample Filter -{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/pickup_based_voxel_grid_downsample_filter.schema.json") }} +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/pickup_based_voxel_grid_downsample_filter_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md index 8f4da273861a3..0d0423d40ab83 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/dual-return-outlier-filter.md @@ -36,11 +36,11 @@ This implementation inherits `autoware::pointcloud_preprocessor::Filter` class, ### Output -| Name | Type | Description | -| ---------------------------------------------- | --------------------------------------- | ------------------------------------------------------- | -| `/dual_return_outlier_filter/frequency_image` | `sensor_msgs::msg::Image` | The histogram image that represent visibility | -| `/dual_return_outlier_filter/visibility` | `tier4_debug_msgs::msg::Float32Stamped` | A representation of visibility with a value from 0 to 1 | -| `/dual_return_outlier_filter/pointcloud_noise` | `sensor_msgs::msg::Pointcloud2` | The pointcloud removed as noise | +| Name | Type | Description | +| ---------------------------------------------- | --------------------------------------------------- | ------------------------------------------------------- | +| `/dual_return_outlier_filter/frequency_image` | `sensor_msgs::msg::Image` | The histogram image that represent visibility | +| `/dual_return_outlier_filter/visibility` | `autoware_internal_debug_msgs::msg::Float32Stamped` | A representation of visibility with a value from 0 to 1 | +| `/dual_return_outlier_filter/pointcloud_noise` | `sensor_msgs::msg::Pointcloud2` | The pointcloud removed as noise | ## Parameters diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_algorithm.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_algorithm.drawio.svg new file mode 100644 index 0000000000000..0ca825f5acaa6 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_algorithm.drawio.svg @@ -0,0 +1,810 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Collector +
+
+
+ + + + + + + + + + + + + + +
+
+
+ left pc +
+
+
+ + + + + + + + + + + + + + +
+
+
+ top pc +
+
+
+ + + + + + + + + + + + + + +
+
+
+ right pc +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ L +
+
+
+ + + + + + + + + + + + + + +
+
+
stamp: t1_left
+
+
+ + + + + + + + + + + + + + +
+
+
+ stamp: t1_right +
+
+
+ + + + + + + + + + + + + + +
+
+
stamp: t1_top
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ reference timestamp +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ T +
+
+
+ + + + + + + + + + + + + + +
+
+
+ R +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
arrival time
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ compare with the +
reference timestamp.
+
If match, add to the group
+
if it doesn't match, create new group
+
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ Left pointcloud +
+
+
+ + + + + + + + + + + + + + +
+
+
+ Right pointcloud +
+
+
+ + + + + + + + + + + + + + +
+
+
Top pointcloud
+
+
+ + + + + + + + + + + + + + +
+
+
+ timer +
+
+
+ + + + + + + + + + + + + + +
+
+
+ add to the collector +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ create a collector +
+
+
+ + + + + + + + + + + + + + +
+
+
+ set stamp as +
reference timestamp
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ trigger the timer +
+
+
+ + + + + + + + + + + + + + +
+
+
+ When timer count to zero +
+ concatenate + publish +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ When group has +
left, right, top pointclouds
+
+ + concatenate + publish + +
+
+
+
+ + + + + + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_all.png b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_all.png new file mode 100644 index 0000000000000..2410df95a74c7 Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_all.png differ diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg deleted file mode 100644 index 786fee4c22ed7..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_data.drawio.svg +++ /dev/null @@ -1,298 +0,0 @@ - - - - - - - - - - -
-
-
- input topic 1 -
-
-
- - input topic... - - - - - - -
-
-
- input topic 2 -
-
-
- - input topic... - - - - - - -
-
-
- input topic 3 -
-
-
- - input topic... - - - - - - - - - -
-
-
- concatenated topic -
-
-
- - concatenate... - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- timer -
- start -
-
-
- - timer... - - - - - - -
-
-
- this data is abandoned -
-
-
- - this data i... - - - - - - - - - - - -
-
-
- t0 -
-
-
- - t0 - - - - - - -
-
-
- t1 -
-
-
- - t1 - - - - - - -
-
-
- t2 -
-
-
- - t2 - - - - - - -
-
-
- t3 -
-
-
- - t3 - - - - - - - - - - - - - - - - -
-
-
- timer -
- start -
-
-
- - timer... - - - - - - - -
-
-
- timeout -
-
-
- - timeout - - - - - - - - -
-
-
- t4 -
-
-
- - t4 - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_edge_case.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_edge_case.drawio.svg new file mode 100644 index 0000000000000..33835b6396e51 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_edge_case.drawio.svg @@ -0,0 +1,2361 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 50 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 100 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 150 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 50 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 100 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 150 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 50 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 100 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 150 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 50 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 100 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 150 +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+
+ receive all pc +
+ concatenate +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+
+ collector + 1: + timeout +
+ concatenate +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + Normal + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ + One pointcloud + +
+ + drop + +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Several pointclouds + +
+ + delay + +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Several pointclouds + +
+ + delay, and one drop + +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Several pointclouds + +
+ + delay, and drop + +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + One pointcloud + +
+ + delay + +
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 50 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 100 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 150 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ collector + 1 created. +
+
timer start
+
+
+
+ + + + + + + + + + + + + + +
+
+
+
collector 2 created.
+
timer start
+
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+
+ collector + 2 +
+
concatenate
+
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+
+ collector + 1 created. +
+
timer start
+
+
+
+ + + + + + + + + + + + + + +
+
+
+
+ collector + 1 +
+
concatenate
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 50 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 100 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 150 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ collector + 1 created. +
+
timer start
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ collector + 1 +
+
concatenate
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 50 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 100 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 150 +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ collector + 1 created. +
+
timer start
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+
+ collector + 1 +
+
concatenate
+
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+
Can decide to
+
publish or not
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + +
+
+
120 ms
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
120 ms
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
120 ms
+
+
+ + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_left.png b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_left.png new file mode 100644 index 0000000000000..87e78ab91fed6 Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_left.png differ diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_right.png b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_right.png new file mode 100644 index 0000000000000..cd6d64eabd49a Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_right.png differ diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_top.png b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_top.png new file mode 100644 index 0000000000000..c317642bd9799 Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/concatenate_top.png differ diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/ideal_timestamp_offset.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/ideal_timestamp_offset.drawio.svg new file mode 100644 index 0000000000000..0b8eca9dd75aa --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/docs/image/ideal_timestamp_offset.drawio.svg @@ -0,0 +1,784 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0.01 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.05 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.09 +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ reference +
stamp
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0.01 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.05 +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ reference +
stamp
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ minus offset 40 ms +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ minus offset 80 ms +
+
+
+ + + + + + + + + + + + + + +
+
+
+ lidar_timestamp_offset: [0.0, 0.04, 0.08] +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ ideal timestamp +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ first arrive pointcloud +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ reference timestamp +
+
+
+ + + + + + + + + + + + + + + + + + + + + +
+
+
+ minus offset 80 ms +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ minus offset 40 ms +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ compare with +
reference stamp
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.09 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ compare with +
reference stamp
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ compare with +
reference stamp
+
+
+
+ + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/jitter.png b/sensing/autoware_pointcloud_preprocessor/docs/image/jitter.png new file mode 100644 index 0000000000000..c984e940171a6 Binary files /dev/null and b/sensing/autoware_pointcloud_preprocessor/docs/image/jitter.png differ diff --git a/sensing/autoware_pointcloud_preprocessor/docs/image/noise_timestamp_offset.drawio.svg b/sensing/autoware_pointcloud_preprocessor/docs/image/noise_timestamp_offset.drawio.svg new file mode 100644 index 0000000000000..bf5d604d2fb94 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/docs/image/noise_timestamp_offset.drawio.svg @@ -0,0 +1,2023 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0.01 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.05 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.09 +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ reference +
stamp
+
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ 0 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 20 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 40 +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ reference +
stamp
+
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ minus offset 20 ms +
+
+
+ + + + + + + + + + + + + + + + + + + + +
+
+
+ minus offset 40 ms +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
-5ms
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+10ms
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
5
+
+
+ + + + + + + + + + + + + + +
+
+
-5
+
+
+ + + + + + + + + + + + + + +
+
+
-5
+
+
+ + + + + + + + + + + + + + +
+
+
-10
+
+
+ + + + + + + + + + + + + + +
+
+
5
+
+
+ + + + + + + + + + + + + + +
+
+
10
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ minus offset 40 ms +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
-15
+
+
+ + + + + + + + + + + + + + +
+
+
-5
+
+
+ + + + + + + + + + + + + + +
+
+
5
+
+
+ + + + + + + + + + + + + + +
+
+
15
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
-15ms
+
+
+ + + + + + + + + + + + + + +
+
+
+ minus offset 80 ms +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
-10
+
+
+ + + + + + + + + + + + + + +
+
+
10
+
+
+ + + + + + + + + + + + + + +
+
+
5
+
+
+ + + + + + + + + + + + + + +
+
+
-5
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+5ms
+
+
+ + + + + + + + + + + + + + +
+
+
+10ms
+
+
+ + + + + + + + + + + + + + +
+
+
10
+
+
+ + + + + + + + + + + + + + +
+
+
-10
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
-10
+
+
+ + + + + + + + + + + + + + +
+
+
10
+
+
+ + + + + + + + + + + + + + +
+
+
15
+
+
+ + + + + + + + + + + + + + +
+
+
-15
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
-15ms
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ ideal timestamp +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ first arrive pointcloud +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ reference timestamp +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
real timestamp
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ possible reference timestamp +
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ possible timestamp +
for non-first pointcloud
+
+
+
+ + + + + + + + + + + + + + + + + + + +
+
+
+ timestamp of non-first pointcloud +
+
+
+ + + + + + + + + + + + + + +
+
+
+ lidar_timestamp_offset: [0.0, 0.04, 0.08] +
+ + lidar_timestamp_noise_window: [0.005, 0.01, 0.015] +
+ +
+
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Case 2 + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ + Case 1 + +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.01 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.05 +
+
+
+ + + + + + + + + + + + + + +
+
+
+ 0.09 +
+
+
+ + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md b/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md index 58da60c8fe90e..25d2f3d4127a0 100644 --- a/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md +++ b/sensing/autoware_pointcloud_preprocessor/docs/passthrough-filter.md @@ -25,8 +25,7 @@ The `passthrough_filter` is a node that removes points on the outside of a range ### Core Parameters -{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/passthrough_filter_uint16_node.schema.json -") }} +{{ json_to_markdown("sensing/autoware_pointcloud_preprocessor/schema/passthrough_filter_uint16_node.schema.json") }} ## Assumptions / Known limits diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp index 3601b492c4fe3..2dbda770f6cec 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/blockage_diag/blockage_diag_node.hpp @@ -22,10 +22,10 @@ #include #include +#include #include #include #include -#include #if __has_include() #include @@ -58,10 +58,13 @@ class BlockageDiagComponent : public autoware::pointcloud_preprocessor::Filter image_transport::Publisher single_frame_dust_mask_pub; image_transport::Publisher multi_frame_dust_mask_pub; image_transport::Publisher blockage_dust_merged_pub; - rclcpp::Publisher::SharedPtr ground_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr sky_blockage_ratio_pub_; - rclcpp::Publisher::SharedPtr ground_dust_ratio_pub_; - rclcpp::Publisher::SharedPtr blockage_type_pub_; + rclcpp::Publisher::SharedPtr + ground_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr + sky_blockage_ratio_pub_; + rclcpp::Publisher::SharedPtr + ground_dust_ratio_pub_; + rclcpp::Publisher::SharedPtr blockage_type_pub_; private: void onBlockageChecker(DiagnosticStatusWrapper & stat); diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp new file mode 100644 index 0000000000000..13604569df9a8 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp @@ -0,0 +1,89 @@ +// 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. + +#pragma once + +#include "combine_cloud_handler.hpp" + +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +class PointCloudConcatenateDataSynchronizerComponent; +class CombineCloudHandler; + +struct CollectorInfoBase +{ + virtual ~CollectorInfoBase() = default; +}; + +struct NaiveCollectorInfo : public CollectorInfoBase +{ + double timestamp; + + explicit NaiveCollectorInfo(double timestamp = 0.0) : timestamp(timestamp) {} +}; + +struct AdvancedCollectorInfo : public CollectorInfoBase +{ + double timestamp; + double noise_window; + + explicit AdvancedCollectorInfo(double timestamp = 0.0, double noise_window = 0.0) + : timestamp(timestamp), noise_window(noise_window) + { + } +}; + +class CloudCollector +{ +public: + CloudCollector( + std::shared_ptr && ros2_parent_node, + std::shared_ptr & combine_cloud_handler, int num_of_clouds, + double timeout_sec, bool debug_mode); + bool topic_exists(const std::string & topic_name); + bool process_pointcloud( + const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud); + void concatenate_callback(); + + ConcatenatedCloudResult concatenate_pointclouds( + std::unordered_map topic_to_cloud_map); + + std::unordered_map + get_topic_to_cloud_map(); + + [[nodiscard]] bool concatenate_finished() const; + + void set_info(std::shared_ptr collector_info); + [[nodiscard]] std::shared_ptr get_info() const; + void show_debug_message(); + +private: + std::shared_ptr ros2_parent_node_; + std::shared_ptr combine_cloud_handler_; + rclcpp::TimerBase::SharedPtr timer_; + std::unordered_map topic_to_cloud_map_; + uint64_t num_of_clouds_; + double timeout_sec_; + bool debug_mode_; + bool concatenate_finished_{false}; + std::mutex concatenate_mutex_; + std::shared_ptr collector_info_; +}; + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp new file mode 100644 index 0000000000000..c314b24a07921 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/collector_matching_strategy.hpp @@ -0,0 +1,81 @@ +// 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. + +#pragma once + +#include "cloud_collector.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +struct MatchingParams +{ + std::string topic_name; + double cloud_timestamp; + double cloud_arrival_time; +}; + +class CollectorMatchingStrategy +{ +public: + virtual ~CollectorMatchingStrategy() = default; + + [[nodiscard]] virtual std::optional> match_cloud_to_collector( + const std::list> & cloud_collectors, + const MatchingParams & params) const = 0; + virtual void set_collector_info( + std::shared_ptr & collector, const MatchingParams & matching_params) = 0; +}; + +class NaiveMatchingStrategy : public CollectorMatchingStrategy +{ +public: + explicit NaiveMatchingStrategy(rclcpp::Node & node); + [[nodiscard]] std::optional> match_cloud_to_collector( + const std::list> & cloud_collectors, + const MatchingParams & params) const override; + void set_collector_info( + std::shared_ptr & collector, const MatchingParams & matching_params) override; +}; + +class AdvancedMatchingStrategy : public CollectorMatchingStrategy +{ +public: + explicit AdvancedMatchingStrategy(rclcpp::Node & node, std::vector input_topics); + + [[nodiscard]] std::optional> match_cloud_to_collector( + const std::list> & cloud_collectors, + const MatchingParams & params) const override; + void set_collector_info( + std::shared_ptr & collector, const MatchingParams & matching_params) override; + +private: + std::unordered_map topic_to_offset_map_; + std::unordered_map topic_to_noise_window_map_; +}; + +std::shared_ptr parse_matching_strategy(rclcpp::Node & node); + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp new file mode 100644 index 0000000000000..fa8c58aa74c11 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp @@ -0,0 +1,110 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include + +// ROS includes +#include "autoware/point_types/types.hpp" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ +using autoware::point_types::PointXYZIRC; +using point_cloud_msg_wrapper::PointCloud2Modifier; + +struct ConcatenatedCloudResult +{ + sensor_msgs::msg::PointCloud2::SharedPtr concatenate_cloud_ptr{nullptr}; + std::optional> + topic_to_transformed_cloud_map; + std::unordered_map topic_to_original_stamp_map; +}; + +class CombineCloudHandler +{ +private: + rclcpp::Node & node_; + + std::string output_frame_; + bool is_motion_compensated_; + bool publish_synchronized_pointcloud_; + bool keep_input_frame_in_synchronized_pointcloud_; + std::unique_ptr managed_tf_buffer_{nullptr}; + + std::deque twist_queue_; + + /// @brief RclcppTimeHash structure defines a custom hash function for the rclcpp::Time type by + /// using its nanoseconds representation as the hash value. + struct RclcppTimeHash + { + std::size_t operator()(const rclcpp::Time & t) const + { + return std::hash()(t.nanoseconds()); + } + }; + + static void convert_to_xyzirc_cloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_cloud, + sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud); + + void correct_pointcloud_motion( + const std::shared_ptr & transformed_cloud_ptr, + const std::vector & pc_stamps, + std::unordered_map & transform_memo, + std::shared_ptr transformed_delay_compensated_cloud_ptr); + +public: + CombineCloudHandler( + rclcpp::Node & node, std::string output_frame, bool is_motion_compensated, + bool publish_synchronized_pointcloud, bool keep_input_frame_in_synchronized_pointcloud, + bool has_static_tf_only); + void process_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg); + void process_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_msg); + + ConcatenatedCloudResult combine_pointclouds( + std::unordered_map & topic_to_cloud_map); + + Eigen::Matrix4f compute_transform_to_adjust_for_old_timestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); + + std::deque get_twist_queue(); +}; + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp new file mode 100644 index 0000000000000..654593e317ad9 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp @@ -0,0 +1,124 @@ +// 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. + +#pragma once + +#include +#include +#include +#include +#include +#include + +// ROS includes +#include "cloud_collector.hpp" +#include "collector_matching_strategy.hpp" +#include "combine_cloud_handler.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ +class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node +{ +public: + explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); + ~PointCloudConcatenateDataSynchronizerComponent() override = default; + void publish_clouds( + ConcatenatedCloudResult && concatenated_cloud_result, + std::shared_ptr collector_info); + void manage_collector_list(); + std::list> get_cloud_collectors(); + void add_cloud_collector(const std::shared_ptr & collector); + +private: + struct Parameters + { + bool use_naive_approach; + bool debug_mode; + bool has_static_tf_only; + double rosbag_length; + int maximum_queue_size; + double timeout_sec; + bool is_motion_compensated; + bool publish_synchronized_pointcloud; + bool keep_input_frame_in_synchronized_pointcloud; + bool publish_previous_but_late_pointcloud; + std::string synchronized_pointcloud_postfix; + std::string input_twist_topic_type; + std::vector input_topics; + std::string output_frame; + std::string matching_strategy; + } params_; + + double current_concatenate_cloud_timestamp_{0.0}; + double latest_concatenate_cloud_timestamp_{0.0}; + bool drop_previous_but_late_pointcloud_{false}; + bool publish_pointcloud_{false}; + bool is_concatenated_cloud_empty_{false}; + std::shared_ptr diagnostic_collector_info_; + std::unordered_map diagnostic_topic_to_original_stamp_map_; + + std::shared_ptr combine_cloud_handler_; + std::list> cloud_collectors_; + std::unique_ptr collector_matching_strategy_; + std::mutex cloud_collectors_mutex_; + + // default postfix name for synchronized pointcloud + static constexpr const char * default_sync_topic_postfix = "_synchronized"; + + // subscribers + std::vector::SharedPtr> pointcloud_subs_; + rclcpp::Subscription::SharedPtr twist_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + + // publishers + rclcpp::Publisher::SharedPtr concatenated_cloud_publisher_; + std::unordered_map::SharedPtr> + topic_to_transformed_cloud_publisher_map_; + std::unique_ptr debug_publisher_; + + std::unique_ptr> stop_watch_ptr_; + diagnostic_updater::Updater diagnostic_updater_{this}; + + void cloud_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name); + void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); + + static std::string format_timestamp(double timestamp); + void check_concat_status(diagnostic_updater::DiagnosticStatusWrapper & stat); + std::string replace_sync_topic_name_postfix( + const std::string & original_topic_name, const std::string & postfix); +}; + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp deleted file mode 100644 index 40adad3521f8d..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ /dev/null @@ -1,197 +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. - -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2009, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ - * - */ - -#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ -#define AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ - -#include -#include -#include -#include -#include -#include -#include - -// ROS includes -#include "autoware/point_types/types.hpp" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace autoware::pointcloud_preprocessor -{ -using autoware::point_types::PointXYZIRC; -using point_cloud_msg_wrapper::PointCloud2Modifier; - -/** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data - * synchronizer: it listens for a set of input PointCloud messages on the same topic, - * checks their timestamps, and concatenates their fields together into a single - * PointCloud output message. - * \author Radu Bogdan Rusu - */ -class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node -{ -public: - typedef sensor_msgs::msg::PointCloud2 PointCloud2; - - /** \brief constructor. */ - explicit PointCloudConcatenateDataSynchronizerComponent(const rclcpp::NodeOptions & node_options); - - /** \brief constructor. - * \param queue_size the maximum queue size - */ - PointCloudConcatenateDataSynchronizerComponent( - const rclcpp::NodeOptions & node_options, int queue_size); - - /** \brief Empty destructor. */ - virtual ~PointCloudConcatenateDataSynchronizerComponent() {} - -private: - /** \brief The output PointCloud publisher. */ - rclcpp::Publisher::SharedPtr pub_output_; - /** \brief Delay Compensated PointCloud publisher*/ - std::map::SharedPtr> - transformed_raw_pc_publisher_map_; - - /** \brief The maximum number of messages that we can store in the queue. */ - int maximum_queue_size_ = 3; - - double timeout_sec_ = 0.1; - - bool publish_synchronized_pointcloud_; - bool keep_input_frame_in_synchronized_pointcloud_; - std::string synchronized_pointcloud_postfix_; - - std::set not_subscribed_topic_names_; - - /** \brief A vector of subscriber. */ - std::vector::SharedPtr> filters_; - - rclcpp::Subscription::SharedPtr sub_twist_; - rclcpp::Subscription::SharedPtr sub_odom_; - - rclcpp::TimerBase::SharedPtr timer_; - diagnostic_updater::Updater updater_{this}; - - const std::string input_twist_topic_type_; - - /** \brief Output TF frame the concatenated points should be transformed to. */ - std::string output_frame_; - - /** \brief The flag to indicate if only static TF are used. */ - bool has_static_tf_only_; - - /** \brief Input point cloud topics. */ - // XmlRpc::XmlRpcValue input_topics_; - std::vector input_topics_; - - std::unique_ptr managed_tf_buffer_{nullptr}; - - std::deque twist_ptr_queue_; - - std::map cloud_stdmap_; - std::map cloud_stdmap_tmp_; - std::mutex mutex_; - - std::vector input_offset_; - std::map offset_map_; - - Eigen::Matrix4f computeTransformToAdjustForOldTimestamp( - const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp); - std::map combineClouds( - sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr); - void publish(); - - void convertToXYZIRCCloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, - sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); - void setPeriod(const int64_t new_period); - void cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, - const std::string & topic_name); - void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); - void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); - void timer_callback(); - - void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); - std::string replaceSyncTopicNamePostfix( - const std::string & original_topic_name, const std::string & postfix); - - /** \brief processing time publisher. **/ - std::unique_ptr> stop_watch_ptr_; - std::unique_ptr debug_publisher_; -}; - -} // namespace autoware::pointcloud_preprocessor - -// clang-format off -#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_AND_TIME_SYNC_NODELET_HPP_ // NOLINT -// clang-format on diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index 0e959eedae1b6..d57e44431a8d2 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -68,13 +68,13 @@ #include #include +#include +#include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp index ef33e88ef5c98..fa55222fb6530 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/dual_return_outlier_filter_node.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #if __has_include() #include @@ -54,7 +54,7 @@ class DualReturnOutlierFilterComponent : public autoware::pointcloud_preprocesso /** \brief Parameter service callback */ rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); image_transport::Publisher image_pub_; - rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; rclcpp::Publisher::SharedPtr noise_cloud_pub_; private: diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp index c898cbacf33ea..96bd2fb2411a0 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_node.hpp @@ -53,7 +53,7 @@ class RingOutlierFilterComponent : public autoware::pointcloud_preprocessor::Fil const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info) override; - rclcpp::Publisher::SharedPtr visibility_pub_; + rclcpp::Publisher::SharedPtr visibility_pub_; private: /** \brief publisher of excluded pointcloud for debug reason. **/ diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp index d36fcb36a7be1..f4921be66a7e2 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/time_synchronizer/time_synchronizer_node.hpp @@ -68,14 +68,14 @@ #include #include +#include +#include #include #include #include #include #include #include -#include -#include #include #include diff --git a/sensing/autoware_pointcloud_preprocessor/launch/concatenate_and_time_sync_node.launch.xml b/sensing/autoware_pointcloud_preprocessor/launch/concatenate_and_time_sync_node.launch.xml new file mode 100644 index 0000000000000..f553c15f01210 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/launch/concatenate_and_time_sync_node.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/sensing/autoware_pointcloud_preprocessor/package.xml b/sensing/autoware_pointcloud_preprocessor/package.xml index 4511c5497a3a0..ab345f03bd694 100644 --- a/sensing/autoware_pointcloud_preprocessor/package.xml +++ b/sensing/autoware_pointcloud_preprocessor/package.xml @@ -2,7 +2,7 @@ autoware_pointcloud_preprocessor - 0.40.0 + 0.41.0 The ROS 2 autoware_pointcloud_preprocessor package amc-nu Yukihiro Saito @@ -26,6 +26,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_lanelet2_extension autoware_pcl_extensions autoware_point_types @@ -51,7 +52,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_debug_msgs ament_lint_auto autoware_lint_common diff --git a/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json index 0e4a02d37bd16..cdca58a6a73f0 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/blockage_diag_node.schema.json @@ -99,7 +99,8 @@ "type": "number", "minimum": 0.0, "maximum": 360.0 - } + }, + "default": [0.0, 360.0] }, "vertical_bins": { "type": "integer", diff --git a/sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json new file mode 100644 index 0000000000000..c3758d05a8349 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/schema/concatenate_and_time_sync_node.schema.json @@ -0,0 +1,163 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Concatenate and Time Synchronize Node", + "type": "object", + "definitions": { + "concatenate_and_time_sync_node": { + "type": "object", + "properties": { + "debug_mode": { + "type": "boolean", + "default": false, + "description": "Flag to enables debug mode to display additional logging information." + }, + "has_static_tf_only": { + "type": "boolean", + "default": false, + "description": "Flag to indicate if only static TF is used." + }, + "rosbag_length": { + "type": "number", + "default": 10.0, + "minimum": 0.0, + "description": " This value determine if the rosbag has started from the beginning again. The value should be set smaller than the actual length of the bag." + }, + "maximum_queue_size": { + "type": "integer", + "default": "5", + "minimum": 1, + "description": "Maximum size of the queue for the Keep Last policy in QoS history." + }, + "timeout_sec": { + "type": "number", + "default": "0.1", + "minimum": 0.001, + "description": "Timer's timeout duration in seconds when collectors are created. Collectors will concatenate the point clouds when timeout_sec reaches zero." + }, + "is_motion_compensated": { + "type": "boolean", + "default": true, + "description": "Flag to indicate if motion compensation is enabled." + }, + "publish_synchronized_pointcloud": { + "type": "boolean", + "default": true, + "description": "Flag to indicate if synchronized point cloud should be published." + }, + "keep_input_frame_in_synchronized_pointcloud": { + "type": "boolean", + "default": true, + "description": "Flag to indicate if input frame should be kept in synchronized point cloud." + }, + "publish_previous_but_late_pointcloud": { + "type": "boolean", + "default": false, + "description": "Flag to indicate if a concatenated point cloud should be published if its timestamp is earlier than the previous published concatenated point cloud." + }, + "synchronized_pointcloud_postfix": { + "type": "string", + "default": "pointcloud", + "description": "Postfix for the topic name of the synchronized point cloud." + }, + "input_twist_topic_type": { + "type": "string", + "enum": ["twist", "odom"], + "default": "twist", + "description": "Type of the input twist topic." + }, + "input_topics": { + "type": "array", + "items": { + "type": "string", + "minLength": 1 + }, + "default": ["cloud_topic1", "cloud_topic2", "cloud_topic3"], + "minItems": 2, + "description": "List of input point cloud topics." + }, + "output_frame": { + "type": "string", + "default": "base_link", + "minLength": 1, + "description": "Output frame." + }, + "matching_strategy": { + "type": "object", + "properties": { + "type": { + "type": "string", + "enum": ["naive", "advanced"], + "default": "advanced", + "description": "Set it to `advanced` if you can synchronize your LiDAR sensor; otherwise, set it to `naive`." + }, + "lidar_timestamp_offsets": { + "type": "array", + "items": { + "type": "number", + "minimum": 0.0 + }, + "default": [0.0, 0.0, 0.0], + "minItems": 2, + "description": "List of LiDAR timestamp offsets in seconds (relative to the earliest LiDAR timestamp). The offsets should be provided in the same order as the input topics." + }, + "lidar_timestamp_noise_window": { + "type": "array", + "items": { + "type": "number", + "minimum": 0.0 + }, + "default": [0.01, 0.01, 0.01], + "minItems": 2, + "description": "List of LiDAR timestamp noise windows in seconds. The noise values should be specified in the same order as the input_topics." + } + }, + "required": ["type"], + "dependencies": { + "type": { + "oneOf": [ + { + "properties": { "type": { "const": "naive" } }, + "not": { + "required": ["lidar_timestamp_offsets", "lidar_timestamp_noise_window"] + } + }, + { + "properties": { "type": { "const": "advanced" } }, + "required": ["lidar_timestamp_offsets", "lidar_timestamp_noise_window"] + } + ] + } + } + } + }, + "required": [ + "debug_mode", + "has_static_tf_only", + "rosbag_length", + "maximum_queue_size", + "timeout_sec", + "is_motion_compensated", + "publish_synchronized_pointcloud", + "keep_input_frame_in_synchronized_pointcloud", + "publish_previous_but_late_pointcloud", + "synchronized_pointcloud_postfix", + "input_twist_topic_type", + "input_topics", + "output_frame", + "matching_strategy" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/concatenate_and_time_sync_node" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json index 091695716c610..c59541d1d2fba 100644 --- a/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json +++ b/sensing/autoware_pointcloud_preprocessor/schema/distortion_corrector_node.schema.json @@ -29,7 +29,7 @@ "has_static_tf_only": { "type": "boolean", "description": "Flag to indicate if only static TF is used.", - "default": false + "default": "false" } }, "required": [ diff --git a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp index 5021a3551c939..e05d5f789f680 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/blockage_diag/blockage_diag_node.cpp @@ -68,7 +68,7 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options std::string(this->get_namespace()) + ": dust_validation", this, &BlockageDiagComponent::dustChecker); - ground_dust_ratio_pub_ = create_publisher( + ground_dust_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_dust_ratio", rclcpp::SensorDataQoS()); if (publish_debug_image_) { single_frame_dust_mask_pub = @@ -86,9 +86,9 @@ BlockageDiagComponent::BlockageDiagComponent(const rclcpp::NodeOptions & options blockage_mask_pub_ = image_transport::create_publisher(this, "blockage_diag/debug/blockage_mask_image"); } - ground_blockage_ratio_pub_ = create_publisher( + ground_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/ground_blockage_ratio", rclcpp::SensorDataQoS()); - sky_blockage_ratio_pub_ = create_publisher( + sky_blockage_ratio_pub_ = create_publisher( "blockage_diag/debug/sky_blockage_ratio", rclcpp::SensorDataQoS()); using std::placeholders::_1; set_param_res_ = this->add_on_set_parameters_callback( @@ -315,7 +315,7 @@ void BlockageDiagComponent::filter( cv::Mat ground_mask(cv::Size(ideal_horizontal_bins, horizontal_ring_id_), CV_8UC1); cv::vconcat(sky_blank, single_dust_ground_img, single_dust_img); - tier4_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped ground_dust_ratio_msg; ground_dust_ratio_ = static_cast(cv::countNonZero(single_dust_ground_img)) / (single_dust_ground_img.cols * single_dust_ground_img.rows); ground_dust_ratio_msg.data = ground_dust_ratio_; @@ -386,12 +386,12 @@ void BlockageDiagComponent::filter( } } - tier4_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped ground_blockage_ratio_msg; ground_blockage_ratio_msg.data = ground_blockage_ratio_; ground_blockage_ratio_msg.stamp = now(); ground_blockage_ratio_pub_->publish(ground_blockage_ratio_msg); - tier4_debug_msgs::msg::Float32Stamped sky_blockage_ratio_msg; + autoware_internal_debug_msgs::msg::Float32Stamped sky_blockage_ratio_msg; sky_blockage_ratio_msg.data = sky_blockage_ratio_; sky_blockage_ratio_msg.stamp = now(); sky_blockage_ratio_pub_->publish(sky_blockage_ratio_msg); diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp new file mode 100644 index 0000000000000..63ee23d204788 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/cloud_collector.cpp @@ -0,0 +1,156 @@ +// 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/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp" + +#include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +CloudCollector::CloudCollector( + std::shared_ptr && ros2_parent_node, + std::shared_ptr & combine_cloud_handler, int num_of_clouds, + double timeout_sec, bool debug_mode) +: ros2_parent_node_(std::move(ros2_parent_node)), + combine_cloud_handler_(combine_cloud_handler), + num_of_clouds_(num_of_clouds), + timeout_sec_(timeout_sec), + debug_mode_(debug_mode) +{ + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(timeout_sec_)); + + timer_ = + rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() { + std::lock_guard concatenate_lock(concatenate_mutex_); + if (concatenate_finished_) return; + concatenate_callback(); + }); +} + +void CloudCollector::set_info(std::shared_ptr collector_info) +{ + collector_info_ = std::move(collector_info); +} + +std::shared_ptr CloudCollector::get_info() const +{ + return collector_info_; +} + +bool CloudCollector::topic_exists(const std::string & topic_name) +{ + return topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end(); +} + +bool CloudCollector::process_pointcloud( + const std::string & topic_name, sensor_msgs::msg::PointCloud2::SharedPtr cloud) +{ + std::lock_guard concatenate_lock(concatenate_mutex_); + if (concatenate_finished_) return false; + + // Check if the map already contains an entry for the same topic. This shouldn't happen if the + // parameter 'lidar_timestamp_noise_window' is set correctly. + if (topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end()) { + RCLCPP_WARN_STREAM_THROTTLE( + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), + std::chrono::milliseconds(10000).count(), + "Topic '" << topic_name + << "' already exists in the collector. Check the timestamp of the pointcloud."); + } + topic_to_cloud_map_[topic_name] = cloud; + if (topic_to_cloud_map_.size() == num_of_clouds_) { + concatenate_callback(); + } + + return true; +} + +bool CloudCollector::concatenate_finished() const +{ + return concatenate_finished_; +} + +void CloudCollector::concatenate_callback() +{ + if (debug_mode_) { + show_debug_message(); + } + + // All pointclouds are received or the timer has timed out, cancel the timer and concatenate the + // pointclouds in the collector. + timer_->cancel(); + + auto concatenated_cloud_result = concatenate_pointclouds(topic_to_cloud_map_); + + ros2_parent_node_->publish_clouds(std::move(concatenated_cloud_result), collector_info_); + + concatenate_finished_ = true; +} + +ConcatenatedCloudResult CloudCollector::concatenate_pointclouds( + std::unordered_map topic_to_cloud_map) +{ + return combine_cloud_handler_->combine_pointclouds(topic_to_cloud_map); +} + +std::unordered_map +CloudCollector::get_topic_to_cloud_map() +{ + return topic_to_cloud_map_; +} + +void CloudCollector::show_debug_message() +{ + auto time_until_trigger = timer_->time_until_trigger(); + std::stringstream log_stream; + log_stream << std::fixed << std::setprecision(6); + log_stream << "Collector's concatenate callback time: " + << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; + + if (auto advanced_info = std::dynamic_pointer_cast(collector_info_)) { + log_stream << "Advanced strategy:\n Collector's reference time min: " + << advanced_info->timestamp - advanced_info->noise_window + << " to max: " << advanced_info->timestamp + advanced_info->noise_window + << " seconds\n"; + } else if (auto naive_info = std::dynamic_pointer_cast(collector_info_)) { + log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp + << " seconds\n"; + } + + log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; + + log_stream << "Pointclouds: ["; + std::string separator = ""; + for (const auto & [topic, cloud] : topic_to_cloud_map_) { + log_stream << separator; + log_stream << "[" << topic << ", " << rclcpp::Time(cloud->header.stamp).seconds() << "]"; + separator = ", "; + } + + log_stream << "]\n"; + + RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); +} + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp new file mode 100644 index 0000000000000..50058df3d91af --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/collector_matching_strategy.cpp @@ -0,0 +1,121 @@ +// 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/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +NaiveMatchingStrategy::NaiveMatchingStrategy(rclcpp::Node & node) +{ + RCLCPP_INFO(node.get_logger(), "Utilize naive matching strategy"); +} + +std::optional> NaiveMatchingStrategy::match_cloud_to_collector( + const std::list> & cloud_collectors, + const MatchingParams & params) const +{ + std::optional smallest_time_difference = std::nullopt; + std::shared_ptr closest_collector = nullptr; + + for (const auto & cloud_collector : cloud_collectors) { + if (!cloud_collector->topic_exists(params.topic_name)) { + auto info = cloud_collector->get_info(); + if (auto naive_info = std::dynamic_pointer_cast(info)) { + double time_difference = std::abs(params.cloud_arrival_time - naive_info->timestamp); + if (!smallest_time_difference || time_difference < smallest_time_difference) { + smallest_time_difference = time_difference; + closest_collector = cloud_collector; + } + } + } + } + + if (closest_collector != nullptr) { + return closest_collector; + } + return std::nullopt; +} + +void NaiveMatchingStrategy::set_collector_info( + std::shared_ptr & collector, const MatchingParams & matching_params) +{ + auto info = std::make_shared(matching_params.cloud_arrival_time); + collector->set_info(info); +} + +AdvancedMatchingStrategy::AdvancedMatchingStrategy( + rclcpp::Node & node, std::vector input_topics) +{ + auto lidar_timestamp_offsets = + node.declare_parameter>("matching_strategy.lidar_timestamp_offsets"); + auto lidar_timestamp_noise_window = + node.declare_parameter>("matching_strategy.lidar_timestamp_noise_window"); + + if (lidar_timestamp_offsets.size() != input_topics.size()) { + throw std::runtime_error( + "The number of topics does not match the number of timestamp offsets."); + } + if (lidar_timestamp_noise_window.size() != input_topics.size()) { + throw std::runtime_error( + "The number of topics does not match the number of timestamp noise window."); + } + + for (size_t i = 0; i < input_topics.size(); i++) { + topic_to_offset_map_[input_topics[i]] = lidar_timestamp_offsets[i]; + topic_to_noise_window_map_[input_topics[i]] = lidar_timestamp_noise_window[i]; + } + + RCLCPP_INFO(node.get_logger(), "Utilize advanced matching strategy"); +} + +std::optional> AdvancedMatchingStrategy::match_cloud_to_collector( + const std::list> & cloud_collectors, + const MatchingParams & params) const +{ + for (const auto & cloud_collector : cloud_collectors) { + auto info = cloud_collector->get_info(); + if (auto advanced_info = std::dynamic_pointer_cast(info)) { + auto reference_timestamp_min = advanced_info->timestamp - advanced_info->noise_window; + auto reference_timestamp_max = advanced_info->timestamp + advanced_info->noise_window; + double time = params.cloud_timestamp - topic_to_offset_map_.at(params.topic_name); + if ( + time < reference_timestamp_max + topic_to_noise_window_map_.at(params.topic_name) && + time > reference_timestamp_min - topic_to_noise_window_map_.at(params.topic_name)) { + return cloud_collector; + } + } + } + return std::nullopt; +} + +void AdvancedMatchingStrategy::set_collector_info( + std::shared_ptr & collector, const MatchingParams & matching_params) +{ + auto info = std::make_shared( + matching_params.cloud_timestamp - topic_to_offset_map_.at(matching_params.topic_name), + topic_to_noise_window_map_[matching_params.topic_name]); + collector->set_info(info); +} + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp new file mode 100644 index 0000000000000..d68218314830b --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/combine_cloud_handler.cpp @@ -0,0 +1,336 @@ +// 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/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +CombineCloudHandler::CombineCloudHandler( + rclcpp::Node & node, std::string output_frame, bool is_motion_compensated, + bool publish_synchronized_pointcloud, bool keep_input_frame_in_synchronized_pointcloud, + bool has_static_tf_only) +: node_(node), + output_frame_(std::move(output_frame)), + is_motion_compensated_(is_motion_compensated), + publish_synchronized_pointcloud_(publish_synchronized_pointcloud), + keep_input_frame_in_synchronized_pointcloud_(keep_input_frame_in_synchronized_pointcloud), + managed_tf_buffer_( + std::make_unique(&node_, has_static_tf_only)) +{ +} + +void CombineCloudHandler::process_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr & twist_msg) +{ + geometry_msgs::msg::TwistStamped msg; + msg.header = twist_msg->header; + msg.twist = twist_msg->twist.twist; + + // If time jumps backwards (e.g. when a rosbag restarts), clear buffer + if (!twist_queue_.empty()) { + if (rclcpp::Time(twist_queue_.front().header.stamp) > rclcpp::Time(msg.header.stamp)) { + twist_queue_.clear(); + } + } + + // Twist data in the queue that is older than the current twist by 1 second will be cleared. + auto cutoff_time = rclcpp::Time(msg.header.stamp) - rclcpp::Duration::from_seconds(1.0); + + while (!twist_queue_.empty()) { + if (rclcpp::Time(twist_queue_.front().header.stamp) > cutoff_time) { + break; + } + twist_queue_.pop_front(); + } + + twist_queue_.push_back(msg); +} + +void CombineCloudHandler::process_odometry( + const nav_msgs::msg::Odometry::ConstSharedPtr & odometry_msg) +{ + geometry_msgs::msg::TwistStamped msg; + msg.header = odometry_msg->header; + msg.twist = odometry_msg->twist.twist; + + // If time jumps backwards (e.g. when a rosbag restarts), clear buffer + if (!twist_queue_.empty()) { + if (rclcpp::Time(twist_queue_.front().header.stamp) > rclcpp::Time(msg.header.stamp)) { + twist_queue_.clear(); + } + } + + // Twist data in the queue that is older than the current twist by 1 second will be cleared. + auto cutoff_time = rclcpp::Time(msg.header.stamp) - rclcpp::Duration::from_seconds(1.0); + + while (!twist_queue_.empty()) { + if (rclcpp::Time(twist_queue_.front().header.stamp) > cutoff_time) { + break; + } + twist_queue_.pop_front(); + } + + twist_queue_.push_back(msg); +} + +std::deque CombineCloudHandler::get_twist_queue() +{ + return twist_queue_; +} + +void CombineCloudHandler::convert_to_xyzirc_cloud( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_cloud, + sensor_msgs::msg::PointCloud2::SharedPtr & xyzirc_cloud) +{ + xyzirc_cloud->header = input_cloud->header; + + PointCloud2Modifier output_modifier{ + *xyzirc_cloud, input_cloud->header.frame_id}; + output_modifier.reserve(input_cloud->width); + + bool has_valid_intensity = + std::any_of(input_cloud->fields.begin(), input_cloud->fields.end(), [](const auto & field) { + return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_return_type = + std::any_of(input_cloud->fields.begin(), input_cloud->fields.end(), [](const auto & field) { + return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; + }); + + bool has_valid_channel = + std::any_of(input_cloud->fields.begin(), input_cloud->fields.end(), [](const auto & field) { + return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; + }); + + sensor_msgs::PointCloud2Iterator it_x(*input_cloud, "x"); + sensor_msgs::PointCloud2Iterator it_y(*input_cloud, "y"); + sensor_msgs::PointCloud2Iterator it_z(*input_cloud, "z"); + + if (has_valid_intensity && has_valid_return_type && has_valid_channel) { + sensor_msgs::PointCloud2Iterator it_i(*input_cloud, "intensity"); + sensor_msgs::PointCloud2Iterator it_r(*input_cloud, "return_type"); + sensor_msgs::PointCloud2Iterator it_c(*input_cloud, "channel"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { + PointXYZIRC point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = *it_i; + point.return_type = *it_r; + point.channel = *it_c; + output_modifier.push_back(std::move(point)); + } + } else { + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { + PointXYZIRC point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + output_modifier.push_back(std::move(point)); + } + } +} + +void CombineCloudHandler::correct_pointcloud_motion( + const std::shared_ptr & transformed_cloud_ptr, + const std::vector & pc_stamps, + std::unordered_map & transform_memo, + std::shared_ptr transformed_delay_compensated_cloud_ptr) +{ + Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); + rclcpp::Time current_cloud_stamp = rclcpp::Time(transformed_cloud_ptr->header.stamp); + for (const auto & stamp : pc_stamps) { + if (stamp >= current_cloud_stamp) continue; + + Eigen::Matrix4f new_to_old_transform; + if (transform_memo.find(stamp) != transform_memo.end()) { + new_to_old_transform = transform_memo[stamp]; + } else { + new_to_old_transform = + compute_transform_to_adjust_for_old_timestamp(stamp, current_cloud_stamp); + transform_memo[stamp] = new_to_old_transform; + } + adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform; + current_cloud_stamp = stamp; + } + pcl_ros::transformPointCloud( + adjust_to_old_data_transform, *transformed_cloud_ptr, *transformed_delay_compensated_cloud_ptr); +} + +ConcatenatedCloudResult CombineCloudHandler::combine_pointclouds( + std::unordered_map & topic_to_cloud_map) +{ + ConcatenatedCloudResult concatenate_cloud_result; + + std::vector pc_stamps; + for (const auto & [topic, cloud] : topic_to_cloud_map) { + pc_stamps.emplace_back(cloud->header.stamp); + } + std::sort(pc_stamps.begin(), pc_stamps.end(), std::greater()); + const auto oldest_stamp = pc_stamps.back(); + + std::unordered_map transform_memo; + + // Before combining the pointclouds, initialize and reserve space for the concatenated pointcloud + concatenate_cloud_result.concatenate_cloud_ptr = + std::make_shared(); + + // Reserve space based on the total size of the pointcloud data to speed up the concatenation + // process + size_t total_data_size = 0; + for (const auto & [topic, cloud] : topic_to_cloud_map) { + total_data_size += cloud->data.size(); + } + concatenate_cloud_result.concatenate_cloud_ptr->data.reserve(total_data_size); + + for (const auto & [topic, cloud] : topic_to_cloud_map) { + // convert to XYZIRC pointcloud if pointcloud is not empty + auto xyzirc_cloud = std::make_shared(); + convert_to_xyzirc_cloud(cloud, xyzirc_cloud); + + auto transformed_cloud_ptr = std::make_shared(); + managed_tf_buffer_->transformPointcloud(output_frame_, *xyzirc_cloud, *transformed_cloud_ptr); + + concatenate_cloud_result.topic_to_original_stamp_map[topic] = + rclcpp::Time(cloud->header.stamp).seconds(); + + // compensate pointcloud + std::shared_ptr transformed_delay_compensated_cloud_ptr; + if (is_motion_compensated_) { + transformed_delay_compensated_cloud_ptr = std::make_shared(); + correct_pointcloud_motion( + transformed_cloud_ptr, pc_stamps, transform_memo, transformed_delay_compensated_cloud_ptr); + } else { + transformed_delay_compensated_cloud_ptr = transformed_cloud_ptr; + } + + pcl::concatenatePointCloud( + *concatenate_cloud_result.concatenate_cloud_ptr, *transformed_delay_compensated_cloud_ptr, + *concatenate_cloud_result.concatenate_cloud_ptr); + + if (publish_synchronized_pointcloud_) { + if (!concatenate_cloud_result.topic_to_transformed_cloud_map) { + // Initialize the map if it is not present + concatenate_cloud_result.topic_to_transformed_cloud_map = + std::unordered_map(); + } + // convert to original sensor frame if necessary + bool need_transform_to_sensor_frame = (cloud->header.frame_id != output_frame_); + if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { + auto transformed_cloud_ptr_in_sensor_frame = + std::make_shared(); + managed_tf_buffer_->transformPointcloud( + cloud->header.frame_id, *transformed_delay_compensated_cloud_ptr, + *transformed_cloud_ptr_in_sensor_frame); + transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp; + transformed_cloud_ptr_in_sensor_frame->header.frame_id = cloud->header.frame_id; + + (*concatenate_cloud_result.topic_to_transformed_cloud_map)[topic] = + transformed_cloud_ptr_in_sensor_frame; + } else { + transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; + transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; + (*concatenate_cloud_result.topic_to_transformed_cloud_map)[topic] = + transformed_delay_compensated_cloud_ptr; + } + } + } + concatenate_cloud_result.concatenate_cloud_ptr->header.stamp = oldest_stamp; + + return concatenate_cloud_result; +} + +Eigen::Matrix4f CombineCloudHandler::compute_transform_to_adjust_for_old_timestamp( + const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) +{ + // return identity if no twist is available + if (twist_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), std::chrono::milliseconds(10000).count(), + "No twist is available. Please confirm twist topic and timestamp. Leaving point cloud " + "untransformed."); + return Eigen::Matrix4f::Identity(); + } + + auto old_twist_it = std::lower_bound( + std::begin(twist_queue_), std::end(twist_queue_), old_stamp, + [](const geometry_msgs::msg::TwistStamped & x, const rclcpp::Time & t) { + return rclcpp::Time(x.header.stamp) < t; + }); + old_twist_it = old_twist_it == twist_queue_.end() ? (twist_queue_.end() - 1) : old_twist_it; + + auto new_twist_it = std::lower_bound( + std::begin(twist_queue_), std::end(twist_queue_), new_stamp, + [](const geometry_msgs::msg::TwistStamped & x, const rclcpp::Time & t) { + return rclcpp::Time(x.header.stamp) < t; + }); + new_twist_it = new_twist_it == twist_queue_.end() ? (twist_queue_.end() - 1) : new_twist_it; + + auto prev_time = old_stamp; + double x = 0.0; + double y = 0.0; + double yaw = 0.0; + for (auto twist_it = old_twist_it; twist_it != new_twist_it + 1; ++twist_it) { + const double dt = + (twist_it != new_twist_it) + ? (rclcpp::Time((*twist_it).header.stamp) - rclcpp::Time(prev_time)).seconds() + : (rclcpp::Time(new_stamp) - rclcpp::Time(prev_time)).seconds(); + + if (std::fabs(dt) > 0.1) { + RCLCPP_WARN_STREAM_THROTTLE( + node_.get_logger(), *node_.get_clock(), std::chrono::milliseconds(10000).count(), + "Time difference is too large. Cloud not interpolate. Please confirm twist topic and " + "timestamp"); + break; + } + + const double distance = (*twist_it).twist.linear.x * dt; + yaw += (*twist_it).twist.angular.z * dt; + x += distance * std::cos(yaw); + y += distance * std::sin(yaw); + prev_time = (*twist_it).header.stamp; + } + + Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity(); + + float cos_yaw = std::cos(yaw); + float sin_yaw = std::sin(yaw); + + transformation_matrix(0, 3) = x; + transformation_matrix(1, 3) = y; + transformation_matrix(0, 0) = cos_yaw; + transformation_matrix(0, 1) = -sin_yaw; + transformation_matrix(1, 0) = sin_yaw; + transformation_matrix(1, 1) = cos_yaw; + + return transformation_matrix; +} + +} // namespace autoware::pointcloud_preprocessor diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp new file mode 100644 index 0000000000000..e168781c647ed --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_node.cpp @@ -0,0 +1,459 @@ +// 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/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp" + +#include "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp" +#include "autoware/pointcloud_preprocessor/utility/memory.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace autoware::pointcloud_preprocessor +{ + +PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( + const rclcpp::NodeOptions & node_options) +: Node("point_cloud_concatenator_component", node_options) +{ + // initialize debug tool + using autoware::universe_utils::DebugPublisher; + using autoware::universe_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ = std::make_unique(this, "concatenate_data_synchronizer"); + stop_watch_ptr_->tic("cyclic_time"); + stop_watch_ptr_->tic("processing_time"); + + // initialize parameters + params_.debug_mode = declare_parameter("debug_mode"); + params_.has_static_tf_only = declare_parameter("has_static_tf_only"); + params_.rosbag_length = declare_parameter("rosbag_length"); + params_.maximum_queue_size = static_cast(declare_parameter("maximum_queue_size")); + params_.timeout_sec = declare_parameter("timeout_sec"); + params_.is_motion_compensated = declare_parameter("is_motion_compensated"); + params_.publish_synchronized_pointcloud = + declare_parameter("publish_synchronized_pointcloud"); + params_.keep_input_frame_in_synchronized_pointcloud = + declare_parameter("keep_input_frame_in_synchronized_pointcloud"); + params_.publish_previous_but_late_pointcloud = + declare_parameter("publish_previous_but_late_pointcloud"); + params_.synchronized_pointcloud_postfix = + declare_parameter("synchronized_pointcloud_postfix"); + params_.input_twist_topic_type = declare_parameter("input_twist_topic_type"); + params_.input_topics = declare_parameter>("input_topics"); + params_.output_frame = declare_parameter("output_frame"); + + if (params_.input_topics.empty()) { + throw std::runtime_error("Need a 'input_topics' parameter to be set before continuing."); + } + if (params_.input_topics.size() == 1) { + throw std::runtime_error("Only one topic given. Need at least two topics to continue."); + } + + if (params_.output_frame.empty()) { + throw std::runtime_error("Need an 'output_frame' parameter to be set before continuing."); + } + + params_.matching_strategy = declare_parameter("matching_strategy.type"); + if (params_.matching_strategy == "naive") { + collector_matching_strategy_ = std::make_unique(*this); + } else if (params_.matching_strategy == "advanced") { + collector_matching_strategy_ = + std::make_unique(*this, params_.input_topics); + } else { + throw std::runtime_error("Matching strategy must be 'advanced' or 'naive'"); + } + + // Publishers + concatenated_cloud_publisher_ = this->create_publisher( + "output", rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size)); + + // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud + if (params_.publish_synchronized_pointcloud) { + for (auto & topic : params_.input_topics) { + std::string new_topic = + replace_sync_topic_name_postfix(topic, params_.synchronized_pointcloud_postfix); + auto publisher = this->create_publisher( + new_topic, rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size)); + topic_to_transformed_cloud_publisher_map_.insert({topic, publisher}); + } + } + + // Subscribers + if (params_.is_motion_compensated) { + if (params_.input_twist_topic_type == "twist") { + twist_sub_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, + std::bind( + &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, + std::placeholders::_1)); + } else if (params_.input_twist_topic_type == "odom") { + odom_sub_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, + std::bind( + &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, + std::placeholders::_1)); + } else { + throw std::runtime_error( + "input_twist_topic_type is invalid: " + params_.input_twist_topic_type); + } + } + + for (const std::string & topic : params_.input_topics) { + std::function callback = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this, std::placeholders::_1, + topic); + + auto pointcloud_sub = this->create_subscription( + topic, rclcpp::SensorDataQoS().keep_last(params_.maximum_queue_size), callback); + pointcloud_subs_.push_back(pointcloud_sub); + } + RCLCPP_DEBUG_STREAM( + get_logger(), + "Subscribing to " << params_.input_topics.size() << " user given topics as inputs:"); + for (const auto & input_topic : params_.input_topics) { + RCLCPP_DEBUG_STREAM(get_logger(), " - " << input_topic); + } + + // Combine cloud handler + combine_cloud_handler_ = std::make_shared( + *this, params_.output_frame, params_.is_motion_compensated, + params_.publish_synchronized_pointcloud, params_.keep_input_frame_in_synchronized_pointcloud, + params_.has_static_tf_only); + + // Diagnostic Updater + diagnostic_updater_.setHardwareID("concatenate_data_checker"); + diagnostic_updater_.add( + "concat_status", this, &PointCloudConcatenateDataSynchronizerComponent::check_concat_status); +} + +std::string PointCloudConcatenateDataSynchronizerComponent::replace_sync_topic_name_postfix( + const std::string & original_topic_name, const std::string & postfix) +{ + std::string replaced_topic_name; + // separate the topic name by '/' and replace the last element with the new postfix + size_t pos = original_topic_name.find_last_of('/'); + if (pos == std::string::npos) { + // not found '/': this is not a namespaced topic + RCLCPP_WARN_STREAM( + get_logger(), + "The topic name is not namespaced. The postfix will be added to the end of the topic name."); + return original_topic_name + postfix; + } + + // replace the last element with the new postfix + replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix; + + // if topic name is the same with original topic name, add postfix to the end of the topic name + if (replaced_topic_name == original_topic_name) { + RCLCPP_WARN_STREAM( + get_logger(), "The topic name " + << original_topic_name + << " have the same postfix with synchronized pointcloud. We use " + "the postfix " + "to the end of the topic name."); + replaced_topic_name = original_topic_name + default_sync_topic_postfix; + } + return replaced_topic_name; +} + +void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name) +{ + stop_watch_ptr_->toc("processing_time", true); + double cloud_arrival_time = this->get_clock()->now().seconds(); + manage_collector_list(); + + if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); + + if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { + RCLCPP_ERROR( + get_logger(), + "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); + } + + return; + } + + if (params_.debug_mode) { + RCLCPP_INFO( + this->get_logger(), " pointcloud %s timestamp: %lf arrive time: %lf seconds, latency: %lf", + topic_name.c_str(), rclcpp::Time(input_ptr->header.stamp).seconds(), cloud_arrival_time, + cloud_arrival_time - rclcpp::Time(input_ptr->header.stamp).seconds()); + } + + if (input_ptr->data.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + } + + // protect cloud collectors list + std::unique_lock cloud_collectors_lock(cloud_collectors_mutex_); + + // For each callback, check whether there is a exist collector that matches this cloud + std::optional> cloud_collector = std::nullopt; + MatchingParams matching_params; + matching_params.topic_name = topic_name; + matching_params.cloud_arrival_time = cloud_arrival_time; + matching_params.cloud_timestamp = rclcpp::Time(input_ptr->header.stamp).seconds(); + + if (!cloud_collectors_.empty()) { + cloud_collector = + collector_matching_strategy_->match_cloud_to_collector(cloud_collectors_, matching_params); + } + + bool process_success = false; + if (cloud_collector.has_value()) { + auto collector = cloud_collector.value(); + if (collector) { + cloud_collectors_lock.unlock(); + process_success = cloud_collector.value()->process_pointcloud(topic_name, input_ptr); + } + } + + if (!process_success) { + auto new_cloud_collector = std::make_shared( + std::dynamic_pointer_cast(shared_from_this()), + combine_cloud_handler_, params_.input_topics.size(), params_.timeout_sec, params_.debug_mode); + + cloud_collectors_.push_back(new_cloud_collector); + cloud_collectors_lock.unlock(); + + collector_matching_strategy_->set_collector_info(new_cloud_collector, matching_params); + (void)new_cloud_collector->process_pointcloud(topic_name, input_ptr); + } +} + +void PointCloudConcatenateDataSynchronizerComponent::twist_callback( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input) +{ + combine_cloud_handler_->process_twist(input); +} + +void PointCloudConcatenateDataSynchronizerComponent::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) +{ + combine_cloud_handler_->process_odometry(input); +} + +void PointCloudConcatenateDataSynchronizerComponent::publish_clouds( + ConcatenatedCloudResult && concatenated_cloud_result, + std::shared_ptr collector_info) +{ + // should never come to this state. + if (concatenated_cloud_result.concatenate_cloud_ptr == nullptr) { + RCLCPP_ERROR(this->get_logger(), "Concatenated cloud is a nullptr."); + return; + } + + if (concatenated_cloud_result.concatenate_cloud_ptr->data.empty()) { + RCLCPP_ERROR(this->get_logger(), "Concatenated cloud is an empty pointcloud."); + is_concatenated_cloud_empty_ = true; + } + + current_concatenate_cloud_timestamp_ = + rclcpp::Time(concatenated_cloud_result.concatenate_cloud_ptr->header.stamp).seconds(); + + if ( + current_concatenate_cloud_timestamp_ < latest_concatenate_cloud_timestamp_ && + !params_.publish_previous_but_late_pointcloud) { + // Publish the cloud if the rosbag replays in loop + if ( + latest_concatenate_cloud_timestamp_ - current_concatenate_cloud_timestamp_ > + params_.rosbag_length) { + publish_pointcloud_ = true; // Force publishing in this case + } else { + drop_previous_but_late_pointcloud_ = true; // Otherwise, drop the late pointcloud + } + } else { + // Publish pointcloud if timestamps are valid or the condition doesn't apply + publish_pointcloud_ = true; + } + + if (publish_pointcloud_) { + latest_concatenate_cloud_timestamp_ = current_concatenate_cloud_timestamp_; + auto concatenate_pointcloud_output = std::make_unique( + std::move(*concatenated_cloud_result.concatenate_cloud_ptr)); + concatenated_cloud_publisher_->publish(std::move(concatenate_pointcloud_output)); + + // publish transformed raw pointclouds + if ( + params_.publish_synchronized_pointcloud && + concatenated_cloud_result.topic_to_transformed_cloud_map) { + for (const auto & topic : params_.input_topics) { + // Get a reference to the internal map + if ( + (*concatenated_cloud_result.topic_to_transformed_cloud_map).find(topic) != + (*concatenated_cloud_result.topic_to_transformed_cloud_map).end()) { + auto transformed_cloud_output = std::make_unique( + *(*concatenated_cloud_result.topic_to_transformed_cloud_map).at(topic)); + topic_to_transformed_cloud_publisher_map_[topic]->publish( + std::move(transformed_cloud_output)); + } else { + RCLCPP_WARN( + this->get_logger(), + "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", topic.c_str()); + } + } + } + } + + diagnostic_collector_info_ = collector_info; + + diagnostic_topic_to_original_stamp_map_ = concatenated_cloud_result.topic_to_original_stamp_map; + diagnostic_updater_.force_update(); + + // add processing time for debug + if (debug_publisher_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); + debug_publisher_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_->publish( + "debug/processing_time_ms", processing_time_ms); + + for (const auto & [topic, stamp] : concatenated_cloud_result.topic_to_original_stamp_map) { + const auto pipeline_latency_ms = (this->get_clock()->now().seconds() - stamp) * 1000; + debug_publisher_->publish( + "debug" + topic + "/pipeline_latency_ms", pipeline_latency_ms); + } + } +} + +void PointCloudConcatenateDataSynchronizerComponent::manage_collector_list() +{ + std::lock_guard cloud_collectors_lock(cloud_collectors_mutex_); + + for (auto it = cloud_collectors_.begin(); it != cloud_collectors_.end();) { + if ((*it)->concatenate_finished()) { + it = cloud_collectors_.erase(it); // Erase and move the iterator to the next element + } else { + ++it; // Move to the next element + } + } +} + +std::string PointCloudConcatenateDataSynchronizerComponent::format_timestamp(double timestamp) +{ + std::ostringstream oss; + oss << std::fixed << std::setprecision(9) << timestamp; + return oss.str(); +} + +void PointCloudConcatenateDataSynchronizerComponent::check_concat_status( + diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (publish_pointcloud_ || drop_previous_but_late_pointcloud_) { + stat.add( + "concatenated_cloud_timestamp", format_timestamp(current_concatenate_cloud_timestamp_)); + + if ( + auto naive_info = std::dynamic_pointer_cast(diagnostic_collector_info_)) { + stat.add("first_cloud_arrival_timestamp", format_timestamp(naive_info->timestamp)); + } else if ( + auto advanced_info = + std::dynamic_pointer_cast(diagnostic_collector_info_)) { + stat.add( + "reference_timestamp_min", + format_timestamp(advanced_info->timestamp - advanced_info->noise_window)); + stat.add( + "reference_timestamp_max", + format_timestamp(advanced_info->timestamp + advanced_info->noise_window)); + } + + bool topic_miss = false; + + bool concatenation_success = true; + for (const auto & topic : params_.input_topics) { + bool input_cloud_concatenated = true; + if ( + diagnostic_topic_to_original_stamp_map_.find(topic) != + diagnostic_topic_to_original_stamp_map_.end()) { + stat.add( + topic + "/timestamp", format_timestamp(diagnostic_topic_to_original_stamp_map_[topic])); + } else { + topic_miss = true; + concatenation_success = false; + input_cloud_concatenated = false; + } + stat.add(topic + "/is_concatenated", input_cloud_concatenated); + } + + stat.add("cloud_concatenation_success", concatenation_success); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string message = "Concatenated pointcloud is published and include all topics"; + + if (drop_previous_but_late_pointcloud_) { + if (topic_miss) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = + "Concatenated pointcloud misses some topics and is not published because it arrived " + "too late"; + } else { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Concatenated pointcloud is not published as it is too late"; + } + } else { + if (is_concatenated_cloud_empty_) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Concatenated pointcloud is empty"; + } else if (topic_miss) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + message = "Concatenated pointcloud is published but misses some topics"; + } + } + + stat.summary(level, message); + + publish_pointcloud_ = false; + drop_previous_but_late_pointcloud_ = false; + is_concatenated_cloud_empty_ = false; + } else { + const int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + const std::string message = + "Concatenate node launch successfully, but waiting for input pointcloud"; + stat.summary(level, message); + } +} + +std::list> +PointCloudConcatenateDataSynchronizerComponent::get_cloud_collectors() +{ + return cloud_collectors_; +} + +void PointCloudConcatenateDataSynchronizerComponent::add_cloud_collector( + const std::shared_ptr & collector) +{ + cloud_collectors_.push_back(collector); +} + +} // namespace autoware::pointcloud_preprocessor + +#include +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent) diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp deleted file mode 100644 index f8baae3405873..0000000000000 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ /dev/null @@ -1,718 +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. - -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2009, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage, Inc. nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - * $Id: concatenate_data.cpp 35231 2011-01-14 05:33:20Z rusu $ - * - */ - -#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp" - -#include "autoware/pointcloud_preprocessor/utility/memory.hpp" - -#include - -#include - -#include -#include -#include -#include -#include -#include - -#define DEFAULT_SYNC_TOPIC_POSTFIX \ - "_synchronized" // default postfix name for synchronized pointcloud - -////////////////////////////////////////////////////////////////////////////////////////////// - -namespace autoware::pointcloud_preprocessor -{ -PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( - const rclcpp::NodeOptions & node_options) -: Node("point_cloud_concatenator_component", node_options), - input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) -{ - // initialize debug tool - { - using autoware::universe_utils::DebugPublisher; - using autoware::universe_utils::StopWatch; - stop_watch_ptr_ = std::make_unique>(); - debug_publisher_ = std::make_unique(this, "concatenate_data_synchronizer"); - stop_watch_ptr_->tic("cyclic_time"); - stop_watch_ptr_->tic("processing_time"); - } - - // Set parameters - { - output_frame_ = static_cast(declare_parameter("output_frame", "")); - if (output_frame_.empty()) { - RCLCPP_ERROR(get_logger(), "Need an 'output_frame' parameter to be set before continuing!"); - return; - } - has_static_tf_only_ = declare_parameter("has_static_tf_only", false); - declare_parameter("input_topics", std::vector()); - input_topics_ = get_parameter("input_topics").as_string_array(); - if (input_topics_.empty()) { - RCLCPP_ERROR(get_logger(), "Need a 'input_topics' parameter to be set before continuing!"); - return; - } - if (input_topics_.size() == 1) { - RCLCPP_ERROR(get_logger(), "Only one topic given. Need at least two topics to continue."); - return; - } - - // Optional parameters - maximum_queue_size_ = static_cast(declare_parameter("max_queue_size", 5)); - timeout_sec_ = static_cast(declare_parameter("timeout_sec", 0.1)); - - input_offset_ = declare_parameter("input_offset", std::vector{}); - if (!input_offset_.empty() && input_topics_.size() != input_offset_.size()) { - RCLCPP_ERROR(get_logger(), "The number of topics does not match the number of offsets."); - return; - } - - // Check if publish synchronized pointcloud - publish_synchronized_pointcloud_ = declare_parameter("publish_synchronized_pointcloud", true); - keep_input_frame_in_synchronized_pointcloud_ = - declare_parameter("keep_input_frame_in_synchronized_pointcloud", true); - synchronized_pointcloud_postfix_ = - declare_parameter("synchronized_pointcloud_postfix", "pointcloud"); - } - - // Initialize not_subscribed_topic_names_ - { - for (const std::string & e : input_topics_) { - not_subscribed_topic_names_.insert(e); - } - } - - // Initialize offset map - { - for (size_t i = 0; i < input_offset_.size(); ++i) { - offset_map_[input_topics_[i]] = input_offset_[i]; - } - } - - // tf2 listener - { - managed_tf_buffer_ = - std::make_unique(this, has_static_tf_only_); - } - - // Output Publishers - { - rclcpp::PublisherOptions pub_options; - pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - pub_output_ = this->create_publisher( - "output", rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options); - } - - // Subscribers - { - RCLCPP_DEBUG_STREAM( - get_logger(), "Subscribing to " << input_topics_.size() << " user given topics as inputs:"); - for (const auto & input_topic : input_topics_) { - RCLCPP_DEBUG_STREAM(get_logger(), " - " << input_topic); - } - - // Subscribe to the filters - filters_.resize(input_topics_.size()); - - // First input_topics_.size () filters are valid - for (size_t d = 0; d < input_topics_.size(); ++d) { - cloud_stdmap_.insert(std::make_pair(input_topics_[d], nullptr)); - cloud_stdmap_tmp_ = cloud_stdmap_; - - // CAN'T use auto type here. - std::function cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::cloud_callback, this, - std::placeholders::_1, input_topics_[d]); - - filters_[d].reset(); - filters_[d] = this->create_subscription( - input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); - } - - if (input_twist_topic_type_ == "twist") { - auto twist_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, - std::placeholders::_1); - sub_twist_ = this->create_subscription( - "~/input/twist", rclcpp::QoS{100}, twist_cb); - } else if (input_twist_topic_type_ == "odom") { - auto odom_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, - std::placeholders::_1); - sub_odom_ = this->create_subscription( - "~/input/odom", rclcpp::QoS{100}, odom_cb); - } else { - RCLCPP_ERROR_STREAM( - get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); - throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); - } - } - - // Transformed Raw PointCloud2 Publisher to publish the transformed pointcloud - if (publish_synchronized_pointcloud_) { - rclcpp::PublisherOptions pub_options; - pub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); - - for (auto & topic : input_topics_) { - std::string new_topic = replaceSyncTopicNamePostfix(topic, synchronized_pointcloud_postfix_); - auto publisher = this->create_publisher( - new_topic, rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), pub_options); - transformed_raw_pc_publisher_map_.insert({topic, publisher}); - } - } - - // Set timer - { - const auto period_ns = std::chrono::duration_cast( - std::chrono::duration(timeout_sec_)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, - std::bind(&PointCloudConcatenateDataSynchronizerComponent::timer_callback, this)); - } - - // Diagnostic Updater - { - updater_.setHardwareID("concatenate_data_checker"); - updater_.add( - "concat_status", this, &PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus); - } -} - -std::string PointCloudConcatenateDataSynchronizerComponent::replaceSyncTopicNamePostfix( - const std::string & original_topic_name, const std::string & postfix) -{ - std::string replaced_topic_name; - // separate the topic name by '/' and replace the last element with the new postfix - size_t pos = original_topic_name.find_last_of("/"); - if (pos == std::string::npos) { - // not found '/': this is not a namespaced topic - RCLCPP_WARN_STREAM( - get_logger(), - "The topic name is not namespaced. The postfix will be added to the end of the topic name."); - return original_topic_name + postfix; - } else { - // replace the last element with the new postfix - replaced_topic_name = original_topic_name.substr(0, pos) + "/" + postfix; - } - - // if topic name is the same with original topic name, add postfix to the end of the topic name - if (replaced_topic_name == original_topic_name) { - RCLCPP_WARN_STREAM( - get_logger(), "The topic name " - << original_topic_name - << " have the same postfix with synchronized pointcloud. We use the postfix " - "to the end of the topic name."); - replaced_topic_name = original_topic_name + DEFAULT_SYNC_TOPIC_POSTFIX; - } - return replaced_topic_name; -} - -/** - * @brief compute transform to adjust for old timestamp - * - * @param old_stamp - * @param new_stamp - * @return Eigen::Matrix4f: transformation matrix from new_stamp to old_stamp - */ -Eigen::Matrix4f -PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( - const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) -{ - // return identity if no twist is available - if (twist_ptr_queue_.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), - "No twist is available. Please confirm twist topic and timestamp"); - return Eigen::Matrix4f::Identity(); - } - - // return identity if old_stamp is newer than new_stamp - if (old_stamp > new_stamp) { - RCLCPP_DEBUG_STREAM_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), - "old_stamp is newer than new_stamp,"); - return Eigen::Matrix4f::Identity(); - } - - auto old_twist_ptr_it = std::lower_bound( - std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), old_stamp, - [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { - return rclcpp::Time(x_ptr->header.stamp) < t; - }); - old_twist_ptr_it = - old_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : old_twist_ptr_it; - - auto new_twist_ptr_it = std::lower_bound( - std::begin(twist_ptr_queue_), std::end(twist_ptr_queue_), new_stamp, - [](const geometry_msgs::msg::TwistStamped::ConstSharedPtr & x_ptr, const rclcpp::Time & t) { - return rclcpp::Time(x_ptr->header.stamp) < t; - }); - new_twist_ptr_it = - new_twist_ptr_it == twist_ptr_queue_.end() ? (twist_ptr_queue_.end() - 1) : new_twist_ptr_it; - - auto prev_time = old_stamp; - double x = 0.0; - double y = 0.0; - double yaw = 0.0; - for (auto twist_ptr_it = old_twist_ptr_it; twist_ptr_it != new_twist_ptr_it + 1; ++twist_ptr_it) { - const double dt = - (twist_ptr_it != new_twist_ptr_it) - ? (rclcpp::Time((*twist_ptr_it)->header.stamp) - rclcpp::Time(prev_time)).seconds() - : (rclcpp::Time(new_stamp) - rclcpp::Time(prev_time)).seconds(); - - if (std::fabs(dt) > 0.1) { - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), - "Time difference is too large. Cloud not interpolate. Please confirm twist topic and " - "timestamp"); - break; - } - - const double dis = (*twist_ptr_it)->twist.linear.x * dt; - yaw += (*twist_ptr_it)->twist.angular.z * dt; - x += dis * std::cos(yaw); - y += dis * std::sin(yaw); - prev_time = (*twist_ptr_it)->header.stamp; - } - Eigen::AngleAxisf rotation_x(0, Eigen::Vector3f::UnitX()); - Eigen::AngleAxisf rotation_y(0, Eigen::Vector3f::UnitY()); - Eigen::AngleAxisf rotation_z(yaw, Eigen::Vector3f::UnitZ()); - Eigen::Translation3f translation(x, y, 0); - Eigen::Matrix4f rotation_matrix = (translation * rotation_z * rotation_y * rotation_x).matrix(); - return rotation_matrix; -} - -std::map -PointCloudConcatenateDataSynchronizerComponent::combineClouds( - sensor_msgs::msg::PointCloud2::SharedPtr & concat_cloud_ptr) -{ - // map for storing the transformed point clouds - std::map transformed_clouds; - - // Step1. gather stamps and sort it - std::vector pc_stamps; - for (const auto & e : cloud_stdmap_) { - transformed_clouds[e.first] = nullptr; - if (e.second != nullptr) { - if (e.second->data.size() == 0) { - continue; - } - pc_stamps.push_back(rclcpp::Time(e.second->header.stamp)); - } - } - if (pc_stamps.empty()) { - return transformed_clouds; - } - // sort stamps and get oldest stamp - std::sort(pc_stamps.begin(), pc_stamps.end()); - std::reverse(pc_stamps.begin(), pc_stamps.end()); - const auto oldest_stamp = pc_stamps.back(); - - // Step2. Calculate compensation transform and concatenate with the oldest stamp - for (const auto & e : cloud_stdmap_) { - if (e.second != nullptr) { - if (e.second->data.size() == 0) { - continue; - } - sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr( - new sensor_msgs::msg::PointCloud2()); - managed_tf_buffer_->transformPointcloud(output_frame_, *e.second, *transformed_cloud_ptr); - - // calculate transforms to oldest stamp - Eigen::Matrix4f adjust_to_old_data_transform = Eigen::Matrix4f::Identity(); - rclcpp::Time transformed_stamp = rclcpp::Time(e.second->header.stamp); - for (const auto & stamp : pc_stamps) { - const auto new_to_old_transform = - computeTransformToAdjustForOldTimestamp(stamp, transformed_stamp); - adjust_to_old_data_transform = new_to_old_transform * adjust_to_old_data_transform; - transformed_stamp = std::min(transformed_stamp, stamp); - } - sensor_msgs::msg::PointCloud2::SharedPtr transformed_delay_compensated_cloud_ptr( - new sensor_msgs::msg::PointCloud2()); - pcl_ros::transformPointCloud( - adjust_to_old_data_transform, *transformed_cloud_ptr, - *transformed_delay_compensated_cloud_ptr); - - // concatenate - if (concat_cloud_ptr == nullptr) { - concat_cloud_ptr = - std::make_shared(*transformed_delay_compensated_cloud_ptr); - } else { - pcl::concatenatePointCloud( - *concat_cloud_ptr, *transformed_delay_compensated_cloud_ptr, *concat_cloud_ptr); - } - // convert to original sensor frame if necessary - bool need_transform_to_sensor_frame = (e.second->header.frame_id != output_frame_); - if (keep_input_frame_in_synchronized_pointcloud_ && need_transform_to_sensor_frame) { - sensor_msgs::msg::PointCloud2::SharedPtr transformed_cloud_ptr_in_sensor_frame( - new sensor_msgs::msg::PointCloud2()); - managed_tf_buffer_->transformPointcloud( - e.second->header.frame_id, *transformed_delay_compensated_cloud_ptr, - *transformed_cloud_ptr_in_sensor_frame); - transformed_cloud_ptr_in_sensor_frame->header.stamp = oldest_stamp; - transformed_cloud_ptr_in_sensor_frame->header.frame_id = e.second->header.frame_id; - transformed_clouds[e.first] = transformed_cloud_ptr_in_sensor_frame; - } else { - transformed_delay_compensated_cloud_ptr->header.stamp = oldest_stamp; - transformed_delay_compensated_cloud_ptr->header.frame_id = output_frame_; - transformed_clouds[e.first] = transformed_delay_compensated_cloud_ptr; - } - - } else { - not_subscribed_topic_names_.insert(e.first); - } - } - concat_cloud_ptr->header.stamp = oldest_stamp; - return transformed_clouds; -} - -void PointCloudConcatenateDataSynchronizerComponent::publish() -{ - stop_watch_ptr_->toc("processing_time", true); - sensor_msgs::msg::PointCloud2::SharedPtr concat_cloud_ptr = nullptr; - not_subscribed_topic_names_.clear(); - - const auto & transformed_raw_points = - PointCloudConcatenateDataSynchronizerComponent::combineClouds(concat_cloud_ptr); - - // publish concatenated pointcloud - if (concat_cloud_ptr) { - auto output = std::make_unique(*concat_cloud_ptr); - pub_output_->publish(std::move(output)); - } else { - RCLCPP_WARN(this->get_logger(), "concat_cloud_ptr is nullptr, skipping pointcloud publish."); - } - - // publish transformed raw pointclouds - if (publish_synchronized_pointcloud_) { - for (const auto & e : transformed_raw_points) { - if (e.second) { - auto output = std::make_unique(*e.second); - transformed_raw_pc_publisher_map_[e.first]->publish(std::move(output)); - } else { - RCLCPP_WARN( - this->get_logger(), "transformed_raw_points[%s] is nullptr, skipping pointcloud publish.", - e.first.c_str()); - } - } - } - - updater_.force_update(); - - cloud_stdmap_ = cloud_stdmap_tmp_; - std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { - e.second = nullptr; - }); - // add processing time for debug - if (debug_publisher_) { - const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); - const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( - "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( - "debug/processing_time_ms", processing_time_ms); - } - for (const auto & e : cloud_stdmap_) { - if (e.second != nullptr) { - if (debug_publisher_) { - const auto pipeline_latency_ms = - std::chrono::duration( - std::chrono::nanoseconds( - (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) - .count(); - debug_publisher_->publish( - "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); - } - } - } -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void PointCloudConcatenateDataSynchronizerComponent::convertToXYZIRCCloud( - const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, - sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) -{ - output_ptr->header = input_ptr->header; - - PointCloud2Modifier output_modifier{ - *output_ptr, input_ptr->header.frame_id}; - output_modifier.reserve(input_ptr->width); - - bool has_valid_intensity = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { - return field.name == "intensity" && field.datatype == sensor_msgs::msg::PointField::UINT8; - }); - - bool has_valid_return_type = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { - return field.name == "return_type" && field.datatype == sensor_msgs::msg::PointField::UINT8; - }); - - bool has_valid_channel = - std::any_of(input_ptr->fields.begin(), input_ptr->fields.end(), [](const auto & field) { - return field.name == "channel" && field.datatype == sensor_msgs::msg::PointField::UINT16; - }); - - sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); - sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); - sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); - - if (has_valid_intensity && has_valid_return_type && has_valid_channel) { - sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); - sensor_msgs::PointCloud2Iterator it_r(*input_ptr, "return_type"); - sensor_msgs::PointCloud2Iterator it_c(*input_ptr, "channel"); - - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i, ++it_r, ++it_c) { - PointXYZIRC point; - point.x = *it_x; - point.y = *it_y; - point.z = *it_z; - point.intensity = *it_i; - point.return_type = *it_r; - point.channel = *it_c; - output_modifier.push_back(std::move(point)); - } - } else { - for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { - PointXYZIRC point; - point.x = *it_x; - point.y = *it_y; - point.z = *it_z; - output_modifier.push_back(std::move(point)); - } - } -} - -void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new_period) -{ - if (!timer_) { - return; - } - int64_t old_period = 0; - rcl_ret_t ret = rcl_timer_get_period(timer_->get_timer_handle().get(), &old_period); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't get old period"); - } - ret = rcl_timer_exchange_period(timer_->get_timer_handle().get(), new_period, &old_period); - if (ret != RCL_RET_OK) { - rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't exchange_period"); - } -} - -void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) -{ - if (!utils::is_data_layout_compatible_with_point_xyzirc(*input_ptr)) { - RCLCPP_ERROR( - get_logger(), "The pointcloud layout is not compatible with PointXYZIRC. Aborting"); - - if (utils::is_data_layout_compatible_with_point_xyzi(*input_ptr)) { - RCLCPP_ERROR( - get_logger(), - "The pointcloud layout is compatible with PointXYZI. You may be using legacy code/data"); - } - - return; - } - - std::lock_guard lock(mutex_); - sensor_msgs::msg::PointCloud2::SharedPtr xyzirc_input_ptr(new sensor_msgs::msg::PointCloud2()); - auto input = std::make_shared(*input_ptr); - if (input->data.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - } else { - // convert to XYZIRC pointcloud if pointcloud is not empty - convertToXYZIRCCloud(input, xyzirc_input_ptr); - } - - const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); - const bool is_already_subscribed_tmp = std::any_of( - std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), - [](const auto & e) { return e.second != nullptr; }); - - if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyzirc_input_ptr; - - if (!is_already_subscribed_tmp) { - auto period = std::chrono::duration_cast( - std::chrono::duration(timeout_sec_)); - try { - setPeriod(period.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - } - timer_->reset(); - } - } else { - cloud_stdmap_[topic_name] = xyzirc_input_ptr; - - const bool is_subscribed_all = std::all_of( - std::begin(cloud_stdmap_), std::end(cloud_stdmap_), - [](const auto & e) { return e.second != nullptr; }); - - if (is_subscribed_all) { - for (const auto & e : cloud_stdmap_tmp_) { - if (e.second != nullptr) { - cloud_stdmap_[e.first] = e.second; - } - } - std::for_each(std::begin(cloud_stdmap_tmp_), std::end(cloud_stdmap_tmp_), [](auto & e) { - e.second = nullptr; - }); - - timer_->cancel(); - publish(); - } else if (offset_map_.size() > 0) { - timer_->cancel(); - auto period = std::chrono::duration_cast( - std::chrono::duration(timeout_sec_ - offset_map_[topic_name])); - try { - setPeriod(period.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - } - timer_->reset(); - } - } -} - -void PointCloudConcatenateDataSynchronizerComponent::timer_callback() -{ - using std::chrono_literals::operator""ms; - timer_->cancel(); - if (mutex_.try_lock()) { - publish(); - mutex_.unlock(); - } else { - try { - std::chrono::nanoseconds period = 10ms; - setPeriod(period.count()); - } catch (rclcpp::exceptions::RCLError & ex) { - RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "%s", ex.what()); - } - timer_->reset(); - } -} - -void PointCloudConcatenateDataSynchronizerComponent::twist_callback( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input) -{ - // if rosbag restart, clear buffer - if (!twist_ptr_queue_.empty()) { - if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { - twist_ptr_queue_.clear(); - } - } - - // pop old data - while (!twist_ptr_queue_.empty()) { - if ( - rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > - rclcpp::Time(input->header.stamp)) { - break; - } - twist_ptr_queue_.pop_front(); - } - - auto twist_ptr = std::make_shared(); - twist_ptr->header = input->header; - twist_ptr->twist = input->twist.twist; - twist_ptr_queue_.push_back(twist_ptr); -} - -void PointCloudConcatenateDataSynchronizerComponent::odom_callback( - const nav_msgs::msg::Odometry::ConstSharedPtr input) -{ - // if rosbag restart, clear buffer - if (!twist_ptr_queue_.empty()) { - if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { - twist_ptr_queue_.clear(); - } - } - - // pop old data - while (!twist_ptr_queue_.empty()) { - if ( - rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > - rclcpp::Time(input->header.stamp)) { - break; - } - twist_ptr_queue_.pop_front(); - } - - auto twist_ptr = std::make_shared(); - twist_ptr->header = input->header; - twist_ptr->twist = input->twist.twist; - twist_ptr_queue_.push_back(twist_ptr); -} - -void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus( - diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - for (const std::string & e : input_topics_) { - const std::string subscribe_status = not_subscribed_topic_names_.count(e) ? "NG" : "OK"; - stat.add(e, subscribe_status); - } - - const int8_t level = not_subscribed_topic_names_.empty() - ? diagnostic_msgs::msg::DiagnosticStatus::OK - : diagnostic_msgs::msg::DiagnosticStatus::WARN; - const std::string message = not_subscribed_topic_names_.empty() - ? "Concatenate all topics" - : "Some topics are not concatenated"; - stat.summary(level, message); -} -} // namespace autoware::pointcloud_preprocessor - -#include -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent) diff --git a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp index ba29389b88bf7..ba5e2eb51997a 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/concatenate_data/concatenate_pointclouds.cpp @@ -271,7 +271,7 @@ void PointCloudConcatenationComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } } @@ -297,9 +297,9 @@ void PointCloudConcatenationComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp index ecbc5b8fd13ef..54c940d414db8 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/crop_box_filter/crop_box_filter_node.cpp @@ -194,9 +194,9 @@ void CropBoxFilterComponent::faster_filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -204,7 +204,7 @@ void CropBoxFilterComponent::faster_filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp index 9eaafeae9dbc7..cc05a6cc2765c 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector_node.cpp @@ -123,7 +123,7 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po std::chrono::nanoseconds( (this->get_clock()->now() - pointcloud_msg->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } @@ -133,9 +133,9 @@ void DistortionCorrectorComponent::pointcloud_callback(PointCloud2::UniquePtr po if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp index 10e2fa2511478..0c467234d6591 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp @@ -124,15 +124,7 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( size_t output_global_offset = 0; output.data.resize(voxel_map.size() * input->point_step); for (const auto & kv : voxel_map) { - std::memcpy( - &output.data[output_global_offset + x_offset], &input->data[kv.second + x_offset], - sizeof(float)); - std::memcpy( - &output.data[output_global_offset + y_offset], &input->data[kv.second + y_offset], - sizeof(float)); - std::memcpy( - &output.data[output_global_offset + z_offset], &input->data[kv.second + z_offset], - sizeof(float)); + std::memcpy(&output.data[output_global_offset], &input->data[kv.second], input->point_step); output_global_offset += input->point_step; } @@ -150,9 +142,9 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -160,7 +152,7 @@ void PickupBasedVoxelGridDownsampleFilterComponent::filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h index 55307a2aa5552..0748516665e40 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h +++ b/sensing/autoware_pointcloud_preprocessor/src/downsample_filter/robin_hood.h @@ -2189,12 +2189,6 @@ class Table return 0 == mNumElements; } - ROBIN_HOOD(NODISCARD) size_t mask() const noexcept - { - ROBIN_HOOD_TRACE(this) - return mMask; - } - ROBIN_HOOD(NODISCARD) size_t calcMaxNumElementsAllowed(size_t maxElements) const noexcept { if (ROBIN_HOOD_LIKELY(maxElements <= (std::numeric_limits::max)() / 100)) { diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp index a8024e02de2c1..bac3a59e09528 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/dual_return_outlier_filter_node.cpp @@ -65,7 +65,7 @@ DualReturnOutlierFilterComponent::DualReturnOutlierFilterComponent( image_pub_ = image_transport::create_publisher(this, "dual_return_outlier_filter/debug/frequency_image"); - visibility_pub_ = create_publisher( + visibility_pub_ = create_publisher( "dual_return_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); { rclcpp::PublisherOptions pub_options; @@ -322,7 +322,7 @@ void DualReturnOutlierFilterComponent::filter( frequency_image_msg->header = input->header; // Publish histogram image image_pub_.publish(frequency_image_msg); - tier4_debug_msgs::msg::Float32Stamped visibility_msg; + autoware_internal_debug_msgs::msg::Float32Stamped visibility_msg; visibility_msg.data = (1.0f - filled); visibility_msg.stamp = now(); visibility_pub_->publish(visibility_msg); diff --git a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp index fae9043143ba4..fcea299ba5aff 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_node.cpp @@ -41,7 +41,7 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions outlier_pointcloud_publisher_ = this->create_publisher("debug/ring_outlier_filter", 1, pub_options); } - visibility_pub_ = create_publisher( + visibility_pub_ = create_publisher( "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); @@ -262,7 +262,7 @@ void RingOutlierFilterComponent::faster_filter( outlier.header = input->header; outlier_pointcloud_publisher_->publish(outlier); - tier4_debug_msgs::msg::Float32Stamped visibility_msg; + autoware_internal_debug_msgs::msg::Float32Stamped visibility_msg; visibility_msg.data = calculateVisibilityScore(outlier); visibility_msg.stamp = input->header.stamp; visibility_pub_->publish(visibility_msg); @@ -272,9 +272,9 @@ void RingOutlierFilterComponent::faster_filter( if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); auto pipeline_latency_ms = @@ -282,7 +282,7 @@ void RingOutlierFilterComponent::faster_filter( std::chrono::nanoseconds((this->get_clock()->now() - input->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug/pipeline_latency_ms", pipeline_latency_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp index 42170fd88ee36..e5bd4a955590f 100644 --- a/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp +++ b/sensing/autoware_pointcloud_preprocessor/src/time_synchronizer/time_synchronizer_node.cpp @@ -378,7 +378,7 @@ void PointCloudDataSynchronizerComponent::publish() std::chrono::nanoseconds( (this->get_clock()->now() - e.second->header.stamp).nanoseconds())) .count(); - debug_publisher_->publish( + debug_publisher_->publish( "debug" + e.first + "/pipeline_latency_ms", pipeline_latency_ms); } auto output = std::make_unique(*e.second); @@ -400,9 +400,9 @@ void PointCloudDataSynchronizerComponent::publish() if (debug_publisher_) { const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true); const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true); - debug_publisher_->publish( + debug_publisher_->publish( "debug/cyclic_time_ms", cyclic_time_ms); - debug_publisher_->publish( + debug_publisher_->publish( "debug/processing_time_ms", processing_time_ms); } } diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_component.py b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_component.py new file mode 100644 index 0000000000000..3d15eecf0101c --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_component.py @@ -0,0 +1,909 @@ +#!/usr/bin/env python3 + +# 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. + +import random +import struct +import time +from typing import List +from typing import Tuple +import unittest + +from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import TwistWithCovarianceStamped +import launch +import launch.actions +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode +import launch_testing +import numpy as np +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from rclpy.qos import QoSReliabilityPolicy +from rclpy.time import Time +from sensor_msgs.msg import PointCloud2 +from sensor_msgs.msg import PointField +from sensor_msgs_py import point_cloud2 +from std_msgs.msg import Header +from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster + +INPUT_LIDAR_TOPICS = [ + "/test/sensing/lidar/left/pointcloud", + "/test/sensing/lidar/right/pointcloud", + "/test/sensing/lidar/top/pointcloud", +] +FRAME_ID_LISTS = [ + "left_lidar", + "right_lidar", + "top_lidar", +] + +TIMEOUT_SEC = 0.2 +TIMESTAMP_OFFSET = [0.0, 0.04, 0.08] +TIMESTAMP_NOISE = 0.01 # 10 ms + +NUM_OF_POINTS = 3 +MILLISECONDS = 1000000 + + +STANDARD_TOLERANCE = 1e-4 +COARSE_TOLERANCE = TIMESTAMP_NOISE * 2 + +GLOBAL_SECONDS = 10 +GLOBAL_NANOSECONDS = 100000000 + +# Set to True if you want to check the output of the component tests. +DEBUG = False + + +@pytest.mark.launch_test +def generate_test_description(): + nodes = [] + + nodes.append( + ComposableNode( + package="autoware_pointcloud_preprocessor", + plugin="autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", + name="test_concatenate_node", + remappings=[ + ("~/input/twist", "/test/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "/test/sensing/lidar/concatenated/pointcloud"), + ], + parameters=[ + { + "debug_mode": False, + "has_static_tf_only": False, + "rosbag_length": 0.0, + "maximum_queue_size": 5, + "timeout_sec": TIMEOUT_SEC, + "is_motion_compensated": True, + "publish_synchronized_pointcloud": True, + "keep_input_frame_in_synchronized_pointcloud": True, + "publish_previous_but_late_pointcloud": True, + "synchronized_pointcloud_postfix": "pointcloud", + "input_twist_topic_type": "twist", + "input_topics": INPUT_LIDAR_TOPICS, + "output_frame": "base_link", + "matching_strategy.type": "advanced", + "matching_strategy.lidar_timestamp_offsets": TIMESTAMP_OFFSET, + "matching_strategy.lidar_timestamp_noise_window": [ + TIMESTAMP_NOISE, + TIMESTAMP_NOISE, + TIMESTAMP_NOISE, + ], + } + ], + extra_arguments=[{"use_intra_process_comms": True}], + ) + ) + + container = ComposableNodeContainer( + name="test_concatenate_data_container", + namespace="pointcloud_preprocessor", + package="rclcpp_components", + executable="component_container", + composable_node_descriptions=nodes, + output="screen", + ) + + return launch.LaunchDescription( + [ + container, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ) + + +def create_header(timestamp: Time, frame_id_index: int, is_base_link: bool) -> Header: + header = Header() + header.stamp = timestamp + + if is_base_link: + header.frame_id = "base_link" + else: + header.frame_id = FRAME_ID_LISTS[frame_id_index] + return header + + +def create_points() -> List[Tuple[float, float, float]]: + return [(1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0)] + + +def create_fields() -> ( + Tuple[List[int], List[int], List[int], List[float], List[float], List[float], List[int]] +): + # The values of the fields do not influence the results. + intensities = [255] * NUM_OF_POINTS + return_types = [1] * NUM_OF_POINTS + channels = [1] * NUM_OF_POINTS + azimuths = [0.0] * NUM_OF_POINTS + elevations = [0.0] * NUM_OF_POINTS + distances = [1.0] * NUM_OF_POINTS + timestamps = [0] * NUM_OF_POINTS + return intensities, return_types, channels, azimuths, elevations, distances, timestamps + + +def get_pointcloud_msg( + timestamp: Time, is_generate_points: bool, frame_id_index: int, is_base_link: bool +) -> PointCloud2: + header = create_header(timestamp, frame_id_index, is_base_link) + points = create_points() + intensities, return_types, channels, azimuths, elevations, distances, timestamps = ( + create_fields() + ) + + pointcloud_data = bytearray() + + if is_generate_points: + for i in range(NUM_OF_POINTS): + pointcloud_data += struct.pack("fff", points[i][0], points[i][1], points[i][2]) + pointcloud_data += struct.pack("B", intensities[i]) + pointcloud_data += struct.pack("B", return_types[i]) + pointcloud_data += struct.pack("H", channels[i]) + pointcloud_data += struct.pack("f", azimuths[i]) + pointcloud_data += struct.pack("f", elevations[i]) + pointcloud_data += struct.pack("f", distances[i]) + pointcloud_data += struct.pack("I", timestamps[i]) + + fields = [ + PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1), + PointField(name="intensity", offset=12, datatype=PointField.UINT8, count=1), + PointField(name="return_type", offset=13, datatype=PointField.UINT8, count=1), + PointField(name="channel", offset=14, datatype=PointField.UINT16, count=1), + PointField(name="azimuth", offset=16, datatype=PointField.FLOAT32, count=1), + PointField(name="elevation", offset=20, datatype=PointField.FLOAT32, count=1), + PointField(name="distance", offset=24, datatype=PointField.FLOAT32, count=1), + PointField(name="time_stamp", offset=28, datatype=PointField.UINT32, count=1), + ] + + pointcloud_msg = PointCloud2( + header=header, + height=1, + width=NUM_OF_POINTS, + is_dense=True, + is_bigendian=False, + point_step=32, # 3*4 + 1 + 1 + 2 + 4 + 4 + 4 + 4 = 32 bytes per point + row_step=32 * NUM_OF_POINTS, + fields=fields, + data=pointcloud_data, + ) + + return pointcloud_msg + + +def generate_transform_msg( + parent_frame: str, + child_frame: str, + x: float, + y: float, + z: float, + qx: float, + qy: float, + qz: float, + qw: float, +) -> TransformStamped: + tf_msg = TransformStamped() + tf_msg.header.stamp = Time(seconds=GLOBAL_SECONDS, nanoseconds=GLOBAL_NANOSECONDS).to_msg() + tf_msg.header.frame_id = parent_frame + tf_msg.child_frame_id = child_frame + tf_msg.transform.translation.x = x + tf_msg.transform.translation.y = y + tf_msg.transform.translation.z = z + tf_msg.transform.rotation.x = qx + tf_msg.transform.rotation.y = qy + tf_msg.transform.rotation.z = qz + tf_msg.transform.rotation.w = qw + return tf_msg + + +def generate_static_transform_msgs() -> List[TransformStamped]: + tf_top_lidar_msg = generate_transform_msg( + parent_frame="base_link", + child_frame=FRAME_ID_LISTS[0], + x=0.0, + y=0.0, + z=5.0, + qx=0.0, + qy=0.0, + qz=0.0, + qw=1.0, + ) + + tf_right_lidar_msg = generate_transform_msg( + parent_frame="base_link", + child_frame=FRAME_ID_LISTS[1], + x=0.0, + y=5.0, + z=5.0, + qx=0.0, + qy=0.0, + qz=0.0, + qw=1.0, + ) + + tf_left_lidar_msg = generate_transform_msg( + parent_frame="base_link", + child_frame=FRAME_ID_LISTS[2], + x=0.0, + y=-5.0, + z=5.0, + qx=0.0, + qy=0.0, + qz=0.0, + qw=1.0, + ) + + return [tf_top_lidar_msg, tf_right_lidar_msg, tf_left_lidar_msg] + + +def generate_twist_msg() -> TwistWithCovarianceStamped: + twist_header = Header() + twist_header.stamp = Time(seconds=GLOBAL_SECONDS, nanoseconds=GLOBAL_NANOSECONDS).to_msg() + twist_header.frame_id = "base_link" + twist_msg = TwistWithCovarianceStamped() + twist_msg.header = twist_header + twist_msg.twist.twist.linear.x = 1.0 + + return twist_msg + + +def get_output_points(cloud_msg) -> np.ndarray: + points_list = [] + for point in point_cloud2.read_points(cloud_msg, field_names=("x", "y", "z")): + points_list.append([point[0], point[1], point[2]]) + points = np.array(points_list, dtype=np.float32) + return points + + +class TestConcatenateNode(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Init ROS at once + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown ROS at once + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node("test_concat_node") + tf_msg = generate_static_transform_msgs() + self.tf_broadcaster = StaticTransformBroadcaster(self.node) + self.tf_broadcaster.sendTransform(tf_msg) + self.msg_buffer = [] + self.twist_publisher, self.pointcloud_publishers = self.create_pub_sub() + + def tearDown(self): + self.node.destroy_node() + + def callback(self, msg: PointCloud2): + self.msg_buffer.append(msg) + + def create_pub_sub(self): + # QoS profile for sensor data + sensor_qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=10, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + durability=QoSDurabilityPolicy.VOLATILE, + ) + # Publishers + twist_publisher = self.node.create_publisher( + TwistWithCovarianceStamped, + "/test/sensing/vehicle_velocity_converter/twist_with_covariance", + 10, + ) + + pointcloud_publishers = {} + for idx, input_lidar_topic in enumerate(INPUT_LIDAR_TOPICS): + pointcloud_publishers[idx] = self.node.create_publisher( + PointCloud2, + input_lidar_topic, + qos_profile=sensor_qos, + ) + + # create subscriber + self.msg_buffer = [] + self.node.create_subscription( + PointCloud2, + "/test/sensing/lidar/concatenated/pointcloud", + self.callback, + qos_profile=sensor_qos, + ) + + return twist_publisher, pointcloud_publishers + + def test_1_normal_inputs(self): + """Test the normal situation when no pointcloud is delayed or dropped. + + This can test that + 1. Concatenate works fine when all pointclouds are arrived in time. + 2. The motion compensation of concatenation works well. + """ + time.sleep(1) + global GLOBAL_SECONDS + + twist_msg = generate_twist_msg() + self.twist_publisher.publish(twist_msg) + + for frame_idx, _ in enumerate(INPUT_LIDAR_TOPICS): + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = GLOBAL_NANOSECONDS + frame_idx * MILLISECONDS * 40 # add 40 ms + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=frame_idx, + is_base_link=False, + ) + self.pointcloud_publishers[frame_idx].publish(pointcloud_msg) + time.sleep(0.01) + + rclpy.spin_once(self.node, timeout_sec=0.1) + + self.assertEqual( + len(self.msg_buffer), + 1, + "The number of concatenate pointcloud has different number as expected.", + ) + + expected_pointcloud = np.array( + [ + [1.08, -5, 5], + [0.08, -4, 5], + [0.08, -5, 6], + [1.04, 5, 5], + [0.04, 6, 5], + [0.04, 5, 6], + [1, 0, 5], + [0, 1, 5], + [0, 0, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud = get_output_points(self.msg_buffer[0]) + + if DEBUG: + print("concatenate_cloud: ", concatenate_cloud) + + self.assertTrue( + np.allclose(concatenate_cloud, expected_pointcloud, atol=1e-3), + "The concatenation node have weird output", + ) + + self.assertEqual( + self.msg_buffer[0].header.frame_id, + "base_link", + "The concatenate pointcloud frame id is not base_link", + ) + + GLOBAL_SECONDS += 1 + + def test_2_normal_inputs_with_noise(self): + """Test the normal situation when no pointcloud is delayed or dropped. Additionally, the pointcloud's timestamp is not ideal which has some noise. + + This can test that + 1. Concatenate works fine when pointclouds' timestamp has noise. + """ + time.sleep(1) + global GLOBAL_SECONDS + + twist_msg = generate_twist_msg() + self.twist_publisher.publish(twist_msg) + + for frame_idx, _ in enumerate(INPUT_LIDAR_TOPICS): + noise = random.uniform(-10, 10) * MILLISECONDS + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = ( + GLOBAL_NANOSECONDS + frame_idx * MILLISECONDS * 40 + noise + ) # add 40 ms and noise (-10 to 10 ms) + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=frame_idx, + is_base_link=False, + ) + self.pointcloud_publishers[frame_idx].publish(pointcloud_msg) + time.sleep(0.01) + + rclpy.spin_once(self.node, timeout_sec=0.1) + + self.assertEqual( + len(self.msg_buffer), + 1, + "The number of concatenate pointcloud has different number as expected.", + ) + + expected_pointcloud = np.array( + [ + [1.08, -5, 5], + [0.08, -4, 5], + [0.08, -5, 6], + [1.04, 5, 5], + [0.04, 6, 5], + [0.04, 5, 6], + [1, 0, 5], + [0, 1, 5], + [0, 0, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud = get_output_points(self.msg_buffer[0]) + if DEBUG: + print("concatenate_cloud: ", concatenate_cloud) + + self.assertTrue( + np.allclose(concatenate_cloud, expected_pointcloud, atol=2e-2), + "The concatenation node have weird output", + ) + + def test_3_abnormal_null_pointcloud(self): + """Test the abnormal situation when a pointcloud is empty. + + This can test that + 1. The concatenate node ignore empty pointcloud and concatenate the remain pointcloud. + """ + time.sleep(1) + global GLOBAL_SECONDS + + twist_msg = generate_twist_msg() + self.twist_publisher.publish(twist_msg) + + for frame_idx, _ in enumerate(INPUT_LIDAR_TOPICS): + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = GLOBAL_NANOSECONDS + frame_idx * MILLISECONDS * 40 # add 40 ms + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + + if frame_idx == len(INPUT_LIDAR_TOPICS) - 1: + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=False, + frame_id_index=len(INPUT_LIDAR_TOPICS) - 1, + is_base_link=False, + ) + else: + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=frame_idx, + is_base_link=False, + ) + + self.pointcloud_publishers[frame_idx].publish(pointcloud_msg) + time.sleep(0.01) + + time.sleep(TIMEOUT_SEC) # timeout threshold + rclpy.spin_once(self.node, timeout_sec=0.1) + + self.assertEqual( + len(self.msg_buffer), + 1, + "The number of concatenate pointcloud has different number as expected.", + ) + + expected_pointcloud = np.array( + [ + [1.04, 5, 5], + [0.04, 6, 5], + [0.04, 5, 6], + [1, 0, 5], + [0, 1, 5], + [0, 0, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud = get_output_points(self.msg_buffer[0]) + if DEBUG: + print("concatenate_cloud: ", concatenate_cloud) + + self.assertTrue( + np.allclose(concatenate_cloud, expected_pointcloud, atol=1e-3), + "The concatenation node have weird output", + ) + + GLOBAL_SECONDS += 1 + + def test_4_abnormal_null_pointcloud_and_drop(self): + """Test the abnormal situation when a pointcloud is empty and other pointclouds are dropped. + + This can test that + 1. The concatenate node publish an empty pointcloud. + """ + time.sleep(1) + global GLOBAL_SECONDS + + twist_msg = generate_twist_msg() + self.twist_publisher.publish(twist_msg) + + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = GLOBAL_NANOSECONDS + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=False, + frame_id_index=0, + is_base_link=False, + ) + + self.pointcloud_publishers[0].publish(pointcloud_msg) + time.sleep(0.01) + + time.sleep(TIMEOUT_SEC) # timeout threshold + rclpy.spin_once(self.node, timeout_sec=0.1) + + self.assertEqual( + len(self.msg_buffer), + 1, + "The number of concatenate pointcloud has different number as expected.", + ) + + GLOBAL_SECONDS += 1 + + def test_5_abnormal_multiple_pointcloud_drop(self): + """Test the abnormal situation when multiple pointclouds were dropped (only one pointcloud arrive). + + This can test that + 1. The concatenate node concatenates the single pointcloud after the timeout. + """ + time.sleep(1) + global GLOBAL_SECONDS + + twist_msg = generate_twist_msg() + self.twist_publisher.publish(twist_msg) + + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = GLOBAL_NANOSECONDS + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=0, + is_base_link=False, + ) + + self.pointcloud_publishers[0].publish(pointcloud_msg) + time.sleep(0.01) + + time.sleep(TIMEOUT_SEC) # timeout threshold + rclpy.spin_once(self.node, timeout_sec=0.1) + + self.assertEqual( + len(self.msg_buffer), + 1, + "The number of concatenate pointcloud has different number as expected.", + ) + + expected_pointcloud = np.array( + [ + [1, 0, 5], + [0, 1, 5], + [0, 0, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud = get_output_points(self.msg_buffer[0]) + if DEBUG: + print("concatenate_cloud: ", concatenate_cloud) + + self.assertTrue( + np.allclose(concatenate_cloud, expected_pointcloud, atol=1e-3), + "The concatenation node have weird output", + ) + + def test_6_abnormal_single_pointcloud_drop(self): + """Test the abnormal situation when a pointcloud was dropped. + + This can test that + 1. The concatenate node concatenate the remain pointcloud after the timeout. + """ + time.sleep(1) + global GLOBAL_SECONDS + + twist_msg = generate_twist_msg() + self.twist_publisher.publish(twist_msg) + + for frame_idx, _ in enumerate(INPUT_LIDAR_TOPICS[:-1]): + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = GLOBAL_NANOSECONDS + frame_idx * MILLISECONDS * 40 # add 40 ms + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=frame_idx, + is_base_link=False, + ) + self.pointcloud_publishers[frame_idx].publish(pointcloud_msg) + time.sleep(0.02) + + time.sleep(TIMEOUT_SEC) # timeout threshold + rclpy.spin_once(self.node, timeout_sec=0.1) + + # Should receive only one concatenate pointcloud + self.assertEqual( + len(self.msg_buffer), + 1, + "The number of concatenate pointcloud has different number as expected.", + ) + + expected_pointcloud = np.array( + [ + [1.04, 5, 5], + [0.04, 6, 5], + [0.04, 5, 6], + [1, 0, 5], + [0, 1, 5], + [0, 0, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud = get_output_points(self.msg_buffer[0]) + if DEBUG: + print("concatenate_cloud: ", concatenate_cloud) + + self.assertTrue( + np.allclose(concatenate_cloud, expected_pointcloud, atol=1e-3), + "The concatenation node have weird output", + ) + + GLOBAL_SECONDS += 1 + + def test_7_abnormal_pointcloud_delay(self): + """Test the abnormal situation when a pointcloud was delayed after the timeout. + + This can test that + 1. The concatenate node concatenate the remain pointcloud after the timeout. + 2. The concatenate node will publish the delayed pointcloud after the timeout. + """ + time.sleep(1) + global GLOBAL_SECONDS + + twist_msg = generate_twist_msg() + self.twist_publisher.publish(twist_msg) + + for frame_idx, _ in enumerate(INPUT_LIDAR_TOPICS[:-1]): + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = GLOBAL_NANOSECONDS + frame_idx * MILLISECONDS * 40 # add 40 ms + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=frame_idx, + is_base_link=False, + ) + self.pointcloud_publishers[frame_idx].publish(pointcloud_msg) + time.sleep(0.02) + + time.sleep(TIMEOUT_SEC) # timeout threshold + rclpy.spin_once(self.node, timeout_sec=0.1) + + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = ( + GLOBAL_NANOSECONDS + (len(INPUT_LIDAR_TOPICS) - 1) * MILLISECONDS * 40 + ) # add 40 ms + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=len(INPUT_LIDAR_TOPICS) - 1, + is_base_link=False, + ) + + self.pointcloud_publishers[len(INPUT_LIDAR_TOPICS) - 1].publish(pointcloud_msg) + + time.sleep(TIMEOUT_SEC) # timeout threshold + rclpy.spin_once(self.node, timeout_sec=0.1) + + # Should receive only one concatenate pointcloud + self.assertEqual( + len(self.msg_buffer), + 2, + "The number of concatenate pointcloud has different number as expected.", + ) + + expected_pointcloud1 = np.array( + [ + [1.04, 5, 5], + [0.04, 6, 5], + [0.04, 5, 6], + [1, 0, 5], + [0, 1, 5], + [0, 0, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud1 = get_output_points(self.msg_buffer[0]) + if DEBUG: + print("concatenate_cloud 1: ", concatenate_cloud1) + + self.assertTrue( + np.allclose(concatenate_cloud1, expected_pointcloud1, atol=1e-3), + "The concatenation node have weird output", + ) + + expected_pointcloud2 = np.array( + [ + [1, -5, 5], + [0, -4, 5], + [0, -5, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud2 = get_output_points(self.msg_buffer[1]) + if DEBUG: + print("concatenate_cloud 2: ", concatenate_cloud2) + + self.assertTrue( + np.allclose(concatenate_cloud2, expected_pointcloud2, atol=1e-3), + "The concatenation node have weird output", + ) + + GLOBAL_SECONDS += 1 + + def test_8_abnormal_pointcloud_drop_continue_normal(self): + """Test the abnormal situation when a pointcloud was dropped. Afterward, next iteration of pointclouds comes normally. + + This can test that + 1. The concatenate node concatenate the remain pointcloud after the timeout. + 2. The concatenate node concatenate next iteration pointclouds when all of the pointcloud arrived. + """ + time.sleep(1) + global GLOBAL_SECONDS + + twist_msg = generate_twist_msg() + self.twist_publisher.publish(twist_msg) + + for frame_idx, _ in enumerate(INPUT_LIDAR_TOPICS[:-1]): + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = GLOBAL_NANOSECONDS + frame_idx * MILLISECONDS * 40 # add 40 ms + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=frame_idx, + is_base_link=False, + ) + self.pointcloud_publishers[frame_idx].publish(pointcloud_msg) + time.sleep(0.01) + + time.sleep(TIMEOUT_SEC) + rclpy.spin_once(self.node) + + next_global_nanosecond = GLOBAL_NANOSECONDS + 100 * MILLISECONDS + for frame_idx, _ in enumerate(INPUT_LIDAR_TOPICS): + pointcloud_seconds = GLOBAL_SECONDS + pointcloud_nanoseconds = ( + next_global_nanosecond + frame_idx * MILLISECONDS * 40 + ) # add 40 ms + pointcloud_timestamp = Time( + seconds=pointcloud_seconds, nanoseconds=pointcloud_nanoseconds + ).to_msg() + pointcloud_msg = get_pointcloud_msg( + timestamp=pointcloud_timestamp, + is_generate_points=True, + frame_id_index=frame_idx, + is_base_link=False, + ) + self.pointcloud_publishers[frame_idx].publish(pointcloud_msg) + time.sleep(0.01) + + rclpy.spin_once(self.node) + + self.assertEqual( + len(self.msg_buffer), + 2, + "The number of concatenate pointcloud has different number as expected.", + ) + + expected_pointcloud1 = np.array( + [ + [1.04, 5, 5], + [0.04, 6, 5], + [0.04, 5, 6], + [1, 0, 5], + [0, 1, 5], + [0, 0, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud1 = get_output_points(self.msg_buffer[0]) + if DEBUG: + print("concatenate_cloud 1: ", concatenate_cloud1) + + self.assertTrue( + np.allclose(concatenate_cloud1, expected_pointcloud1, atol=1e-3), + "The concatenation node have weird output", + ) + + expected_pointcloud2 = np.array( + [ + [1.08, -5, 5], + [0.08, -4, 5], + [0.08, -5, 6], + [1.04, 5, 5], + [0.04, 6, 5], + [0.04, 5, 6], + [1, 0, 5], + [0, 1, 5], + [0, 0, 6], + ], + dtype=np.float32, + ) + + concatenate_cloud2 = get_output_points(self.msg_buffer[1]) + if DEBUG: + print("concatenate_cloud 2: ", concatenate_cloud2) + + self.assertTrue( + np.allclose(concatenate_cloud2, expected_pointcloud2, atol=1e-3), + "The concatenation node have weird output", + ) + + GLOBAL_SECONDS += 1 diff --git a/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp new file mode 100644 index 0000000000000..bc3c7e9538c84 --- /dev/null +++ b/sensing/autoware_pointcloud_preprocessor/test/test_concatenate_node_unit.cpp @@ -0,0 +1,539 @@ +// 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. + +// Note: To regenerate the ground truth (GT) for the expected undistorted point cloud values, +// set the "debug_" value to true to display the point cloud values. Then, +// replace the expected values with the newly displayed undistorted point cloud values. + +#include "autoware/pointcloud_preprocessor/concatenate_data/cloud_collector.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp" +#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp" + +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +class ConcatenateCloudTest : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::NodeOptions node_options; + // Instead of "input_topics", other parameters are not used. + // They just helps to setup the concatenate node + node_options.parameter_overrides( + {{"debug_mode", false}, + {"has_static_tf_only", false}, + {"rosbag_length", 0.0}, + {"maximum_queue_size", 5}, + {"timeout_sec", 0.2}, + {"is_motion_compensated", true}, + {"publish_synchronized_pointcloud", true}, + {"keep_input_frame_in_synchronized_pointcloud", true}, + {"publish_previous_but_late_pointcloud", false}, + {"synchronized_pointcloud_postfix", "pointcloud"}, + {"input_twist_topic_type", "twist"}, + {"input_topics", std::vector{"lidar_top", "lidar_left", "lidar_right"}}, + {"output_frame", "base_link"}, + {"matching_strategy.type", "advanced"}, + {"matching_strategy.lidar_timestamp_offsets", std::vector{0.0, 0.04, 0.08}}, + {"matching_strategy.lidar_timestamp_noise_window", std::vector{0.01, 0.01, 0.01}}}); + + concatenate_node_ = std::make_shared< + autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent>( + node_options); + combine_cloud_handler_ = + std::make_shared( + *concatenate_node_, "base_link", true, true, true, false); + + collector_ = std::make_shared( + std::dynamic_pointer_cast< + autoware::pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent>( + concatenate_node_->shared_from_this()), + combine_cloud_handler_, number_of_pointcloud, timeout_sec, collector_debug_mode); + + // Setup TF + tf_broadcaster_ = std::make_shared(concatenate_node_); + tf_broadcaster_->sendTransform(generate_static_transform_msgs()); + + // Spin the node for a while to ensure transforms are published + auto start = std::chrono::steady_clock::now(); + auto timeout = std::chrono::milliseconds(100); + while (std::chrono::steady_clock::now() - start < timeout) { + rclcpp::spin_some(concatenate_node_); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + + static geometry_msgs::msg::TransformStamped generate_transform_msg( + const std::string & parent_frame, const std::string & child_frame, double x, double y, double z, + double qx, double qy, double qz, double qw) + { + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + geometry_msgs::msg::TransformStamped tf_msg; + tf_msg.header.stamp = timestamp; + tf_msg.header.frame_id = parent_frame; + tf_msg.child_frame_id = child_frame; + tf_msg.transform.translation.x = x; + tf_msg.transform.translation.y = y; + tf_msg.transform.translation.z = z; + tf_msg.transform.rotation.x = qx; + tf_msg.transform.rotation.y = qy; + tf_msg.transform.rotation.z = qz; + tf_msg.transform.rotation.w = qw; + return tf_msg; + } + + static sensor_msgs::msg::PointCloud2 generate_pointcloud_msg( + bool generate_points, bool is_lidar_frame, const std::string & topic_name, + const rclcpp::Time & stamp) + { + sensor_msgs::msg::PointCloud2 pointcloud_msg; + pointcloud_msg.header.stamp = stamp; + pointcloud_msg.header.frame_id = is_lidar_frame ? topic_name : "base_link"; + pointcloud_msg.height = 1; + pointcloud_msg.is_dense = true; + pointcloud_msg.is_bigendian = false; + + if (generate_points) { + std::array points = {{ + Eigen::Vector3f(10.0f, 0.0f, 0.0f), // point 1 + Eigen::Vector3f(0.0f, 10.0f, 0.0f), // point 2 + Eigen::Vector3f(0.0f, 0.0f, 10.0f), // point 3 + }}; + + sensor_msgs::PointCloud2Modifier modifier(pointcloud_msg); + modifier.setPointCloud2Fields( + 10, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, + sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "intensity", 1, sensor_msgs::msg::PointField::UINT8, "return_type", 1, + sensor_msgs::msg::PointField::UINT8, "channel", 1, sensor_msgs::msg::PointField::UINT16, + "azimuth", 1, sensor_msgs::msg::PointField::FLOAT32, "elevation", 1, + sensor_msgs::msg::PointField::FLOAT32, "distance", 1, sensor_msgs::msg::PointField::FLOAT32, + "time_stamp", 1, sensor_msgs::msg::PointField::UINT32); + + modifier.resize(number_of_points); + + sensor_msgs::PointCloud2Iterator iter_x(pointcloud_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(pointcloud_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(pointcloud_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_t(pointcloud_msg, "time_stamp"); + + for (size_t i = 0; i < number_of_points; ++i) { + *iter_x = points[i].x(); + *iter_y = points[i].y(); + *iter_z = points[i].z(); + *iter_t = 0; + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_t; + } + } else { + pointcloud_msg.width = 0; + pointcloud_msg.row_step = 0; + } + + return pointcloud_msg; + } + + static std::vector generate_static_transform_msgs() + { + // generate defined transformations + return { + generate_transform_msg("base_link", "lidar_top", 5.0, 5.0, 5.0, 0.683, 0.5, 0.183, 0.499), + generate_transform_msg("base_link", "lidar_left", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453)}; + generate_transform_msg("base_link", "lidar_right", 1.0, 1.0, 3.0, 0.278, 0.717, 0.441, 0.453); + } + + std::shared_ptr + concatenate_node_; + std::shared_ptr combine_cloud_handler_; + std::shared_ptr collector_; + std::shared_ptr tf_broadcaster_; + + static constexpr int32_t timestamp_seconds{10}; + static constexpr uint32_t timestamp_nanoseconds{100'000'000}; + static constexpr size_t number_of_points{3}; + static constexpr float standard_tolerance{1e-4}; + static constexpr int number_of_pointcloud{3}; + static constexpr float timeout_sec{0.2}; + static constexpr bool collector_debug_mode{false}; // For showing collector information + bool debug_{false}; // For the Unit test +}; + +//////////////////////////////// Test combine_cloud_handler //////////////////////////////// +TEST_F(ConcatenateCloudTest, TestProcessTwist) +{ + auto twist_msg = std::make_shared(); + twist_msg->header.stamp = rclcpp::Time(10, 0); + twist_msg->twist.twist.linear.x = 1.0; + twist_msg->twist.twist.angular.z = 0.1; + + combine_cloud_handler_->process_twist(twist_msg); + + ASSERT_FALSE(combine_cloud_handler_->get_twist_queue().empty()); + EXPECT_EQ(combine_cloud_handler_->get_twist_queue().front().twist.linear.x, 1.0); + EXPECT_EQ(combine_cloud_handler_->get_twist_queue().front().twist.angular.z, 0.1); +} + +TEST_F(ConcatenateCloudTest, TestProcessOdometry) +{ + auto odom_msg = std::make_shared(); + odom_msg->header.stamp = rclcpp::Time(10, 0); + odom_msg->twist.twist.linear.x = 1.0; + odom_msg->twist.twist.angular.z = 0.1; + + combine_cloud_handler_->process_odometry(odom_msg); + + ASSERT_FALSE(combine_cloud_handler_->get_twist_queue().empty()); + EXPECT_EQ(combine_cloud_handler_->get_twist_queue().front().twist.linear.x, 1.0); + EXPECT_EQ(combine_cloud_handler_->get_twist_queue().front().twist.angular.z, 0.1); +} + +TEST_F(ConcatenateCloudTest, TestComputeTransformToAdjustForOldTimestamp) +{ + // If time difference between twist msg and pointcloud stamp is more than 100 miliseconds, return + // Identity transformation. case 1: time difference larger than 100 miliseconds + rclcpp::Time pointcloud_stamp1(10, 100'000'000, RCL_ROS_TIME); + rclcpp::Time pointcloud_stamp2(10, 210'000'000, RCL_ROS_TIME); + auto twist_msg1 = std::make_shared(); + twist_msg1->header.stamp = rclcpp::Time(9, 130'000'000, RCL_ROS_TIME); + twist_msg1->twist.twist.linear.x = 1.0; + twist_msg1->twist.twist.angular.z = 0.1; + combine_cloud_handler_->process_twist(twist_msg1); + + auto twist_msg2 = std::make_shared(); + twist_msg2->header.stamp = rclcpp::Time(9, 160'000'000, RCL_ROS_TIME); + twist_msg2->twist.twist.linear.x = 1.0; + twist_msg2->twist.twist.angular.z = 0.1; + combine_cloud_handler_->process_twist(twist_msg2); + + Eigen::Matrix4f transform = combine_cloud_handler_->compute_transform_to_adjust_for_old_timestamp( + pointcloud_stamp1, pointcloud_stamp2); + + // translation + EXPECT_NEAR(transform(0, 3), 0.0, standard_tolerance); + EXPECT_NEAR(transform(1, 3), 0.0, standard_tolerance); + + EXPECT_NEAR(transform(0, 0), 1.0, standard_tolerance); + EXPECT_NEAR(transform(0, 1), 0.0, standard_tolerance); + EXPECT_NEAR(transform(1, 0), 0.0, standard_tolerance); + EXPECT_NEAR(transform(1, 1), 1.0, standard_tolerance); + + std::ostringstream oss; + oss << "Transformation matrix from cloud 2 to cloud 1:\n" << transform; + + if (debug_) { + RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str()); + } + + // case 2: time difference smaller than 100 miliseconds + rclcpp::Time pointcloud_stamp3(11, 100'000'000, RCL_ROS_TIME); + rclcpp::Time pointcloud_stamp4(11, 150'000'000, RCL_ROS_TIME); + auto twist_msg3 = std::make_shared(); + twist_msg3->header.stamp = rclcpp::Time(11, 130'000'000, RCL_ROS_TIME); + twist_msg3->twist.twist.linear.x = 1.0; + twist_msg3->twist.twist.angular.z = 0.1; + combine_cloud_handler_->process_twist(twist_msg3); + + auto twist_msg4 = std::make_shared(); + twist_msg4->header.stamp = rclcpp::Time(11, 160'000'000, RCL_ROS_TIME); + twist_msg4->twist.twist.linear.x = 1.0; + twist_msg4->twist.twist.angular.z = 0.1; + combine_cloud_handler_->process_twist(twist_msg4); + + transform = combine_cloud_handler_->compute_transform_to_adjust_for_old_timestamp( + pointcloud_stamp3, pointcloud_stamp4); + + // translation + EXPECT_NEAR(transform(0, 3), 0.0499996, standard_tolerance); + EXPECT_NEAR(transform(1, 3), 0.000189999, standard_tolerance); + + // rotation, yaw = 0.005 + EXPECT_NEAR(transform(0, 0), 0.999987, standard_tolerance); + EXPECT_NEAR(transform(0, 1), -0.00499998, standard_tolerance); + EXPECT_NEAR(transform(1, 0), 0.00499998, standard_tolerance); + EXPECT_NEAR(transform(1, 1), 0.999987, standard_tolerance); + + oss.str(""); + oss.clear(); + oss << "Transformation matrix from cloud 4 to cloud 3:\n" << transform; + + if (debug_) { + RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str()); + } +} + +//////////////////////////////// Test cloud_collector //////////////////////////////// + +TEST_F(ConcatenateCloudTest, TestSetAndGetNaiveCollectorInfo) +{ + auto naive_info = std::make_shared(); + naive_info->timestamp = 15.0; + + collector_->set_info(naive_info); + auto collector_info_new = collector_->get_info(); + + auto naive_info_new = + std::dynamic_pointer_cast( + collector_info_new); + ASSERT_NE(naive_info_new, nullptr) << "Collector info is not of type NaiveCollectorInfo"; + + EXPECT_DOUBLE_EQ(naive_info_new->timestamp, 15.0); +} + +TEST_F(ConcatenateCloudTest, TestSetAndGetAdvancedCollectorInfo) +{ + auto advanced_info = std::make_shared(); + advanced_info->timestamp = 10.0; + advanced_info->noise_window = 0.1; + collector_->set_info(advanced_info); + auto collector_info_new = collector_->get_info(); + auto advanced_info_new = + std::dynamic_pointer_cast( + collector_info_new); + ASSERT_NE(advanced_info_new, nullptr) << "Collector info is not of type AdvancedCollectorInfo"; + + // Validate the values + auto min = advanced_info_new->timestamp - advanced_info_new->noise_window; + auto max = advanced_info_new->timestamp + advanced_info_new->noise_window; + EXPECT_DOUBLE_EQ(min, 9.9); + EXPECT_DOUBLE_EQ(max, 10.1); +} + +TEST_F(ConcatenateCloudTest, TestConcatenateClouds) +{ + rclcpp::Time top_timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + rclcpp::Time left_timestamp(timestamp_seconds, timestamp_nanoseconds + 40'000'000, RCL_ROS_TIME); + rclcpp::Time right_timestamp(timestamp_seconds, timestamp_nanoseconds + 80'000'000, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 top_pointcloud = + generate_pointcloud_msg(true, false, "lidar_top", top_timestamp); + sensor_msgs::msg::PointCloud2 left_pointcloud = + generate_pointcloud_msg(true, false, "lidar_left", left_timestamp); + sensor_msgs::msg::PointCloud2 right_pointcloud = + generate_pointcloud_msg(true, false, "lidar_right", right_timestamp); + + sensor_msgs::msg::PointCloud2::SharedPtr top_pointcloud_ptr = + std::make_shared(top_pointcloud); + sensor_msgs::msg::PointCloud2::SharedPtr left_pointcloud_ptr = + std::make_shared(left_pointcloud); + sensor_msgs::msg::PointCloud2::SharedPtr right_pointcloud_ptr = + std::make_shared(right_pointcloud); + + std::unordered_map topic_to_cloud_map; + topic_to_cloud_map["lidar_top"] = top_pointcloud_ptr; + topic_to_cloud_map["lidar_left"] = left_pointcloud_ptr; + topic_to_cloud_map["lidar_right"] = right_pointcloud_ptr; + + auto [concatenate_cloud_ptr, topic_to_transformed_cloud_map, topic_to_original_stamp_map] = + collector_->concatenate_pointclouds(topic_to_cloud_map); + + // test output concatenate cloud + // No input twist, so it will not do the motion compensation + std::array expected_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 10.0f), Eigen::Vector3f(10.0f, 0.0f, 0.0f), + Eigen::Vector3f(0.0f, 10.0f, 0.0f), Eigen::Vector3f(0.0f, 0.0f, 10.0f), + Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 10.0f)}}; + + size_t i = 0; + std::ostringstream oss; + oss << "Concatenated pointcloud:\n"; + + sensor_msgs::PointCloud2Iterator iter_x(*concatenate_cloud_ptr, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*concatenate_cloud_ptr, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*concatenate_cloud_ptr, "z"); + + for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++i) { + oss << "Concatenated point " << i << ": (" << *iter_x << ", " << *iter_y << ", " << *iter_z + << ")\n"; + EXPECT_FLOAT_EQ(*iter_x, expected_pointcloud[i].x()); + EXPECT_FLOAT_EQ(*iter_y, expected_pointcloud[i].y()); + EXPECT_FLOAT_EQ(*iter_z, expected_pointcloud[i].z()); + } + + if (debug_) { + RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str()); + } + + // test concatenate cloud has the oldest pointcloud's timestamp + EXPECT_FLOAT_EQ( + top_timestamp.seconds(), rclcpp::Time(concatenate_cloud_ptr->header.stamp).seconds()); + + // test separated transformed cloud + std::array expected_top_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 10.0f)}}; + std::array expected_left_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 10.0f)}}; + std::array expected_right_pointcloud = { + {Eigen::Vector3f(10.0f, 0.0f, 0.0f), Eigen::Vector3f(0.0f, 10.0f, 0.0f), + Eigen::Vector3f(0.0f, 0.0f, 10.0f)}}; + + oss.clear(); + oss.str(""); + i = 0; + + sensor_msgs::PointCloud2Iterator top_pc_iter_x( + *(topic_to_transformed_cloud_map.value().at("lidar_top")), "x"); + sensor_msgs::PointCloud2Iterator top_pc_iter_y( + *(topic_to_transformed_cloud_map.value().at("lidar_top")), "y"); + sensor_msgs::PointCloud2Iterator top_pc_iter_z( + *(topic_to_transformed_cloud_map.value().at("lidar_top")), "z"); + + for (; top_pc_iter_x != top_pc_iter_x.end(); + ++top_pc_iter_x, ++top_pc_iter_y, ++top_pc_iter_z, ++i) { + oss << "Top point " << i << ": (" << *top_pc_iter_x << ", " << *top_pc_iter_y << ", " + << *top_pc_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*top_pc_iter_x, expected_top_pointcloud[i].x()); + EXPECT_FLOAT_EQ(*top_pc_iter_y, expected_top_pointcloud[i].y()); + EXPECT_FLOAT_EQ(*top_pc_iter_z, expected_top_pointcloud[i].z()); + } + + if (debug_) { + RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str()); + } + + oss.clear(); + oss.str(""); + i = 0; + sensor_msgs::PointCloud2Iterator left_pc_iter_x( + *(topic_to_transformed_cloud_map.value().at("lidar_left")), "x"); + sensor_msgs::PointCloud2Iterator left_pc_iter_y( + *(topic_to_transformed_cloud_map.value().at("lidar_left")), "y"); + sensor_msgs::PointCloud2Iterator left_pc_iter_z( + *(topic_to_transformed_cloud_map.value().at("lidar_left")), "z"); + + for (; left_pc_iter_x != left_pc_iter_x.end(); + ++left_pc_iter_x, ++left_pc_iter_y, ++left_pc_iter_z, ++i) { + oss << "Left point " << i << ": (" << *left_pc_iter_x << ", " << *left_pc_iter_y << ", " + << *left_pc_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*left_pc_iter_x, expected_left_pointcloud[i].x()); + EXPECT_FLOAT_EQ(*left_pc_iter_y, expected_left_pointcloud[i].y()); + EXPECT_FLOAT_EQ(*left_pc_iter_z, expected_left_pointcloud[i].z()); + } + + if (debug_) { + RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str()); + } + + oss.clear(); + oss.str(""); + i = 0; + sensor_msgs::PointCloud2Iterator right_pc_iter_x( + *(topic_to_transformed_cloud_map.value().at("lidar_right")), "x"); + sensor_msgs::PointCloud2Iterator right_pc_iter_y( + *(topic_to_transformed_cloud_map.value().at("lidar_right")), "y"); + sensor_msgs::PointCloud2Iterator right_pc_iter_z( + *(topic_to_transformed_cloud_map.value().at("lidar_right")), "z"); + + for (; right_pc_iter_x != right_pc_iter_x.end(); + ++right_pc_iter_x, ++right_pc_iter_y, ++right_pc_iter_z, ++i) { + oss << "Right point " << i << ": (" << *right_pc_iter_x << ", " << *right_pc_iter_y << ", " + << *right_pc_iter_z << ")\n"; + EXPECT_FLOAT_EQ(*right_pc_iter_x, expected_right_pointcloud[i].x()); + EXPECT_FLOAT_EQ(*right_pc_iter_y, expected_right_pointcloud[i].y()); + EXPECT_FLOAT_EQ(*right_pc_iter_z, expected_right_pointcloud[i].z()); + } + + if (debug_) { + RCLCPP_INFO(concatenate_node_->get_logger(), "%s", oss.str().c_str()); + } + + // test original cloud's timestamps + EXPECT_FLOAT_EQ(top_timestamp.seconds(), topic_to_original_stamp_map["lidar_top"]); + EXPECT_FLOAT_EQ(left_timestamp.seconds(), topic_to_original_stamp_map["lidar_left"]); + EXPECT_FLOAT_EQ(right_timestamp.seconds(), topic_to_original_stamp_map["lidar_right"]); +} + +TEST_F(ConcatenateCloudTest, TestProcessSingleCloud) +{ + concatenate_node_->add_cloud_collector(collector_); + + rclcpp::Time timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 top_pointcloud = + generate_pointcloud_msg(true, false, "lidar_top", timestamp); + sensor_msgs::msg::PointCloud2::SharedPtr top_pointcloud_ptr = + std::make_shared(top_pointcloud); + collector_->process_pointcloud("lidar_top", top_pointcloud_ptr); + + auto topic_to_cloud_map = collector_->get_topic_to_cloud_map(); + EXPECT_EQ(topic_to_cloud_map["lidar_top"], top_pointcloud_ptr); + EXPECT_FALSE(collector_->concatenate_finished()); + concatenate_node_->manage_collector_list(); + EXPECT_FALSE(concatenate_node_->get_cloud_collectors().empty()); + + // Sleep for timeout seconds (200 ms) + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + rclcpp::spin_some(concatenate_node_); + + // Collector should concatenate and publish the pointcloud, also delete itself. + EXPECT_TRUE(collector_->concatenate_finished()); + concatenate_node_->manage_collector_list(); + EXPECT_TRUE(concatenate_node_->get_cloud_collectors().empty()); +} + +TEST_F(ConcatenateCloudTest, TestProcessMultipleCloud) +{ + concatenate_node_->add_cloud_collector(collector_); + + rclcpp::Time top_timestamp(timestamp_seconds, timestamp_nanoseconds, RCL_ROS_TIME); + rclcpp::Time left_timestamp(timestamp_seconds, timestamp_nanoseconds + 40'000'000, RCL_ROS_TIME); + rclcpp::Time right_timestamp(timestamp_seconds, timestamp_nanoseconds + 80'000'000, RCL_ROS_TIME); + sensor_msgs::msg::PointCloud2 top_pointcloud = + generate_pointcloud_msg(true, false, "lidar_top", top_timestamp); + sensor_msgs::msg::PointCloud2 left_pointcloud = + generate_pointcloud_msg(true, false, "lidar_left", left_timestamp); + sensor_msgs::msg::PointCloud2 right_pointcloud = + generate_pointcloud_msg(true, false, "lidar_right", right_timestamp); + + sensor_msgs::msg::PointCloud2::SharedPtr top_pointcloud_ptr = + std::make_shared(top_pointcloud); + sensor_msgs::msg::PointCloud2::SharedPtr left_pointcloud_ptr = + std::make_shared(left_pointcloud); + sensor_msgs::msg::PointCloud2::SharedPtr right_pointcloud_ptr = + std::make_shared(right_pointcloud); + + collector_->process_pointcloud("lidar_top", top_pointcloud_ptr); + collector_->process_pointcloud("lidar_left", left_pointcloud_ptr); + collector_->process_pointcloud("lidar_right", right_pointcloud_ptr); + + EXPECT_TRUE(collector_->concatenate_finished()); + concatenate_node_->manage_collector_list(); + EXPECT_TRUE(concatenate_node_->get_cloud_collectors().empty()); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/sensing/autoware_radar_scan_to_pointcloud2/CHANGELOG.rst b/sensing/autoware_radar_scan_to_pointcloud2/CHANGELOG.rst index 4d92b7237c281..a719f7d396b19 100644 --- a/sensing/autoware_radar_scan_to_pointcloud2/CHANGELOG.rst +++ b/sensing/autoware_radar_scan_to_pointcloud2/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_radar_scan_to_pointcloud2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/sensing/autoware_radar_scan_to_pointcloud2/package.xml b/sensing/autoware_radar_scan_to_pointcloud2/package.xml index b14615cfb68ea..f95a95a27abee 100644 --- a/sensing/autoware_radar_scan_to_pointcloud2/package.xml +++ b/sensing/autoware_radar_scan_to_pointcloud2/package.xml @@ -1,7 +1,7 @@ autoware_radar_scan_to_pointcloud2 - 0.40.0 + 0.41.0 autoware_radar_scan_to_pointcloud2 Satoshi Tanaka Shunsuke Miura diff --git a/sensing/autoware_radar_static_pointcloud_filter/CHANGELOG.rst b/sensing/autoware_radar_static_pointcloud_filter/CHANGELOG.rst index b66f99ba578a6..ec5c7dfc1c19f 100644 --- a/sensing/autoware_radar_static_pointcloud_filter/CHANGELOG.rst +++ b/sensing/autoware_radar_static_pointcloud_filter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_radar_static_pointcloud_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/sensing/autoware_radar_static_pointcloud_filter/package.xml b/sensing/autoware_radar_static_pointcloud_filter/package.xml index 943bd71f29da7..4a5186a6a3ec1 100644 --- a/sensing/autoware_radar_static_pointcloud_filter/package.xml +++ b/sensing/autoware_radar_static_pointcloud_filter/package.xml @@ -1,7 +1,7 @@ autoware_radar_static_pointcloud_filter - 0.40.0 + 0.41.0 autoware_radar_static_pointcloud_filter Satoshi Tanaka Shunsuke Miura diff --git a/sensing/autoware_radar_threshold_filter/CHANGELOG.rst b/sensing/autoware_radar_threshold_filter/CHANGELOG.rst index e217daeca5a35..b5f3328d7e164 100644 --- a/sensing/autoware_radar_threshold_filter/CHANGELOG.rst +++ b/sensing/autoware_radar_threshold_filter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_radar_threshold_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/sensing/autoware_radar_threshold_filter/package.xml b/sensing/autoware_radar_threshold_filter/package.xml index 5e2a5395168fb..69e6969e5a429 100644 --- a/sensing/autoware_radar_threshold_filter/package.xml +++ b/sensing/autoware_radar_threshold_filter/package.xml @@ -1,7 +1,7 @@ autoware_radar_threshold_filter - 0.40.0 + 0.41.0 autoware_radar_threshold_filter Satoshi Tanaka Shunsuke Miura diff --git a/sensing/autoware_radar_tracks_noise_filter/CHANGELOG.rst b/sensing/autoware_radar_tracks_noise_filter/CHANGELOG.rst index c11a0fde76832..8cb85424b9874 100644 --- a/sensing/autoware_radar_tracks_noise_filter/CHANGELOG.rst +++ b/sensing/autoware_radar_tracks_noise_filter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_radar_tracks_noise_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/sensing/autoware_radar_tracks_noise_filter/package.xml b/sensing/autoware_radar_tracks_noise_filter/package.xml index d674e88e19a1a..3b95b33991640 100644 --- a/sensing/autoware_radar_tracks_noise_filter/package.xml +++ b/sensing/autoware_radar_tracks_noise_filter/package.xml @@ -2,7 +2,7 @@ autoware_radar_tracks_noise_filter - 0.40.0 + 0.41.0 autoware_radar_tracks_noise_filter Sathshi Tanaka Shunsuke Miura diff --git a/sensing/autoware_vehicle_velocity_converter/CHANGELOG.rst b/sensing/autoware_vehicle_velocity_converter/CHANGELOG.rst index 004b045a12494..e0f48fb9122e3 100644 --- a/sensing/autoware_vehicle_velocity_converter/CHANGELOG.rst +++ b/sensing/autoware_vehicle_velocity_converter/CHANGELOG.rst @@ -1,7 +1,10 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package vehicle_velocity_converter +Changelog for package autoware_vehicle_velocity_converter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/sensing/autoware_vehicle_velocity_converter/package.xml b/sensing/autoware_vehicle_velocity_converter/package.xml index cc5c555680562..9f27b5ca8d9f5 100644 --- a/sensing/autoware_vehicle_velocity_converter/package.xml +++ b/sensing/autoware_vehicle_velocity_converter/package.xml @@ -2,7 +2,7 @@ autoware_vehicle_velocity_converter - 0.40.0 + 0.41.0 The autoware_vehicle_velocity_converter package Ryu Yamamoto Apache License 2.0 diff --git a/sensing/livox/autoware_livox_tag_filter/CHANGELOG.rst b/sensing/livox/autoware_livox_tag_filter/CHANGELOG.rst index e9e42a6e4e629..b3ad06d1f3ff9 100644 --- a/sensing/livox/autoware_livox_tag_filter/CHANGELOG.rst +++ b/sensing/livox/autoware_livox_tag_filter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_livox_tag_filter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/sensing/livox/autoware_livox_tag_filter/package.xml b/sensing/livox/autoware_livox_tag_filter/package.xml index 61555e186d1e1..bcc8ae86afeb3 100644 --- a/sensing/livox/autoware_livox_tag_filter/package.xml +++ b/sensing/livox/autoware_livox_tag_filter/package.xml @@ -2,7 +2,7 @@ autoware_livox_tag_filter - 0.40.0 + 0.41.0 The autoware_livox_tag_filter package Ryohsuke Mitsudome Kenzo Lobos-Tsunekawa diff --git a/simulator/autoware_carla_interface/CHANGELOG.rst b/simulator/autoware_carla_interface/CHANGELOG.rst index d71cf66049c9d..e869e6f62fdd9 100644 --- a/simulator/autoware_carla_interface/CHANGELOG.rst +++ b/simulator/autoware_carla_interface/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package autoware_carla_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_carla_interface): fix lidar topic name (`#9645 `_) +* Contributors: Fumiya Watanabe, Maxime CLEMENT + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/simulator/autoware_carla_interface/package.xml b/simulator/autoware_carla_interface/package.xml index 1ea183cc86eb5..a8eeabc5f3398 100644 --- a/simulator/autoware_carla_interface/package.xml +++ b/simulator/autoware_carla_interface/package.xml @@ -1,7 +1,7 @@ autoware_carla_interface - 0.40.0 + 0.41.0 The carla autoware bridge package Muhammad Raditya GIOVANNI Maxime CLEMENT diff --git a/simulator/autoware_carla_interface/setup.py b/simulator/autoware_carla_interface/setup.py index 4f06766582f6a..50f6c274d6740 100644 --- a/simulator/autoware_carla_interface/setup.py +++ b/simulator/autoware_carla_interface/setup.py @@ -8,7 +8,7 @@ setup( name=package_name, - version="0.40.0", + version="0.41.0", packages=find_packages(where="src"), data_files=[ ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), diff --git a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py index a4f8dee7af1a0..9b978b66feefb 100644 --- a/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py +++ b/simulator/autoware_carla_interface/src/autoware_carla_interface/carla_ros.py @@ -147,7 +147,7 @@ def __init__(self): elif sensor["type"] == "sensor.lidar.ray_cast": if sensor["id"] in self.sensor_frequencies: self.pub_lidar[sensor["id"]] = self.ros2_node.create_publisher( - PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud', 10 + PointCloud2, f'/sensing/lidar/{sensor["id"]}/pointcloud_before_sync', 10 ) else: self.ros2_node.get_logger().info( diff --git a/simulator/dummy_perception_publisher/CHANGELOG.rst b/simulator/autoware_dummy_perception_publisher/CHANGELOG.rst similarity index 99% rename from simulator/dummy_perception_publisher/CHANGELOG.rst rename to simulator/autoware_dummy_perception_publisher/CHANGELOG.rst index 50b2396ae0311..819303dc2c3c4 100644 --- a/simulator/dummy_perception_publisher/CHANGELOG.rst +++ b/simulator/autoware_dummy_perception_publisher/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package dummy_perception_publisher -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_dummy_perception_publisher +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `dummy_perception_publisher` (`#9987 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/simulator/dummy_perception_publisher/CMakeLists.txt b/simulator/autoware_dummy_perception_publisher/CMakeLists.txt similarity index 70% rename from simulator/dummy_perception_publisher/CMakeLists.txt rename to simulator/autoware_dummy_perception_publisher/CMakeLists.txt index 0acf4694d03a6..9ac146b182a9a 100644 --- a/simulator/dummy_perception_publisher/CMakeLists.txt +++ b/simulator/autoware_dummy_perception_publisher/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(dummy_perception_publisher) +project(autoware_dummy_perception_publisher) find_package(autoware_cmake REQUIRED) autoware_package() @@ -23,19 +23,19 @@ ament_auto_add_library(signed_distance_function SHARED src/signed_distance_function.cpp ) -ament_auto_add_executable(dummy_perception_publisher_node +ament_auto_add_executable(${PROJECT_NAME}_node src/main.cpp src/node.cpp src/pointcloud_creator.cpp ) -target_link_libraries(dummy_perception_publisher_node +target_link_libraries(${PROJECT_NAME}_node signed_distance_function ) -ament_target_dependencies(dummy_perception_publisher_node ${${PROJECT_NAME}_DEPENDENCIES}) +ament_target_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_DEPENDENCIES}) -target_include_directories(dummy_perception_publisher_node +target_include_directories(${PROJECT_NAME}_node PUBLIC $ $) @@ -43,14 +43,14 @@ target_include_directories(dummy_perception_publisher_node # PCL dependencies - `ament_target_dependencies` doesn't respect the # components/modules selected above and only links in `common` ,so we need # to do this manually. -target_compile_definitions(dummy_perception_publisher_node PRIVATE ${PCL_DEFINITIONS}) -target_include_directories(dummy_perception_publisher_node PRIVATE ${PCL_INCLUDE_DIRS}) +target_compile_definitions(${PROJECT_NAME}_node PRIVATE ${PCL_DEFINITIONS}) +target_include_directories(${PROJECT_NAME}_node PRIVATE ${PCL_INCLUDE_DIRS}) # Unfortunately, this one can't be PRIVATE because only the plain or only the # keyword (PRIVATE) signature of target_link_libraries can be used for one # target, not both. The plain signature is already used inside # `ament_target_dependencies` and possibly rosidl_target_interfaces. -target_link_libraries(dummy_perception_publisher_node ${PCL_LIBRARIES}) -target_link_directories(dummy_perception_publisher_node PRIVATE ${PCL_LIBRARY_DIRS}) +target_link_libraries(${PROJECT_NAME}_node ${PCL_LIBRARIES}) +target_link_directories(${PROJECT_NAME}_node PRIVATE ${PCL_LIBRARY_DIRS}) ament_auto_add_executable(empty_objects_publisher diff --git a/simulator/dummy_perception_publisher/README.md b/simulator/autoware_dummy_perception_publisher/README.md similarity index 100% rename from simulator/dummy_perception_publisher/README.md rename to simulator/autoware_dummy_perception_publisher/README.md diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp b/simulator/autoware_dummy_perception_publisher/include/autoware/dummy_perception_publisher/node.hpp similarity index 94% rename from simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp rename to simulator/autoware_dummy_perception_publisher/include/autoware/dummy_perception_publisher/node.hpp index cf46ecddf516f..413ba4d82b836 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/node.hpp +++ b/simulator/autoware_dummy_perception_publisher/include/autoware/dummy_perception_publisher/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_ -#define DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_ +#ifndef AUTOWARE__DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_ +#define AUTOWARE__DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_ #include @@ -41,6 +41,8 @@ #include #include +namespace autoware::dummy_perception_publisher +{ struct ObjectInfo { ObjectInfo( @@ -139,4 +141,6 @@ class DummyPerceptionPublisherNode : public rclcpp::Node ~DummyPerceptionPublisherNode() {} }; -#endif // DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_ +} // namespace autoware::dummy_perception_publisher + +#endif // AUTOWARE__DUMMY_PERCEPTION_PUBLISHER__NODE_HPP_ diff --git a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp b/simulator/autoware_dummy_perception_publisher/include/autoware/dummy_perception_publisher/signed_distance_function.hpp similarity index 85% rename from simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp rename to simulator/autoware_dummy_perception_publisher/include/autoware/dummy_perception_publisher/signed_distance_function.hpp index abf24cb8dd4e6..349cbe42ec0bc 100644 --- a/simulator/dummy_perception_publisher/include/dummy_perception_publisher/signed_distance_function.hpp +++ b/simulator/autoware_dummy_perception_publisher/include/autoware/dummy_perception_publisher/signed_distance_function.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ -#define DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ +#ifndef AUTOWARE__DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ +#define AUTOWARE__DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace signed_distance_function +namespace autoware::dummy_perception_publisher { class AbstractSignedDistanceFunction @@ -69,6 +69,6 @@ class CompositeSDF : public AbstractSignedDistanceFunction std::vector> sdf_ptrs_; }; -} // namespace signed_distance_function +} // namespace autoware::dummy_perception_publisher -#endif // DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ +#endif // AUTOWARE__DUMMY_PERCEPTION_PUBLISHER__SIGNED_DISTANCE_FUNCTION_HPP_ diff --git a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml b/simulator/autoware_dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml similarity index 90% rename from simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml rename to simulator/autoware_dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml index 41025f74cbe50..ed9c38f7dc9b5 100644 --- a/simulator/dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml +++ b/simulator/autoware_dummy_perception_publisher/launch/dummy_perception_publisher.launch.xml @@ -9,7 +9,7 @@ - + @@ -30,7 +30,7 @@ - + diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/autoware_dummy_perception_publisher/package.xml similarity index 83% rename from simulator/dummy_perception_publisher/package.xml rename to simulator/autoware_dummy_perception_publisher/package.xml index 4f78acd6c07f2..e9dd3cb1614c9 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/autoware_dummy_perception_publisher/package.xml @@ -1,10 +1,11 @@ - dummy_perception_publisher - 0.40.0 - The dummy_perception_publisher package + autoware_dummy_perception_publisher + 0.41.0 + The autoware_dummy_perception_publisher package Yukihiro Saito + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp b/simulator/autoware_dummy_perception_publisher/src/empty_objects_publisher.cpp similarity index 89% rename from simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp rename to simulator/autoware_dummy_perception_publisher/src/empty_objects_publisher.cpp index 2d1ea626fb1ac..2a2a1d89ffcb8 100644 --- a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp +++ b/simulator/autoware_dummy_perception_publisher/src/empty_objects_publisher.cpp @@ -19,6 +19,9 @@ #include #include +namespace autoware::dummy_perception_publisher +{ + class EmptyObjectsPublisher : public rclcpp::Node { public: @@ -45,10 +48,12 @@ class EmptyObjectsPublisher : public rclcpp::Node } }; +} // namespace autoware::dummy_perception_publisher + int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/simulator/dummy_perception_publisher/src/main.cpp b/simulator/autoware_dummy_perception_publisher/src/main.cpp similarity index 81% rename from simulator/dummy_perception_publisher/src/main.cpp rename to simulator/autoware_dummy_perception_publisher/src/main.cpp index 5e627f2cbc8ed..224074f15f3c5 100644 --- a/simulator/dummy_perception_publisher/src/main.cpp +++ b/simulator/autoware_dummy_perception_publisher/src/main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "dummy_perception_publisher/node.hpp" +#include "autoware/dummy_perception_publisher/node.hpp" #include @@ -21,7 +21,8 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin( + std::make_shared()); rclcpp::shutdown(); return 0; diff --git a/simulator/dummy_perception_publisher/src/node.cpp b/simulator/autoware_dummy_perception_publisher/src/node.cpp similarity index 99% rename from simulator/dummy_perception_publisher/src/node.cpp rename to simulator/autoware_dummy_perception_publisher/src/node.cpp index d72b16d303cec..22797e252813c 100644 --- a/simulator/dummy_perception_publisher/src/node.cpp +++ b/simulator/autoware_dummy_perception_publisher/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "dummy_perception_publisher/node.hpp" +#include "autoware/dummy_perception_publisher/node.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" @@ -35,6 +35,9 @@ #include #include +namespace autoware::dummy_perception_publisher +{ + using autoware_perception_msgs::msg::TrackedObject; using autoware_perception_msgs::msg::TrackedObjects; @@ -428,3 +431,5 @@ void DummyPerceptionPublisherNode::objectCallback( } } } + +} // namespace autoware::dummy_perception_publisher diff --git a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp b/simulator/autoware_dummy_perception_publisher/src/pointcloud_creator.cpp similarity index 96% rename from simulator/dummy_perception_publisher/src/pointcloud_creator.cpp rename to simulator/autoware_dummy_perception_publisher/src/pointcloud_creator.cpp index 977336f63eee3..a2f84841e4ff2 100644 --- a/simulator/dummy_perception_publisher/src/pointcloud_creator.cpp +++ b/simulator/autoware_dummy_perception_publisher/src/pointcloud_creator.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "dummy_perception_publisher/node.hpp" -#include "dummy_perception_publisher/signed_distance_function.hpp" +#include "autoware/dummy_perception_publisher/node.hpp" +#include "autoware/dummy_perception_publisher/signed_distance_function.hpp" #include @@ -26,7 +26,7 @@ #include #include -namespace +namespace autoware::dummy_perception_publisher { static constexpr double epsilon = 0.001; @@ -45,8 +45,6 @@ pcl::PointXYZ getPointWrtBaseLink( return pcl::PointXYZ(p_wrt_base.x(), p_wrt_base.y(), p_wrt_base.z()); } -} // namespace - void ObjectCentricPointCloudCreator::create_object_pointcloud( const ObjectInfo & obj_info, const tf2::Transform & tf_base_link2map, std::mt19937 & random_generator, pcl::PointCloud::Ptr pointcloud) const @@ -200,13 +198,13 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr const std::vector & obj_infos, const tf2::Transform & tf_base_link2map, std::mt19937 & random_generator, pcl::PointCloud::Ptr & merged_pointcloud) const { - std::vector> sdf_ptrs; + std::vector> sdf_ptrs; for (const auto & obj_info : obj_infos) { - const auto sdf_ptr = std::make_shared( + const auto sdf_ptr = std::make_shared( obj_info.length, obj_info.width, tf_base_link2map * obj_info.tf_map2moved_object); sdf_ptrs.push_back(sdf_ptr); } - const auto composite_sdf = signed_distance_function::CompositeSDF(sdf_ptrs); + const auto composite_sdf = CompositeSDF(sdf_ptrs); std::vector::Ptr> pointclouds(obj_infos.size()); for (size_t i = 0; i < obj_infos.size(); ++i) { @@ -261,3 +259,5 @@ std::vector::Ptr> EgoCentricPointCloudCreator::cr } return pointclouds; } + +} // namespace autoware::dummy_perception_publisher diff --git a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp b/simulator/autoware_dummy_perception_publisher/src/signed_distance_function.cpp similarity index 94% rename from simulator/dummy_perception_publisher/src/signed_distance_function.cpp rename to simulator/autoware_dummy_perception_publisher/src/signed_distance_function.cpp index 01bef1851b25e..c6a720e33fefa 100644 --- a/simulator/dummy_perception_publisher/src/signed_distance_function.cpp +++ b/simulator/autoware_dummy_perception_publisher/src/signed_distance_function.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "dummy_perception_publisher/signed_distance_function.hpp" +#include "autoware/dummy_perception_publisher/signed_distance_function.hpp" #include @@ -20,7 +20,7 @@ #include #include -namespace signed_distance_function +namespace autoware::dummy_perception_publisher { double AbstractSignedDistanceFunction::getSphereTracingDist( @@ -89,4 +89,4 @@ size_t CompositeSDF::nearest_sdf_index(double x, double y) const return idx_min; } -} // namespace signed_distance_function +} // namespace autoware::dummy_perception_publisher diff --git a/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp b/simulator/autoware_dummy_perception_publisher/test/src/test_signed_distance_function.cpp similarity index 82% rename from simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp rename to simulator/autoware_dummy_perception_publisher/test/src/test_signed_distance_function.cpp index 5b8dcee549ea7..e7bb667a1dec2 100644 --- a/simulator/dummy_perception_publisher/test/src/test_signed_distance_function.cpp +++ b/simulator/autoware_dummy_perception_publisher/test/src/test_signed_distance_function.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "dummy_perception_publisher/signed_distance_function.hpp" +#include "autoware/dummy_perception_publisher/signed_distance_function.hpp" #include #include @@ -23,7 +23,8 @@ #include #include -namespace sdf = signed_distance_function; +namespace autoware::dummy_perception_publisher +{ TEST(SignedDistanceFunctionTest, BoxSDF) { @@ -33,7 +34,7 @@ TEST(SignedDistanceFunctionTest, BoxSDF) // test with identity transform const auto q = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), 0.0); const auto tf_global2local = tf2::Transform(q); - const auto func = sdf::BoxSDF(1., 2., tf_global2local); + const auto func = BoxSDF(1., 2., tf_global2local); ASSERT_NEAR(func(0.0, 0.0), -0.5, eps); ASSERT_NEAR(func(0.0, 1.0), 0.0, eps); ASSERT_NEAR(func(0.0, 1.5), 0.5, eps); @@ -50,7 +51,7 @@ TEST(SignedDistanceFunctionTest, BoxSDF) // test with rotation (90 deg) and translation const auto q = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), M_PI * 0.5); const auto tf_global2local = tf2::Transform(q, tf2::Vector3(1.0, 1.0, 0.0)); - const auto func = sdf::BoxSDF(1., 2., tf_global2local); + const auto func = BoxSDF(1., 2., tf_global2local); ASSERT_NEAR(func(1.0, 1.0), -0.5, eps); ASSERT_NEAR(func(0.0, 0.0), 0.5, eps); @@ -63,11 +64,11 @@ TEST(SignedDistanceFunctionTest, CompositeSDF) const double eps = 1e-5; const auto q_identity = tf2::Quaternion(tf2::Vector3(0, 0, 1.0), 0.0); const auto f1 = - std::make_shared(1., 1., tf2::Transform(q_identity, tf2::Vector3(0, 0, 0))); + std::make_shared(1., 1., tf2::Transform(q_identity, tf2::Vector3(0, 0, 0))); const auto f2 = - std::make_shared(1., 1., tf2::Transform(q_identity, tf2::Vector3(0, 2.0, 0))); + std::make_shared(1., 1., tf2::Transform(q_identity, tf2::Vector3(0, 2.0, 0))); const auto func = - sdf::CompositeSDF(std::vector>{f1, f2}); + CompositeSDF(std::vector>{f1, f2}); ASSERT_NEAR(func(0.0, 0.9), 0.4, eps); ASSERT_NEAR(func(0.0, 1.1), 0.4, eps); ASSERT_NEAR(func(0.0, 0.1), -0.4, eps); @@ -77,6 +78,8 @@ TEST(SignedDistanceFunctionTest, CompositeSDF) // ASSERT_NEAR(func.getSphereTracingDist(0.0, 1.0, M_PI * -0.5, eps), 0.5, eps); } +} // namespace autoware::dummy_perception_publisher + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/simulator/fault_injection/CHANGELOG.rst b/simulator/autoware_fault_injection/CHANGELOG.rst similarity index 92% rename from simulator/fault_injection/CHANGELOG.rst rename to simulator/autoware_fault_injection/CHANGELOG.rst index 6116bafaa2088..132cdb733c271 100644 --- a/simulator/fault_injection/CHANGELOG.rst +++ b/simulator/autoware_fault_injection/CHANGELOG.rst @@ -1,7 +1,27 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package fault_injection +Changelog for package autoware_fault_injection ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `fault_injection` (`#9989 `_) + * feat(fault_injection): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(fault_injection): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(fault_injection): fix include header paths + * To follow the previous commit + * rename: `fault_injection` => `autoware_fault_injection` + * Fixed exec_depend + --------- + Co-authored-by: SakodaShintaro +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/simulator/fault_injection/CMakeLists.txt b/simulator/autoware_fault_injection/CMakeLists.txt similarity index 74% rename from simulator/fault_injection/CMakeLists.txt rename to simulator/autoware_fault_injection/CMakeLists.txt index 11bf03facd67f..16f8518609f82 100644 --- a/simulator/fault_injection/CMakeLists.txt +++ b/simulator/autoware_fault_injection/CMakeLists.txt @@ -1,18 +1,18 @@ cmake_minimum_required(VERSION 3.14) -project(fault_injection) +project(autoware_fault_injection) find_package(autoware_cmake REQUIRED) autoware_package() find_package(pluginlib REQUIRED) -ament_auto_add_library(fault_injection_node_component SHARED +ament_auto_add_library(${PROJECT_NAME}_node_component SHARED src/fault_injection_node/fault_injection_node.cpp ) -rclcpp_components_register_node(fault_injection_node_component - PLUGIN "fault_injection::FaultInjectionNode" - EXECUTABLE fault_injection_node +rclcpp_components_register_node(${PROJECT_NAME}_node_component + PLUGIN "autoware::simulator::fault_injection::FaultInjectionNode" + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) @@ -23,7 +23,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_fault_injection_node_component - fault_injection_node_component + ${PROJECT_NAME}_node_component ) # launch_testing diff --git a/simulator/fault_injection/README.md b/simulator/autoware_fault_injection/README.md similarity index 100% rename from simulator/fault_injection/README.md rename to simulator/autoware_fault_injection/README.md diff --git a/simulator/fault_injection/config/fault_injection.param.yaml b/simulator/autoware_fault_injection/config/fault_injection.param.yaml similarity index 100% rename from simulator/fault_injection/config/fault_injection.param.yaml rename to simulator/autoware_fault_injection/config/fault_injection.param.yaml diff --git a/simulator/fault_injection/img/component.drawio.svg b/simulator/autoware_fault_injection/img/component.drawio.svg similarity index 100% rename from simulator/fault_injection/img/component.drawio.svg rename to simulator/autoware_fault_injection/img/component.drawio.svg diff --git a/simulator/fault_injection/include/fault_injection/diagnostic_storage.hpp b/simulator/autoware_fault_injection/include/autoware/fault_injection/diagnostic_storage.hpp similarity index 85% rename from simulator/fault_injection/include/fault_injection/diagnostic_storage.hpp rename to simulator/autoware_fault_injection/include/autoware/fault_injection/diagnostic_storage.hpp index a6c6b816c99f7..e6522188b71df 100644 --- a/simulator/fault_injection/include/fault_injection/diagnostic_storage.hpp +++ b/simulator/autoware_fault_injection/include/autoware/fault_injection/diagnostic_storage.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP_ -#define FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP_ +#ifndef AUTOWARE__FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP_ +#define AUTOWARE__FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP_ #include @@ -22,7 +22,7 @@ #include #include -namespace fault_injection +namespace autoware::simulator::fault_injection { using diagnostic_msgs::msg::DiagnosticStatus; @@ -65,6 +65,6 @@ class DiagnosticStorage std::unordered_map event_diag_map_; }; -} // namespace fault_injection +} // namespace autoware::simulator::fault_injection -#endif // FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP_ +#endif // AUTOWARE__FAULT_INJECTION__DIAGNOSTIC_STORAGE_HPP_ diff --git a/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp b/simulator/autoware_fault_injection/include/autoware/fault_injection/fault_injection_diag_updater.hpp similarity index 95% rename from simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp rename to simulator/autoware_fault_injection/include/autoware/fault_injection/fault_injection_diag_updater.hpp index 0a71c1681d1d3..7d6b553c35999 100644 --- a/simulator/fault_injection/include/fault_injection/fault_injection_diag_updater.hpp +++ b/simulator/autoware_fault_injection/include/autoware/fault_injection/fault_injection_diag_updater.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -46,8 +46,8 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -#ifndef FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ -#define FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ +#ifndef AUTOWARE__FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ +#define AUTOWARE__FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ #include @@ -57,7 +57,7 @@ #include #include -namespace fault_injection +namespace autoware::simulator::fault_injection { class FaultInjectionDiagUpdater : public diagnostic_updater::DiagnosticTaskVector { @@ -240,6 +240,6 @@ class FaultInjectionDiagUpdater : public diagnostic_updater::DiagnosticTaskVecto std::string hardware_id_; std::string node_name_; }; -} // namespace fault_injection +} // namespace autoware::simulator::fault_injection -#endif // FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ +#endif // AUTOWARE__FAULT_INJECTION__FAULT_INJECTION_DIAG_UPDATER_HPP_ diff --git a/simulator/fault_injection/include/fault_injection/fault_injection_node.hpp b/simulator/autoware_fault_injection/include/autoware/fault_injection/fault_injection_node.hpp similarity index 74% rename from simulator/fault_injection/include/fault_injection/fault_injection_node.hpp rename to simulator/autoware_fault_injection/include/autoware/fault_injection/fault_injection_node.hpp index 999d18b38c7b0..b423c9e5e56df 100644 --- a/simulator/fault_injection/include/fault_injection/fault_injection_node.hpp +++ b/simulator/autoware_fault_injection/include/autoware/fault_injection/fault_injection_node.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FAULT_INJECTION__FAULT_INJECTION_NODE_HPP_ -#define FAULT_INJECTION__FAULT_INJECTION_NODE_HPP_ +#ifndef AUTOWARE__FAULT_INJECTION__FAULT_INJECTION_NODE_HPP_ +#define AUTOWARE__FAULT_INJECTION__FAULT_INJECTION_NODE_HPP_ -#include "fault_injection/diagnostic_storage.hpp" -#include "fault_injection/fault_injection_diag_updater.hpp" +#include "autoware/fault_injection/diagnostic_storage.hpp" +#include "autoware/fault_injection/fault_injection_diag_updater.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace fault_injection +namespace autoware::simulator::fault_injection { using tier4_simulation_msgs::msg::SimulationEvents; @@ -49,6 +49,6 @@ class FaultInjectionNode : public rclcpp::Node DiagnosticStorage diagnostic_storage_; }; -} // namespace fault_injection +} // namespace autoware::simulator::fault_injection -#endif // FAULT_INJECTION__FAULT_INJECTION_NODE_HPP_ +#endif // AUTOWARE__FAULT_INJECTION__FAULT_INJECTION_NODE_HPP_ diff --git a/simulator/autoware_fault_injection/launch/fault_injection.launch.xml b/simulator/autoware_fault_injection/launch/fault_injection.launch.xml new file mode 100644 index 0000000000000..9c521acc52794 --- /dev/null +++ b/simulator/autoware_fault_injection/launch/fault_injection.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/simulator/fault_injection/package.xml b/simulator/autoware_fault_injection/package.xml similarity index 85% rename from simulator/fault_injection/package.xml rename to simulator/autoware_fault_injection/package.xml index feedcf3333f7f..a0479ff49ee50 100644 --- a/simulator/fault_injection/package.xml +++ b/simulator/autoware_fault_injection/package.xml @@ -1,10 +1,11 @@ - fault_injection - 0.40.0 - fault_injection + autoware_fault_injection + 0.41.0 + The fault_injection package Keisuke Shima + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp b/simulator/autoware_fault_injection/src/fault_injection_node/fault_injection_node.cpp similarity index 93% rename from simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp rename to simulator/autoware_fault_injection/src/fault_injection_node/fault_injection_node.cpp index 467054dcda8fe..5e65b4bdcc327 100644 --- a/simulator/fault_injection/src/fault_injection_node/fault_injection_node.cpp +++ b/simulator/autoware_fault_injection/src/fault_injection_node/fault_injection_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fault_injection/fault_injection_node.hpp" +#include "autoware/fault_injection/fault_injection_node.hpp" #include @@ -21,7 +21,7 @@ #include #include -namespace +namespace autoware::simulator::fault_injection { std::vector split(const std::string & str, const char delim) { @@ -33,10 +33,7 @@ std::vector split(const std::string & str, const char delim) } return elems; } -} // namespace -namespace fault_injection -{ #ifdef ROS_DISTRO_GALACTIC using rosidl_generator_traits::to_yaml; #endif @@ -101,7 +98,7 @@ std::vector FaultInjectionNode::read_event_diag_list() return diag_configs; } -} // namespace fault_injection +} // namespace autoware::simulator::fault_injection #include -RCLCPP_COMPONENTS_REGISTER_NODE(fault_injection::FaultInjectionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::simulator::fault_injection::FaultInjectionNode) diff --git a/simulator/fault_injection/test/config/test_event_diag.param.yaml b/simulator/autoware_fault_injection/test/config/test_event_diag.param.yaml similarity index 100% rename from simulator/fault_injection/test/config/test_event_diag.param.yaml rename to simulator/autoware_fault_injection/test/config/test_event_diag.param.yaml diff --git a/simulator/autoware_fault_injection/test/launch/test_fault_injection.launch.xml b/simulator/autoware_fault_injection/test/launch/test_fault_injection.launch.xml new file mode 100644 index 0000000000000..76488d625dbe4 --- /dev/null +++ b/simulator/autoware_fault_injection/test/launch/test_fault_injection.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/simulator/fault_injection/test/src/main.cpp b/simulator/autoware_fault_injection/test/src/main.cpp similarity index 95% rename from simulator/fault_injection/test/src/main.cpp rename to simulator/autoware_fault_injection/test/src/main.cpp index bc077b74c48f5..dbd4824169399 100644 --- a/simulator/fault_injection/test/src/main.cpp +++ b/simulator/autoware_fault_injection/test/src/main.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/simulator/fault_injection/test/src/test_diagnostic_storage.cpp b/simulator/autoware_fault_injection/test/src/test_diagnostic_storage.cpp similarity index 82% rename from simulator/fault_injection/test/src/test_diagnostic_storage.cpp rename to simulator/autoware_fault_injection/test/src/test_diagnostic_storage.cpp index 311401df37e59..7d9ea6145bcc8 100644 --- a/simulator/fault_injection/test/src/test_diagnostic_storage.cpp +++ b/simulator/autoware_fault_injection/test/src/test_diagnostic_storage.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2025 Tier IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "fault_injection/diagnostic_storage.hpp" +#include "autoware/fault_injection/diagnostic_storage.hpp" #include @@ -20,6 +20,9 @@ #include #include +namespace autoware::simulator::fault_injection +{ + class TestDiagnosticStorage : public ::testing::Test { protected: @@ -30,8 +33,8 @@ class TestDiagnosticStorage : public ::testing::Test } } - fault_injection::DiagnosticStorage storage_; - const std::vector diag_config{ + DiagnosticStorage storage_; + const std::vector diag_config{ {"foo", "foo_diag"}, }; }; @@ -52,3 +55,5 @@ TEST_F(TestDiagnosticStorage, raise_exception_with_wrong_key) { EXPECT_ANY_THROW(storage_.getDiag("invalid_name")); } + +} // namespace autoware::simulator::fault_injection diff --git a/simulator/fault_injection/test/test_fault_injection_node.test.py b/simulator/autoware_fault_injection/test/test_fault_injection_node.test.py similarity index 98% rename from simulator/fault_injection/test/test_fault_injection_node.test.py rename to simulator/autoware_fault_injection/test/test_fault_injection_node.test.py index 1437d69b1c91b..a9516a531479f 100644 --- a/simulator/fault_injection/test/test_fault_injection_node.test.py +++ b/simulator/autoware_fault_injection/test/test_fault_injection_node.test.py @@ -1,4 +1,4 @@ -# Copyright 2021 Tier IV, Inc. +# Copyright 2025 Tier IV, Inc. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -35,7 +35,7 @@ @pytest.mark.launch_test def generate_test_description(): test_fault_injection_launch_file = os.path.join( - get_package_share_directory("fault_injection"), + get_package_share_directory("autoware_fault_injection"), "launch", "test_fault_injection.launch.xml", ) diff --git a/simulator/learning_based_vehicle_model/CHANGELOG.rst b/simulator/autoware_learning_based_vehicle_model/CHANGELOG.rst similarity index 88% rename from simulator/learning_based_vehicle_model/CHANGELOG.rst rename to simulator/autoware_learning_based_vehicle_model/CHANGELOG.rst index ed271370a6840..82757278b719d 100644 --- a/simulator/learning_based_vehicle_model/CHANGELOG.rst +++ b/simulator/autoware_learning_based_vehicle_model/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package learning_based_vehicle_model -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_learning_based_vehicle_model +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `learning_based_vehicle_model` (`#9991 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/simulator/learning_based_vehicle_model/CMakeLists.txt b/simulator/autoware_learning_based_vehicle_model/CMakeLists.txt similarity index 92% rename from simulator/learning_based_vehicle_model/CMakeLists.txt rename to simulator/autoware_learning_based_vehicle_model/CMakeLists.txt index 58a3ad57e6f9e..02159e7d282bc 100644 --- a/simulator/learning_based_vehicle_model/CMakeLists.txt +++ b/simulator/autoware_learning_based_vehicle_model/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14.4) -project(learning_based_vehicle_model) +project(autoware_learning_based_vehicle_model) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/simulator/learning_based_vehicle_model/README.md b/simulator/autoware_learning_based_vehicle_model/README.md similarity index 100% rename from simulator/learning_based_vehicle_model/README.md rename to simulator/autoware_learning_based_vehicle_model/README.md diff --git a/simulator/learning_based_vehicle_model/image/python_model_interface.png b/simulator/autoware_learning_based_vehicle_model/image/python_model_interface.png similarity index 100% rename from simulator/learning_based_vehicle_model/image/python_model_interface.png rename to simulator/autoware_learning_based_vehicle_model/image/python_model_interface.png diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp b/simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/interconnected_model.hpp similarity index 86% rename from simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp rename to simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/interconnected_model.hpp index 56a196c257189..b08a8957dff7d 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/interconnected_model.hpp +++ b/simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/interconnected_model.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ -#define LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ +#ifndef AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ +#define AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ -#include "learning_based_vehicle_model/model_connections_helpers.hpp" -#include "learning_based_vehicle_model/simple_pymodel.hpp" -#include "learning_based_vehicle_model/submodel_interface.hpp" +#include "autoware/learning_based_vehicle_model/model_connections_helpers.hpp" +#include "autoware/learning_based_vehicle_model/simple_pymodel.hpp" +#include "autoware/learning_based_vehicle_model/submodel_interface.hpp" #include #include @@ -29,6 +29,9 @@ #include #include +namespace autoware::simulator::learning_based_vehicle_model +{ + namespace py = pybind11; class __attribute__((visibility("default"))) InterconnectedModel @@ -127,4 +130,6 @@ class __attribute__((visibility("default"))) InterconnectedModel std::vector updatePyModel(std::vector psim_input); }; -#endif // LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ +} // namespace autoware::simulator::learning_based_vehicle_model + +#endif // AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__INTERCONNECTED_MODEL_HPP_ diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp b/simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/model_connections_helpers.hpp similarity index 68% rename from simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp rename to simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/model_connections_helpers.hpp index 0d46169600b2f..94d206fece130 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/model_connections_helpers.hpp +++ b/simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/model_connections_helpers.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ -#define LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#ifndef AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +#define AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ #include #include +namespace autoware::simulator::learning_based_vehicle_model +{ + std::vector fillVectorUsingMap( std::vector vector1, std::vector vector2, const std::vector & map, bool inverse); @@ -25,4 +28,6 @@ std::vector fillVectorUsingMap( std::vector createConnectionsMap( const std::vector & connection_names_1, const std::vector & connection_names_2); -#endif // LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ +} // namespace autoware::simulator::learning_based_vehicle_model + +#endif // AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__MODEL_CONNECTIONS_HELPERS_HPP_ diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp b/simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/simple_pymodel.hpp similarity index 85% rename from simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp rename to simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/simple_pymodel.hpp index fd9cf5cf44de9..270a30b5e23dd 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/simple_pymodel.hpp +++ b/simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/simple_pymodel.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -13,11 +13,11 @@ // limitations under the License. // cspell:ignore pymodel -#ifndef LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ -#define LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ +#ifndef AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ +#define AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ -#include "learning_based_vehicle_model/model_connections_helpers.hpp" -#include "learning_based_vehicle_model/submodel_interface.hpp" +#include "autoware/learning_based_vehicle_model/model_connections_helpers.hpp" +#include "autoware/learning_based_vehicle_model/submodel_interface.hpp" #include #include @@ -25,6 +25,9 @@ #include #include +namespace autoware::simulator::learning_based_vehicle_model +{ + namespace py = pybind11; /** @@ -99,4 +102,6 @@ class SimplePyModel : public SubModelInterface void mapOutputs(std::vector signals_vec_names) override; }; -#endif // LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ +} // namespace autoware::simulator::learning_based_vehicle_model + +#endif // AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__SIMPLE_PYMODEL_HPP_ diff --git a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp b/simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/submodel_interface.hpp similarity index 82% rename from simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp rename to simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/submodel_interface.hpp index 1cbad103278fb..f97158aadcd2b 100644 --- a/simulator/learning_based_vehicle_model/include/learning_based_vehicle_model/submodel_interface.hpp +++ b/simulator/autoware_learning_based_vehicle_model/include/autoware/learning_based_vehicle_model/submodel_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,11 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ -#define LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ +#ifndef AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ +#define AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ #include +namespace autoware::simulator::learning_based_vehicle_model +{ + class SubModelInterface { public: @@ -59,4 +62,6 @@ class SubModelInterface std::vector model_signals_vec, std::vector model_signals_vec_next) = 0; }; -#endif // LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ +} // namespace autoware::simulator::learning_based_vehicle_model + +#endif // AUTOWARE__LEARNING_BASED_VEHICLE_MODEL__SUBMODEL_INTERFACE_HPP_ diff --git a/simulator/learning_based_vehicle_model/package.xml b/simulator/autoware_learning_based_vehicle_model/package.xml similarity index 75% rename from simulator/learning_based_vehicle_model/package.xml rename to simulator/autoware_learning_based_vehicle_model/package.xml index 317be3d0990ca..6f48df3364a76 100644 --- a/simulator/learning_based_vehicle_model/package.xml +++ b/simulator/autoware_learning_based_vehicle_model/package.xml @@ -1,11 +1,12 @@ - learning_based_vehicle_model - 0.40.0 - learning_based_vehicle_model for simple_planning_simulator + autoware_learning_based_vehicle_model + 0.41.0 + autoware_learning_based_vehicle_model for simple_planning_simulator Maxime Clement Tomas Nagy + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp b/simulator/autoware_learning_based_vehicle_model/src/interconnected_model.cpp similarity index 93% rename from simulator/learning_based_vehicle_model/src/interconnected_model.cpp rename to simulator/autoware_learning_based_vehicle_model/src/interconnected_model.cpp index d2fef15aa4e88..1f92e8cd058a5 100644 --- a/simulator/learning_based_vehicle_model/src/interconnected_model.cpp +++ b/simulator/autoware_learning_based_vehicle_model/src/interconnected_model.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "learning_based_vehicle_model/interconnected_model.hpp" +#include "autoware/learning_based_vehicle_model/interconnected_model.hpp" #include #include #include +namespace autoware::simulator::learning_based_vehicle_model +{ + void InterconnectedModel::mapInputs(std::vector in_names) { // index in "map_in_to_sig_vec" is index in "in_names" and value in "map_in_to_sig_vec" is index @@ -79,8 +82,7 @@ void InterconnectedModel::addSubmodel( std::tuple submodel_desc) { const auto [lib_path, param_path, class_name] = submodel_desc; - auto new_model = new SimplePyModel(lib_path, param_path, class_name); - submodels.push_back(std::unique_ptr(new_model)); + submodels.emplace_back(std::make_unique(lib_path, param_path, class_name)); } void InterconnectedModel::initState(std::vector new_state) @@ -140,3 +142,5 @@ std::vector InterconnectedModel::updatePyModel(std::vector psim_ return psim_next_state; } + +} // namespace autoware::simulator::learning_based_vehicle_model diff --git a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp b/simulator/autoware_learning_based_vehicle_model/src/model_connections_helpers.cpp similarity index 88% rename from simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp rename to simulator/autoware_learning_based_vehicle_model/src/model_connections_helpers.cpp index b300f3837b18a..284607744e218 100644 --- a/simulator/learning_based_vehicle_model/src/model_connections_helpers.cpp +++ b/simulator/autoware_learning_based_vehicle_model/src/model_connections_helpers.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "learning_based_vehicle_model/model_connections_helpers.hpp" +#include "autoware/learning_based_vehicle_model/model_connections_helpers.hpp" #include +namespace autoware::simulator::learning_based_vehicle_model +{ + std::vector fillVectorUsingMap( std::vector vector1, std::vector vector2, const std::vector & map, bool inverse) @@ -48,3 +51,5 @@ std::vector createConnectionsMap( } return connection_map; } + +} // namespace autoware::simulator::learning_based_vehicle_model diff --git a/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp b/simulator/autoware_learning_based_vehicle_model/src/simple_pymodel.cpp similarity index 94% rename from simulator/learning_based_vehicle_model/src/simple_pymodel.cpp rename to simulator/autoware_learning_based_vehicle_model/src/simple_pymodel.cpp index 10581179baf4a..50ceb14ff90e6 100644 --- a/simulator/learning_based_vehicle_model/src/simple_pymodel.cpp +++ b/simulator/autoware_learning_based_vehicle_model/src/simple_pymodel.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "learning_based_vehicle_model/simple_pymodel.hpp" +#include "autoware/learning_based_vehicle_model/simple_pymodel.hpp" #include #include @@ -20,6 +20,9 @@ #include #include +namespace autoware::simulator::learning_based_vehicle_model +{ + namespace py = pybind11; SimplePyModel::SimplePyModel( @@ -110,3 +113,5 @@ void SimplePyModel::mapOutputs(std::vector signals_vec_names) // "map_py_model_outputs_to_sig_vec" is index in "signals_vec_names" map_py_model_outputs_to_sig_vec = createConnectionsMap(signals_vec_names, py_model_state_names); } + +} // namespace autoware::simulator::learning_based_vehicle_model diff --git a/simulator/learning_based_vehicle_model/src/submodel_interface.cpp b/simulator/autoware_learning_based_vehicle_model/src/submodel_interface.cpp similarity index 82% rename from simulator/learning_based_vehicle_model/src/submodel_interface.cpp rename to simulator/autoware_learning_based_vehicle_model/src/submodel_interface.cpp index 906aff4b01ae0..cc525b990801b 100644 --- a/simulator/learning_based_vehicle_model/src/submodel_interface.cpp +++ b/simulator/autoware_learning_based_vehicle_model/src/submodel_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,4 +12,4 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "learning_based_vehicle_model/submodel_interface.hpp" +#include "autoware/learning_based_vehicle_model/submodel_interface.hpp" diff --git a/simulator/simple_planning_simulator/CHANGELOG.rst b/simulator/autoware_simple_planning_simulator/CHANGELOG.rst similarity index 97% rename from simulator/simple_planning_simulator/CHANGELOG.rst rename to simulator/autoware_simple_planning_simulator/CHANGELOG.rst index ac6e093ea55e7..8e36793315331 100644 --- a/simulator/simple_planning_simulator/CHANGELOG.rst +++ b/simulator/autoware_simple_planning_simulator/CHANGELOG.rst @@ -1,7 +1,36 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package simple_planning_simulator +Changelog for package autoware_simple_planning_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `learning_based_vehicle_model` (`#9991 `_) +* feat: apply `autoware\_` prefix for `simple_planning_simulator` (`#9995 `_) + * feat(simple_planning_simulator): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(simple_planning_simulator): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(simple_planning_simulator): fix include header paths + * To follow the previous commit + * rename: `simple_planning_simulator` => `autoware_simple_planning_simulator` + * bug(autoware_simple_planning_simulator): fix missing changes + * style(pre-commit): autofix + * bug(autoware_simple_planning_simulator): fix errors in build and tests + * I had to run after `rm -rf install build`, ... sorry + * style(pre-commit): autofix + * Fixed NOLINT + * Added NOLINT + * Fixed to autoware_simple_planning_simulator + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Shintaro Sakoda +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/autoware_simple_planning_simulator/CMakeLists.txt similarity index 79% rename from simulator/simple_planning_simulator/CMakeLists.txt rename to simulator/autoware_simple_planning_simulator/CMakeLists.txt index 6acb74342c90f..18f2486796d96 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/autoware_simple_planning_simulator/CMakeLists.txt @@ -1,15 +1,15 @@ cmake_minimum_required(VERSION 3.14) -project(simple_planning_simulator) +project(autoware_simple_planning_simulator) find_package(autoware_cmake REQUIRED) find_package(Python3 COMPONENTS Interpreter Development) -find_package(learning_based_vehicle_model REQUIRED) +find_package(autoware_learning_based_vehicle_model REQUIRED) autoware_package() # Component ament_auto_add_library(${PROJECT_NAME} SHARED - include/simple_planning_simulator/simple_planning_simulator_core.hpp - include/simple_planning_simulator/visibility_control.hpp + include/autoware/simple_planning_simulator/simple_planning_simulator_core.hpp + include/autoware/simple_planning_simulator/visibility_control.hpp src/simple_planning_simulator/simple_planning_simulator_core.cpp src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp @@ -25,11 +25,11 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/simple_planning_simulator/utils/csv_loader.cpp src/simple_planning_simulator/utils/mechanical_controller.cpp ) -target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${learning_based_vehicle_model_INCLUDE_DIRS}) +target_include_directories(${PROJECT_NAME} PUBLIC ${Python3_INCLUDE_DIRS} ${autoware_learning_based_vehicle_model_INCLUDE_DIRS}) # Node executable rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "simulation::simple_planning_simulator::SimplePlanningSimulator" - EXECUTABLE ${PROJECT_NAME}_exe + PLUGIN "autoware::simulator::simple_planning_simulator::SimplePlanningSimulator" + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/autoware_simple_planning_simulator/README.md similarity index 99% rename from simulator/simple_planning_simulator/README.md rename to simulator/autoware_simple_planning_simulator/README.md index cd362c0115358..89fe55af1c3ff 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/autoware_simple_planning_simulator/README.md @@ -1,4 +1,4 @@ -# simple_planning_simulator +# autoware_simple_planning_simulator ## Purpose / Use cases @@ -64,7 +64,7 @@ The purpose of this simulator is for the integration test of planning and contro - `DELAY_STEER_ACC_GEARED` - `DELAY_STEER_ACC_GEARED_WO_FALL_GUARD` - `DELAY_STEER_MAP_ACC_GEARED`: applies 1D dynamics and time delay to the steering and acceleration commands. The simulated acceleration is determined by a value converted through the provided acceleration map. This model is valuable for an accurate simulation with acceleration deviations in a real vehicle. -- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../learning_based_vehicle_model). +- `LEARNED_STEER_VEL`: launches learned python models. More about this [here](../autoware_learning_based_vehicle_model). The following models receive `ACTUATION_CMD` generated from `raw_vehicle_cmd_converter`. Therefore, when these models are selected, the `raw_vehicle_cmd_converter` is also launched. diff --git a/simulator/simple_planning_simulator/data/actuation_cmd_map/accel_map.csv b/simulator/autoware_simple_planning_simulator/data/actuation_cmd_map/accel_map.csv similarity index 100% rename from simulator/simple_planning_simulator/data/actuation_cmd_map/accel_map.csv rename to simulator/autoware_simple_planning_simulator/data/actuation_cmd_map/accel_map.csv diff --git a/simulator/simple_planning_simulator/data/actuation_cmd_map/brake_map.csv b/simulator/autoware_simple_planning_simulator/data/actuation_cmd_map/brake_map.csv similarity index 100% rename from simulator/simple_planning_simulator/data/actuation_cmd_map/brake_map.csv rename to simulator/autoware_simple_planning_simulator/data/actuation_cmd_map/brake_map.csv diff --git a/simulator/simple_planning_simulator/data/actuation_cmd_map/steer_map.csv b/simulator/autoware_simple_planning_simulator/data/actuation_cmd_map/steer_map.csv similarity index 100% rename from simulator/simple_planning_simulator/data/actuation_cmd_map/steer_map.csv rename to simulator/autoware_simple_planning_simulator/data/actuation_cmd_map/steer_map.csv diff --git a/simulator/simple_planning_simulator/docs/actuation_cmd_sim.md b/simulator/autoware_simple_planning_simulator/docs/actuation_cmd_sim.md similarity index 100% rename from simulator/simple_planning_simulator/docs/actuation_cmd_sim.md rename to simulator/autoware_simple_planning_simulator/docs/actuation_cmd_sim.md diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/simple_planning_simulator_core.hpp similarity index 96% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/simple_planning_simulator_core.hpp index 3e54bca245f82..26a849dddc278 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/visibility_control.hpp" #include "rclcpp/rclcpp.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "simple_planning_simulator/visibility_control.hpp" #include "tier4_api_utils/tier4_api_utils.hpp" #include "autoware_control_msgs/msg/control.hpp" @@ -57,9 +57,7 @@ #include #include -namespace simulation -{ -namespace simple_planning_simulator +namespace autoware::simulator::simple_planning_simulator { using autoware_control_msgs::msg::Control; @@ -406,7 +404,6 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void publish_tf(const Odometry & odometry); }; -} // namespace simple_planning_simulator -} // namespace simulation +} // namespace autoware::simulator::simple_planning_simulator -#endif // SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/utils/csv_loader.hpp similarity index 78% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/utils/csv_loader.hpp index a8851640fb62b..2965327beaacc 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/csv_loader.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/utils/csv_loader.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Autoware Foundation +// Copyright 2025 Autoware Foundation // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ #include #include @@ -21,6 +21,9 @@ #include #include +namespace autoware::simulator::simple_planning_simulator +{ + using Table = std::vector>; using Map = std::vector>; class CSVLoader @@ -40,4 +43,6 @@ class CSVLoader std::string csv_path_; }; -#endif // SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__UTILS__CSV_LOADER_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/utils/mechanical_controller.hpp similarity index 92% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/utils/mechanical_controller.hpp index fcb8d41dd72b5..7850d74d45036 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/utils/mechanical_controller.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/utils/mechanical_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_ #include #include @@ -21,7 +21,7 @@ #include #include -namespace autoware::simple_planning_simulator::utils::mechanical_controller +namespace autoware::simulator::simple_planning_simulator { using DelayBuffer = std::deque>; @@ -203,6 +203,6 @@ class MechanicalController const SteeringDynamics & dynamics) const; }; -} // namespace autoware::simple_planning_simulator::utils::mechanical_controller +} // namespace autoware::simulator::simple_planning_simulator -#endif // SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_ +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__UTILS__MECHANICAL_CONTROLLER_HPP_ diff --git a/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model.hpp new file mode 100644 index 0000000000000..d7ee90fdf5634 --- /dev/null +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -0,0 +1,30 @@ +// Copyright 2025 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__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ + +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp similarity index 94% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp index 25fac72d283ee..edb52570b0a19 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ #include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/simple_planning_simulator/utils/csv_loader.hpp" +#include "autoware/simple_planning_simulator/utils/mechanical_controller.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "simple_planning_simulator/utils/csv_loader.hpp" -#include "simple_planning_simulator/utils/mechanical_controller.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include @@ -30,8 +30,11 @@ #include #include -using autoware::simple_planning_simulator::utils::mechanical_controller::MechanicalController; -using autoware::simple_planning_simulator::utils::mechanical_controller::MechanicalParams; +namespace autoware::simulator::simple_planning_simulator +{ + +using autoware::simulator::simple_planning_simulator::MechanicalController; +using autoware::simulator::simple_planning_simulator::MechanicalParams; /** * @class ActuationMap @@ -397,4 +400,6 @@ class SimModelActuationCmdMechanical : public SimModelActuationCmdVGR double prev_steer_tire_des_{0.0}; // }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_ACTUATION_CMD_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp similarity index 90% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index f43abd8572874..6437cdc4cdbcb 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include @@ -24,6 +24,9 @@ #include #include +namespace autoware::simulator::simple_planning_simulator +{ + class SimModelDelaySteerAcc : public SimModelInterface { public: @@ -149,4 +152,6 @@ class SimModelDelaySteerAcc : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp similarity index 89% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 1ecb74be41780..d43c86669f2bf 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ // NOLINT +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ // NOLINT -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include @@ -24,6 +24,9 @@ #include #include +namespace autoware::simulator::simple_planning_simulator +{ + class SimModelDelaySteerAccGeared : public SimModelInterface { public: @@ -160,4 +163,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface const double dt); }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +// NOLINTNEXTLINE +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp similarity index 88% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp index 83f574554fe76..300e82b6facb1 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include @@ -24,6 +24,9 @@ #include #include +namespace autoware::simulator::simple_planning_simulator +{ + class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface { public: @@ -145,6 +148,7 @@ class SimModelDelaySteerAccGearedWoFallGuard : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -// clang-format off -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ // NOLINT -// clang-format on +} // namespace autoware::simulator::simple_planning_simulator + +// NOLINTNEXTLINE +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_WO_FALL_GUARD_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp similarity index 90% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index 381b6ae9da4e2..6b7323db2d9fb 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 The Autoware Foundation. +// Copyright 2025 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. @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ // NOLINT +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ // NOLINT #include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/simple_planning_simulator/utils/csv_loader.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "simple_planning_simulator/utils/csv_loader.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include @@ -27,6 +27,9 @@ #include #include +namespace autoware::simulator::simple_planning_simulator +{ + class AccelerationMap { public: @@ -209,4 +212,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface const double dt); }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +// NOLINTNEXTLINE +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_MAP_ACC_GEARED_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp similarity index 89% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp index 61a9e8d0a5643..fa3eafa78a710 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include @@ -23,6 +23,10 @@ #include #include #include + +namespace autoware::simulator::simple_planning_simulator +{ + /** * @class SimModelDelaySteerVel * @brief calculate delay steering dynamics @@ -75,7 +79,6 @@ class SimModelDelaySteerVel : public SimModelInterface const double steer_lim_; //!< @brief steering limit [rad] const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] const double wheelbase_; //!< @brief vehicle wheelbase length [m] - double prev_vx_ = 0.0; double current_ax_ = 0.0; std::deque vx_input_queue_; //!< @brief buffer for velocity command @@ -149,4 +152,6 @@ class SimModelDelaySteerVel : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp similarity index 80% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp index e4fcaa59f1294..86e33b13f0e3d 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,16 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include #include +namespace autoware::simulator::simple_planning_simulator +{ + /** * @class SimModelIdealSteerAcc * @brief calculate ideal steering dynamics @@ -103,4 +106,6 @@ class SimModelIdealSteerAcc : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp similarity index 83% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index c73cc54f4ea99..158326dbbde32 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,16 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include #include +namespace autoware::simulator::simple_planning_simulator +{ + /** * @class SimModelIdealSteerAccGeared * @brief calculate ideal steering dynamics @@ -115,4 +118,6 @@ class SimModelIdealSteerAccGeared : public SimModelInterface const double dt); }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp similarity index 80% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp index 09046cab89dc4..4febc830f33b5 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,15 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include #include + +namespace autoware::simulator::simple_planning_simulator +{ + /** * @class SimModelIdealSteerVel * @brief calculate ideal steering dynamics @@ -104,4 +108,6 @@ class SimModelIdealSteerVel : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp similarity index 91% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index bab4531484aa6..5af19c00e4c07 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #include @@ -22,6 +22,9 @@ #include +namespace autoware::simulator::simple_planning_simulator +{ + /** * @class SimModelInterface * @brief simple_planning_simulator vehicle model class to calculate vehicle dynamics @@ -178,4 +181,6 @@ class SimModelInterface const Eigen::VectorXd & state, const Eigen::VectorXd & input) = 0; }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp similarity index 83% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp index c6301db42620e..0aa525cd3f5c1 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ -#include "learning_based_vehicle_model/interconnected_model.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/learning_based_vehicle_model/interconnected_model.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include #include @@ -27,6 +27,9 @@ #include #include +namespace autoware::simulator::simple_planning_simulator +{ + /** * @class SimModelLearnedSteerVel * @brief calculate delay steering dynamics @@ -74,7 +77,7 @@ class SimModelLearnedSteerVel : public SimModelInterface /*delay steer velocity* STEER_DES, }; - InterconnectedModel vehicle; + autoware::simulator::learning_based_vehicle_model::InterconnectedModel vehicle; double prev_vx_ = 0.0; double current_ax_ = 0.0; @@ -141,4 +144,6 @@ class SimModelLearnedSteerVel : public SimModelInterface /*delay steer velocity* } }; -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ +} // namespace autoware::simulator::simple_planning_simulator + +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_LEARNED_STEER_VEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/visibility_control.hpp similarity index 85% rename from simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp rename to simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/visibility_control.hpp index 107367b286f80..2edd9467532fa 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp +++ b/simulator/autoware_simple_planning_simulator/include/autoware/simple_planning_simulator/visibility_control.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation +// Copyright 2025 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. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ +#ifndef AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ +#define AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ #if defined(__WIN32) #if defined(PLANNING_SIMULATOR_BUILDING_DLL) || defined(PLANNING_SIMULATOR_EXPORTS) @@ -35,4 +35,4 @@ #error "Unsupported Build Configuration" #endif // defined(_WINDOWS) -#endif // SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ +#endif // AUTOWARE__SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/autoware_simple_planning_simulator/launch/simple_planning_simulator.launch.py similarity index 95% rename from simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py rename to simulator/autoware_simple_planning_simulator/launch/simple_planning_simulator.launch.py index aed088de9fd22..df93acc6e0ff7 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/autoware_simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -1,4 +1,4 @@ -# Copyright 2021 The Autoware Foundation. +# Copyright 2025 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. @@ -87,9 +87,8 @@ def launch_setup(context, *args, **kwargs): ) simple_planning_simulator_node = Node( - package="simple_planning_simulator", - executable="simple_planning_simulator_exe", - name="simple_planning_simulator", + package="autoware_simple_planning_simulator", + executable="autoware_simple_planning_simulator_node", namespace="simulation", output="screen", parameters=[ @@ -151,7 +150,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "vehicle_characteristics_param_file", [ - FindPackageShare("simple_planning_simulator"), + FindPackageShare("autoware_simple_planning_simulator"), "/param/vehicle_characteristics.param.yaml", ], "path to config file for vehicle characteristics", @@ -160,7 +159,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "simulator_model_param_file", [ - FindPackageShare("simple_planning_simulator"), + FindPackageShare("autoware_simple_planning_simulator"), "/param/simple_planning_simulator_default.param.yaml", ], "path to config file for simulator_model", @@ -169,7 +168,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "acceleration_param_file", [ - FindPackageShare("simple_planning_simulator"), + FindPackageShare("autoware_simple_planning_simulator"), "/param/acceleration_map.csv", ], ) diff --git a/simulator/simple_planning_simulator/media/acceleration_map.svg b/simulator/autoware_simple_planning_simulator/media/acceleration_map.svg similarity index 100% rename from simulator/simple_planning_simulator/media/acceleration_map.svg rename to simulator/autoware_simple_planning_simulator/media/acceleration_map.svg diff --git a/simulator/simple_planning_simulator/media/mechanical_controller.drawio.svg b/simulator/autoware_simple_planning_simulator/media/mechanical_controller.drawio.svg similarity index 100% rename from simulator/simple_planning_simulator/media/mechanical_controller.drawio.svg rename to simulator/autoware_simple_planning_simulator/media/mechanical_controller.drawio.svg diff --git a/simulator/simple_planning_simulator/media/mechanical_controller_system_identification.png b/simulator/autoware_simple_planning_simulator/media/mechanical_controller_system_identification.png similarity index 100% rename from simulator/simple_planning_simulator/media/mechanical_controller_system_identification.png rename to simulator/autoware_simple_planning_simulator/media/mechanical_controller_system_identification.png diff --git a/simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg b/simulator/autoware_simple_planning_simulator/media/pitch-calculation.drawio.svg similarity index 100% rename from simulator/simple_planning_simulator/media/pitch-calculation.drawio.svg rename to simulator/autoware_simple_planning_simulator/media/pitch-calculation.drawio.svg diff --git a/simulator/simple_planning_simulator/media/vgr_sim.drawio.svg b/simulator/autoware_simple_planning_simulator/media/vgr_sim.drawio.svg similarity index 100% rename from simulator/simple_planning_simulator/media/vgr_sim.drawio.svg rename to simulator/autoware_simple_planning_simulator/media/vgr_sim.drawio.svg diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/autoware_simple_planning_simulator/package.xml similarity index 86% rename from simulator/simple_planning_simulator/package.xml rename to simulator/autoware_simple_planning_simulator/package.xml index fdc782182f9a5..47ca399a9ad95 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/autoware_simple_planning_simulator/package.xml @@ -1,8 +1,8 @@ - simple_planning_simulator - 0.40.0 + autoware_simple_planning_simulator + 0.41.0 simple_planning_simulator as a ROS 2 node Takamasa Horibe Tomoya Kimura @@ -10,6 +10,8 @@ Mamoru Sobue Zulfaqar Azmi Temkei Kem + Junya Sasaki + Kotaro Yoshimoto Apache License 2.0 ament_cmake_auto @@ -17,6 +19,7 @@ autoware_control_msgs autoware_lanelet2_extension + autoware_learning_based_vehicle_model autoware_map_msgs autoware_motion_utils autoware_planning_msgs @@ -25,7 +28,6 @@ autoware_vehicle_msgs geometry_msgs lanelet2_core - learning_based_vehicle_model nav_msgs rclcpp rclcpp_components diff --git a/simulator/simple_planning_simulator/param/acceleration_map.csv b/simulator/autoware_simple_planning_simulator/param/acceleration_map.csv similarity index 100% rename from simulator/simple_planning_simulator/param/acceleration_map.csv rename to simulator/autoware_simple_planning_simulator/param/acceleration_map.csv diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/autoware_simple_planning_simulator/param/simple_planning_simulator_default.param.yaml similarity index 100% rename from simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml rename to simulator/autoware_simple_planning_simulator/param/simple_planning_simulator_default.param.yaml diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_mechanical_sample.param.yaml b/simulator/autoware_simple_planning_simulator/param/simple_planning_simulator_mechanical_sample.param.yaml similarity index 100% rename from simulator/simple_planning_simulator/param/simple_planning_simulator_mechanical_sample.param.yaml rename to simulator/autoware_simple_planning_simulator/param/simple_planning_simulator_mechanical_sample.param.yaml diff --git a/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml b/simulator/autoware_simple_planning_simulator/param/vehicle_characteristics.param.yaml similarity index 100% rename from simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml rename to simulator/autoware_simple_planning_simulator/param/vehicle_characteristics.param.yaml diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp similarity index 98% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index d61654c32af02..b03de1906db78 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include "autoware/simple_planning_simulator/simple_planning_simulator_core.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" #include "autoware/universe_utils/geometry/geometry.hpp" #include "autoware/universe_utils/ros/msg_covariance.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp_components/register_node_macro.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" #include #include @@ -43,7 +43,7 @@ using namespace std::literals::chrono_literals; -namespace +namespace autoware::simulator::simple_planning_simulator { autoware_vehicle_msgs::msg::VelocityReport to_velocity_report( @@ -91,12 +91,6 @@ std::vector convert_centerline_to_points( } return centerline_points; } -} // namespace - -namespace simulation -{ -namespace simple_planning_simulator -{ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & options) : Node("simple_planning_simulator", options), tf_buffer_(get_clock()), tf_listener_(tf_buffer_) @@ -585,9 +579,7 @@ void SimplePlanningSimulator::set_input(const InputCommand & cmd, const double a std::visit( [this, acc_by_slope](auto && arg) { using T = std::decay_t; - if constexpr (std::is_same_v) { - set_input(arg, acc_by_slope); - } else if constexpr (std::is_same_v) { + if constexpr (std::is_same_v || std::is_same_v) { set_input(arg, acc_by_slope); } else { throw std::invalid_argument("Invalid input command type"); @@ -640,7 +632,7 @@ void SimplePlanningSimulator::set_input(const Control & cmd, const double acc_by input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { // NOLINT input << combined_acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || @@ -959,7 +951,7 @@ void SimplePlanningSimulator::publish_actuation_status() actuation_status.value().header.frame_id = simulated_frame_id_; pub_actuation_status_->publish(actuation_status.value()); } -} // namespace simple_planning_simulator -} // namespace simulation +} // namespace autoware::simulator::simple_planning_simulator -RCLCPP_COMPONENTS_REGISTER_NODE(simulation::simple_planning_simulator::SimplePlanningSimulator) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::simulator::simple_planning_simulator::SimplePlanningSimulator) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp similarity index 94% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp index fa1f7978a798c..0a4f03c959ff0 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/utils/csv_loader.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. All rights reserved. +// Copyright 2025 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/utils/csv_loader.hpp" +#include "autoware/simple_planning_simulator/utils/csv_loader.hpp" #include #include @@ -21,6 +21,9 @@ #include #include +namespace autoware::simulator::simple_planning_simulator +{ + CSVLoader::CSVLoader(const std::string & csv_path) { csv_path_ = csv_path; @@ -152,3 +155,5 @@ double CSVLoader::clampValue(const double val, const std::vector & range } return val; } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp similarity index 97% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp index 0977cd9e25a92..84f4ff4ea0ba4 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/utils/mechanical_controller.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/utils/mechanical_controller.hpp" +#include "autoware/simple_planning_simulator/utils/mechanical_controller.hpp" #include #include @@ -24,7 +24,7 @@ #include #include -namespace autoware::simple_planning_simulator::utils::mechanical_controller +namespace autoware::simulator::simple_planning_simulator { using DelayBuffer = std::deque>; @@ -349,4 +349,4 @@ StepResult MechanicalController::run_one_step( return {delay_buffer_new, pid_error, d_state, false}; } -} // namespace autoware::simple_planning_simulator::utils::mechanical_controller +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp similarity index 98% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp index bfcf571d8c09b..4db0b0856ea25 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" #include "autoware_vehicle_msgs/msg/gear_command.hpp" @@ -20,6 +20,9 @@ #include #include +namespace autoware::simulator::simple_planning_simulator +{ + bool ActuationMap::readActuationMapFromCSV(const std::string & csv_path, const bool validation) { CSVLoader csv(csv_path); @@ -517,3 +520,4 @@ void SimModelActuationCmdMechanical::setState(const Eigen::VectorXd & state) state_ = state; } /* ---------------------------------------- */ +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp similarity index 94% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index 6a9b5c65d4760..fbe4db941e8de 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" #include +namespace autoware::simulator::simple_planning_simulator +{ + SimModelDelaySteerAcc::SimModelDelaySteerAcc( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, @@ -135,3 +138,5 @@ Eigen::VectorXd SimModelDelaySteerAcc::calcModel( return d_state; } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp similarity index 96% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 88b0e6c639fd1..8db40f7cc5298 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" #include "autoware_vehicle_msgs/msg/gear_command.hpp" #include +namespace autoware::simulator::simple_planning_simulator +{ + SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, @@ -180,3 +183,5 @@ void SimModelDelaySteerAccGeared::updateStateWithGear( setStopState(); } } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp similarity index 96% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp index 78cfa6caeb946..5e26132aa3e5e 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp" #include "autoware_vehicle_msgs/msg/gear_command.hpp" #include +namespace autoware::simulator::simple_planning_simulator +{ + SimModelDelaySteerAccGearedWoFallGuard::SimModelDelaySteerAccGearedWoFallGuard( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, @@ -181,3 +184,5 @@ Eigen::VectorXd SimModelDelaySteerAccGearedWoFallGuard::calcModel( return d_state; } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp similarity index 95% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index ffef5ab7b236c..dc576872a6c18 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -1,4 +1,4 @@ -// Copyright 2023 The Autoware Foundation. +// Copyright 2025 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. @@ -12,13 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" #include "autoware_vehicle_msgs/msg/gear_command.hpp" #include #include +namespace autoware::simulator::simple_planning_simulator +{ + SimModelDelaySteerMapAccGeared::SimModelDelaySteerMapAccGeared( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double acc_delay, double acc_time_constant, double steer_delay, @@ -168,3 +171,5 @@ void SimModelDelaySteerMapAccGeared::updateStateWithGear( state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); } } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp similarity index 92% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp index 1ee08fad566f5..89d9b573fa96d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,10 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" #include +namespace autoware::simulator::simple_planning_simulator +{ + SimModelDelaySteerVel::SimModelDelaySteerVel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, double dt, double vx_delay, double vx_time_constant, double steer_delay, @@ -80,9 +83,9 @@ void SimModelDelaySteerVel::update(const double & dt) delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); steer_input_queue_.pop_front(); // do not use deadzone_delta_steer (Steer IF does not exist in this model) + const double prev_vx = state_(IDX::VX); updateRungeKutta(dt, delayed_input); - current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; - prev_vx_ = input_(IDX_U::VX_DES); + current_ax_ = (state_(IDX::VX) - prev_vx) / dt; } void SimModelDelaySteerVel::initializeInputQueue(const double & dt) @@ -132,3 +135,5 @@ Eigen::VectorXd SimModelDelaySteerVel::calcModel( return d_state; } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp similarity index 88% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp index 2edf43f0a743c..f5512c4bdb4b3 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" + +namespace autoware::simulator::simple_planning_simulator +{ SimModelIdealSteerAcc::SimModelIdealSteerAcc(double wheelbase) : SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) @@ -72,3 +75,5 @@ Eigen::VectorXd SimModelIdealSteerAcc::calcModel( return d_state; } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp similarity index 93% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 46538778bd1b1..e06ca161d36e0 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,12 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "autoware_vehicle_msgs/msg/gear_command.hpp" #include +namespace autoware::simulator::simple_planning_simulator +{ + SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(double wheelbase) : SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) { @@ -115,3 +118,5 @@ void SimModelIdealSteerAccGeared::updateStateWithGear( // calculate acc from velocity diff current_acc_ = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp similarity index 88% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp index e43ac4aa8eae5..6009b2f0f3fd5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" + +namespace autoware::simulator::simple_planning_simulator +{ SimModelIdealSteerVel::SimModelIdealSteerVel(double wheelbase) : SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) @@ -72,3 +75,5 @@ Eigen::VectorXd SimModelIdealSteerVel::calcModel( return d_state; } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp similarity index 87% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp index 8d17f72e0c3b3..33135f40c4c87 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation. +// Copyright 2025 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. @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +namespace autoware::simulator::simple_planning_simulator +{ SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) { @@ -57,3 +60,5 @@ uint8_t SimModelInterface::getGear() const { return gear_; } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp similarity index 88% rename from simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp rename to simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp index 4ddcc0e498b70..ce7ba2d48bac2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp +++ b/simulator/autoware_simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Foundation. +// Copyright 2025 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. @@ -12,15 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" +#include "autoware/simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" -#include "learning_based_vehicle_model/interconnected_model.hpp" +#include "autoware/learning_based_vehicle_model/interconnected_model.hpp" #include #include #include #include +namespace autoware::simulator::simple_planning_simulator +{ + SimModelLearnedSteerVel::SimModelLearnedSteerVel( double dt, std::vector model_python_paths, std::vector model_param_paths, std::vector model_class_names) @@ -89,3 +92,5 @@ void SimModelLearnedSteerVel::update(const double & dt) current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; prev_vx_ = input_(IDX_U::VX_DES); } + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/simple_planning_simulator/test/actuation_cmd_map/accel_map.csv b/simulator/autoware_simple_planning_simulator/test/actuation_cmd_map/accel_map.csv similarity index 100% rename from simulator/simple_planning_simulator/test/actuation_cmd_map/accel_map.csv rename to simulator/autoware_simple_planning_simulator/test/actuation_cmd_map/accel_map.csv diff --git a/simulator/simple_planning_simulator/test/actuation_cmd_map/brake_map.csv b/simulator/autoware_simple_planning_simulator/test/actuation_cmd_map/brake_map.csv similarity index 100% rename from simulator/simple_planning_simulator/test/actuation_cmd_map/brake_map.csv rename to simulator/autoware_simple_planning_simulator/test/actuation_cmd_map/brake_map.csv diff --git a/simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv b/simulator/autoware_simple_planning_simulator/test/actuation_cmd_map/steer_map.csv similarity index 100% rename from simulator/simple_planning_simulator/test/actuation_cmd_map/steer_map.csv rename to simulator/autoware_simple_planning_simulator/test/actuation_cmd_map/steer_map.csv diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/autoware_simple_planning_simulator/test/test_simple_planning_simulator.cpp similarity index 98% rename from simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp rename to simulator/autoware_simple_planning_simulator/test/test_simple_planning_simulator.cpp index 0baa3302f86c7..afe816ee8944a 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/autoware_simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 The Autoware Foundation +// Copyright 2025 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. @@ -13,8 +13,8 @@ // limitations under the License. #include "ament_index_cpp/get_package_share_directory.hpp" +#include "autoware/simple_planning_simulator/simple_planning_simulator_core.hpp" #include "gtest/gtest.h" -#include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "tf2/utils.h" #include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" @@ -32,15 +32,15 @@ #include +namespace autoware::simulator::simple_planning_simulator +{ + using autoware_control_msgs::msg::Control; using autoware_vehicle_msgs::msg::GearCommand; using geometry_msgs::msg::PoseWithCovarianceStamped; using nav_msgs::msg::Odometry; using tier4_vehicle_msgs::msg::ActuationCommandStamped; -using simulation::simple_planning_simulator::InputCommand; -using simulation::simple_planning_simulator::SimplePlanningSimulator; - std::string toStrInfo(const Odometry & o) { const auto & p = o.pose.pose; @@ -317,7 +317,8 @@ TEST_P(TestSimplePlanningSimulator, TestIdealSteerVel) node_options.append_parameter_override("brake_time_constant", 0.2); node_options.append_parameter_override("convert_accel_cmd", true); node_options.append_parameter_override("convert_brake_cmd", true); - const auto share_dir = ament_index_cpp::get_package_share_directory("simple_planning_simulator"); + const auto share_dir = + ament_index_cpp::get_package_share_directory("autoware_simple_planning_simulator"); const auto accel_map_path = share_dir + "/test/actuation_cmd_map/accel_map.csv"; const auto brake_map_path = share_dir + "/test/actuation_cmd_map/brake_map.csv"; const auto steer_map_path = share_dir + "/test/actuation_cmd_map/steer_map.csv"; @@ -456,3 +457,5 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple(CommandType::Actuation, "ACTUATION_CMD_STEER_MAP"), std::make_tuple(CommandType::Actuation, "ACTUATION_CMD_VGR"), std::make_tuple(CommandType::Actuation, "ACTUATION_CMD_MECHANICAL"))); + +} // namespace autoware::simulator::simple_planning_simulator diff --git a/simulator/vehicle_door_simulator/CHANGELOG.rst b/simulator/autoware_vehicle_door_simulator/CHANGELOG.rst similarity index 86% rename from simulator/vehicle_door_simulator/CHANGELOG.rst rename to simulator/autoware_vehicle_door_simulator/CHANGELOG.rst index 3def98ed36995..30e09a8236754 100644 --- a/simulator/vehicle_door_simulator/CHANGELOG.rst +++ b/simulator/autoware_vehicle_door_simulator/CHANGELOG.rst @@ -1,7 +1,14 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package vehicle_door_simulator +Changelog for package autoware_vehicle_door_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `vehicle_door_simulator` (`#9997 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/simulator/vehicle_door_simulator/CMakeLists.txt b/simulator/autoware_vehicle_door_simulator/CMakeLists.txt similarity index 65% rename from simulator/vehicle_door_simulator/CMakeLists.txt rename to simulator/autoware_vehicle_door_simulator/CMakeLists.txt index fbe989555c4ba..d715608a5d122 100644 --- a/simulator/vehicle_door_simulator/CMakeLists.txt +++ b/simulator/autoware_vehicle_door_simulator/CMakeLists.txt @@ -1,10 +1,10 @@ cmake_minimum_required(VERSION 3.14) -project(vehicle_door_simulator) +project(autoware_vehicle_door_simulator) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(dummy_doors +ament_auto_add_executable(${PROJECT_NAME}_node src/dummy_doors.cpp ) diff --git a/simulator/vehicle_door_simulator/README.md b/simulator/autoware_vehicle_door_simulator/README.md similarity index 68% rename from simulator/vehicle_door_simulator/README.md rename to simulator/autoware_vehicle_door_simulator/README.md index f312216622a5b..d3fe0a30993ab 100644 --- a/simulator/vehicle_door_simulator/README.md +++ b/simulator/autoware_vehicle_door_simulator/README.md @@ -1,3 +1,3 @@ -# vehicle_door_simulator +# autoware_vehicle_door_simulator This package is for testing operations on vehicle devices such as doors. diff --git a/simulator/vehicle_door_simulator/launch/vehicle_door_simulator.launch.xml b/simulator/autoware_vehicle_door_simulator/launch/vehicle_door_simulator.launch.xml similarity index 70% rename from simulator/vehicle_door_simulator/launch/vehicle_door_simulator.launch.xml rename to simulator/autoware_vehicle_door_simulator/launch/vehicle_door_simulator.launch.xml index 66857130a4932..e550a460e8807 100644 --- a/simulator/vehicle_door_simulator/launch/vehicle_door_simulator.launch.xml +++ b/simulator/autoware_vehicle_door_simulator/launch/vehicle_door_simulator.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/simulator/vehicle_door_simulator/package.xml b/simulator/autoware_vehicle_door_simulator/package.xml similarity index 82% rename from simulator/vehicle_door_simulator/package.xml rename to simulator/autoware_vehicle_door_simulator/package.xml index ca9d7c98e691a..d0ecdb3c8f529 100644 --- a/simulator/vehicle_door_simulator/package.xml +++ b/simulator/autoware_vehicle_door_simulator/package.xml @@ -1,10 +1,11 @@ - vehicle_door_simulator - 0.40.0 + autoware_vehicle_door_simulator + 0.41.0 The vehicle_door_simulator package Takagi, Isamu + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/simulator/vehicle_door_simulator/src/dummy_doors.cpp b/simulator/autoware_vehicle_door_simulator/src/dummy_doors.cpp similarity index 93% rename from simulator/vehicle_door_simulator/src/dummy_doors.cpp rename to simulator/autoware_vehicle_door_simulator/src/dummy_doors.cpp index cad0aa721dc73..001213918c900 100644 --- a/simulator/vehicle_door_simulator/src/dummy_doors.cpp +++ b/simulator/autoware_vehicle_door_simulator/src/dummy_doors.cpp @@ -16,10 +16,10 @@ #include -namespace vehicle_door_simulator +namespace autoware::vehicle_door_simulator { -DummyDoors::DummyDoors() : Node("dummy_doors") +DummyDoors::DummyDoors() : Node("vehicle_door_simulator_node") { using std::placeholders::_1; using std::placeholders::_2; @@ -112,13 +112,13 @@ void DummyDoors::on_timer() pub_status_->publish(message); } -} // namespace vehicle_door_simulator +} // namespace autoware::vehicle_door_simulator int main(int argc, char ** argv) { rclcpp::init(argc, argv); rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); + auto node = std::make_shared(); executor.add_node(node); executor.spin(); executor.remove_node(node); diff --git a/simulator/vehicle_door_simulator/src/dummy_doors.hpp b/simulator/autoware_vehicle_door_simulator/src/dummy_doors.hpp similarity index 95% rename from simulator/vehicle_door_simulator/src/dummy_doors.hpp rename to simulator/autoware_vehicle_door_simulator/src/dummy_doors.hpp index 485586fa8724e..452d11c5414e3 100644 --- a/simulator/vehicle_door_simulator/src/dummy_doors.hpp +++ b/simulator/autoware_vehicle_door_simulator/src/dummy_doors.hpp @@ -23,7 +23,7 @@ #include -namespace vehicle_door_simulator +namespace autoware::vehicle_door_simulator { class DummyDoors : public rclcpp::Node @@ -57,6 +57,6 @@ class DummyDoors : public rclcpp::Node std::array statuses_; }; -} // namespace vehicle_door_simulator +} // namespace autoware::vehicle_door_simulator #endif // DUMMY_DOORS_HPP_ diff --git a/simulator/fault_injection/launch/fault_injection.launch.xml b/simulator/fault_injection/launch/fault_injection.launch.xml deleted file mode 100644 index 94b1fe8a62e6b..0000000000000 --- a/simulator/fault_injection/launch/fault_injection.launch.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/simulator/fault_injection/test/launch/test_fault_injection.launch.xml b/simulator/fault_injection/test/launch/test_fault_injection.launch.xml deleted file mode 100644 index 1d4bcd7c96d9b..0000000000000 --- a/simulator/fault_injection/test/launch/test_fault_injection.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp deleted file mode 100644 index 52c3825ae1888..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ /dev/null @@ -1,30 +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 SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ - -#include "simple_planning_simulator/vehicle_model/sim_model_actuation_cmd.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared_wo_fall_guard.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_learned_steer_vel.hpp" - -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ diff --git a/simulator/tier4_dummy_object_rviz_plugin/CHANGELOG.rst b/simulator/tier4_dummy_object_rviz_plugin/CHANGELOG.rst index 1157abff775ab..0c2a51543b7a1 100644 --- a/simulator/tier4_dummy_object_rviz_plugin/CHANGELOG.rst +++ b/simulator/tier4_dummy_object_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_dummy_object_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/simulator/tier4_dummy_object_rviz_plugin/package.xml b/simulator/tier4_dummy_object_rviz_plugin/package.xml index b73d3a1cc15c0..6174317813b59 100644 --- a/simulator/tier4_dummy_object_rviz_plugin/package.xml +++ b/simulator/tier4_dummy_object_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_dummy_object_rviz_plugin - 0.40.0 + 0.41.0 The tier4_dummy_object_rviz_plugin package Yukihiro Saito Apache License 2.0 diff --git a/system/bluetooth_monitor/CHANGELOG.rst b/system/autoware_bluetooth_monitor/CHANGELOG.rst similarity index 81% rename from system/bluetooth_monitor/CHANGELOG.rst rename to system/autoware_bluetooth_monitor/CHANGELOG.rst index 25b55a9bdcc6f..3f01329d06120 100644 --- a/system/bluetooth_monitor/CHANGELOG.rst +++ b/system/autoware_bluetooth_monitor/CHANGELOG.rst @@ -1,7 +1,31 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package bluetooth_monitor +Changelog for package autoware_bluetooth_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `bluetooth_monitor` (`#9960 `_) + * feat(bluetooth_monitor): apply `autoware\_` prefix (see below): + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(bluetooth_monitor): move headers under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(bluetooth_monitor): fix include paths + * To follow the previous commit + * bug(bluetooth_monitor): fix a missing prefix bug + * rename: `bluetooth_monitor` => `autoware_bluetooth_monitor` + * style(pre-commit): autofix + * bug(autoware_bluetooth_monitor): revert wrongly updated copyrights + * bug(autoware_bluetooth_monitor): `autoware\_` prefix is not needed here + * update: `CODEOWNERS` + * update(autoware_bluetooth_monitor): `README.md` + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/bluetooth_monitor/CMakeLists.txt b/system/autoware_bluetooth_monitor/CMakeLists.txt similarity index 65% rename from system/bluetooth_monitor/CMakeLists.txt rename to system/autoware_bluetooth_monitor/CMakeLists.txt index ea91d77abeeb8..042f0ab9ada1b 100644 --- a/system/bluetooth_monitor/CMakeLists.txt +++ b/system/autoware_bluetooth_monitor/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.5) -project(bluetooth_monitor) +project(autoware_bluetooth_monitor) ### Dependencies find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(bluetooth_monitor_lib SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/bluetooth_monitor.cpp ) @@ -21,12 +21,12 @@ find_package(Boost REQUIRED COMPONENTS ) ## Specify libraries to link a library or executable target against -target_link_libraries(bluetooth_monitor_lib ${Boost_LIBRARIES}) +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) target_link_libraries(l2ping_service ${Boost_LIBRARIES}) -rclcpp_components_register_node(bluetooth_monitor_lib - PLUGIN "BluetoothMonitor" - EXECUTABLE bluetooth_monitor +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::bluetooth_monitor::BluetoothMonitor" + EXECUTABLE ${PROJECT_NAME}_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/system/bluetooth_monitor/README.md b/system/autoware_bluetooth_monitor/README.md similarity index 94% rename from system/bluetooth_monitor/README.md rename to system/autoware_bluetooth_monitor/README.md index c0f9c3aecca98..a32069624e113 100644 --- a/system/bluetooth_monitor/README.md +++ b/system/autoware_bluetooth_monitor/README.md @@ -46,7 +46,7 @@ L2ping is only allowed for root by default, so this package provides the followi ## Parameters -{{ json_to_markdown("system/bluetooth_monitor/schema/bluetooth_monitor.schema.json") }} +{{ json_to_markdown("system/autoware_bluetooth_monitor/schema/bluetooth_monitor.schema.json") }} - `rtt_warn` @@ -71,7 +71,7 @@ L2ping is only allowed for root by default, so this package provides the followi ```sh ./build/bluetooth_monitor/l2ping_service - ros2 launch bluetooth_monitor bluetooth_monitor.launch.xml + ros2 launch autoware_bluetooth_monitor bluetooth_monitor.launch.xml ``` ## Known limitations and issues diff --git a/system/bluetooth_monitor/config/bluetooth_monitor.param.yaml b/system/autoware_bluetooth_monitor/config/bluetooth_monitor.param.yaml similarity index 100% rename from system/bluetooth_monitor/config/bluetooth_monitor.param.yaml rename to system/autoware_bluetooth_monitor/config/bluetooth_monitor.param.yaml diff --git a/system/bluetooth_monitor/docs/block_diagram.drawio.svg b/system/autoware_bluetooth_monitor/docs/block_diagram.drawio.svg similarity index 100% rename from system/bluetooth_monitor/docs/block_diagram.drawio.svg rename to system/autoware_bluetooth_monitor/docs/block_diagram.drawio.svg diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp b/system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/bluetooth_monitor.hpp similarity index 92% rename from system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp rename to system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/bluetooth_monitor.hpp index 6ebca9b4189d5..60f6ec1adc648 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/bluetooth_monitor.hpp +++ b/system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/bluetooth_monitor.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ -#define BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ +#ifndef AUTOWARE__BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ +#define AUTOWARE__BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ -#include "bluetooth_monitor/service/l2ping_interface.hpp" +#include "autoware/bluetooth_monitor/service/l2ping_interface.hpp" #include #include @@ -24,6 +24,9 @@ #include #include +namespace autoware::bluetooth_monitor +{ + class BluetoothMonitor : public rclcpp::Node { public: @@ -109,4 +112,6 @@ class BluetoothMonitor : public rclcpp::Node {StatusCode::FUNCTION_ERROR, DiagStatus::ERROR}}; }; -#endif // BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ +} // namespace autoware::bluetooth_monitor + +#endif // AUTOWARE__BLUETOOTH_MONITOR__BLUETOOTH_MONITOR_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp b/system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping.hpp similarity index 87% rename from system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp rename to system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping.hpp index 8d7777f78f743..5ff24f687f465 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping.hpp +++ b/system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping.hpp @@ -12,16 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ -#define BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ +#ifndef AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ +#define AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ -#include "bluetooth_monitor/service/l2ping_interface.hpp" +#include "autoware/bluetooth_monitor/service/l2ping_interface.hpp" #include #include #include #include +namespace autoware::bluetooth_monitor +{ + class L2ping { public: @@ -92,4 +95,6 @@ class L2ping bool stop_; //!< @brief Flag to stop thread }; -#endif // BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ +} // namespace autoware::bluetooth_monitor + +#endif // AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp b/system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping_interface.hpp similarity index 94% rename from system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp rename to system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping_interface.hpp index 26abc5c4b2ac4..f02f2cec12d27 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_interface.hpp +++ b/system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ -#define BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ +#ifndef AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ +#define AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ #include #include @@ -23,6 +23,9 @@ #include #include +namespace autoware::bluetooth_monitor +{ + // 7634-7647 Unassigned static constexpr int DEFAULT_PORT = 7640; static constexpr int DEFAULT_DELAY = 1; @@ -171,4 +174,6 @@ struct L2pingStatus */ typedef std::vector L2pingStatusList; -#endif // BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ +} // namespace autoware::bluetooth_monitor + +#endif // AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_INTERFACE_HPP_ diff --git a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp b/system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping_service.hpp similarity index 85% rename from system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp rename to system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping_service.hpp index 2511311e8ba52..21a42718093b2 100644 --- a/system/bluetooth_monitor/include/bluetooth_monitor/service/l2ping_service.hpp +++ b/system/autoware_bluetooth_monitor/include/autoware/bluetooth_monitor/service/l2ping_service.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ -#define BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ +#ifndef AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ +#define AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ -#include "bluetooth_monitor/service/l2ping.hpp" -#include "bluetooth_monitor/service/l2ping_interface.hpp" +#include "autoware/bluetooth_monitor/service/l2ping.hpp" +#include "autoware/bluetooth_monitor/service/l2ping_interface.hpp" #include #include @@ -24,6 +24,9 @@ #include #include +namespace autoware::bluetooth_monitor +{ + class L2pingService { public: @@ -85,4 +88,6 @@ class L2pingService L2pingStatusList status_list_; //!< @brief List of l2ping status }; -#endif // BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ +} // namespace autoware::bluetooth_monitor + +#endif // AUTOWARE__BLUETOOTH_MONITOR__SERVICE__L2PING_SERVICE_HPP_ diff --git a/system/autoware_bluetooth_monitor/launch/bluetooth_monitor.launch.xml b/system/autoware_bluetooth_monitor/launch/bluetooth_monitor.launch.xml new file mode 100644 index 0000000000000..d23e7b44d3a32 --- /dev/null +++ b/system/autoware_bluetooth_monitor/launch/bluetooth_monitor.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/bluetooth_monitor/package.xml b/system/autoware_bluetooth_monitor/package.xml similarity index 86% rename from system/bluetooth_monitor/package.xml rename to system/autoware_bluetooth_monitor/package.xml index f43db39e987b7..12070b90f3d2a 100644 --- a/system/bluetooth_monitor/package.xml +++ b/system/autoware_bluetooth_monitor/package.xml @@ -1,10 +1,11 @@ - bluetooth_monitor - 0.40.0 + autoware_bluetooth_monitor + 0.41.0 Bluetooth alive monitoring Fumihito Ito + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/bluetooth_monitor/schema/bluetooth_monitor.schema.json b/system/autoware_bluetooth_monitor/schema/bluetooth_monitor.schema.json similarity index 100% rename from system/bluetooth_monitor/schema/bluetooth_monitor.schema.json rename to system/autoware_bluetooth_monitor/schema/bluetooth_monitor.schema.json diff --git a/system/bluetooth_monitor/service/l2ping.cpp b/system/autoware_bluetooth_monitor/service/l2ping.cpp similarity index 96% rename from system/bluetooth_monitor/service/l2ping.cpp rename to system/autoware_bluetooth_monitor/service/l2ping.cpp index 6499f3a2b9612..4911825e5bf04 100644 --- a/system/bluetooth_monitor/service/l2ping.cpp +++ b/system/autoware_bluetooth_monitor/service/l2ping.cpp @@ -13,7 +13,7 @@ // limitations under the License. // -#include "bluetooth_monitor/service/l2ping.hpp" +#include "autoware/bluetooth_monitor/service/l2ping.hpp" #include @@ -25,6 +25,9 @@ #define FMT_HEADER_ONLY #include +namespace autoware::bluetooth_monitor +{ + namespace bp = boost::process; L2ping::L2ping(const std::string & address, const L2pingConfig & config) @@ -189,3 +192,5 @@ const std::string & L2ping::getAddress() const { return status_.address; } + +} // namespace autoware::bluetooth_monitor diff --git a/system/bluetooth_monitor/service/l2ping_service.cpp b/system/autoware_bluetooth_monitor/service/l2ping_service.cpp similarity index 97% rename from system/bluetooth_monitor/service/l2ping_service.cpp rename to system/autoware_bluetooth_monitor/service/l2ping_service.cpp index 827a7e3060acc..20bb6104692af 100644 --- a/system/bluetooth_monitor/service/l2ping_service.cpp +++ b/system/autoware_bluetooth_monitor/service/l2ping_service.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "bluetooth_monitor/service/l2ping_service.hpp" +#include "autoware/bluetooth_monitor/service/l2ping_service.hpp" #include #include @@ -30,6 +30,9 @@ #define FMT_HEADER_ONLY #include +namespace autoware::bluetooth_monitor +{ + namespace bp = boost::process; L2pingService::L2pingService(const int port) : port_(port), socket_(-1) @@ -252,3 +255,5 @@ bool L2pingService::buildDeviceList() return true; } + +} // namespace autoware::bluetooth_monitor diff --git a/system/bluetooth_monitor/service/main.cpp b/system/autoware_bluetooth_monitor/service/main.cpp similarity index 90% rename from system/bluetooth_monitor/service/main.cpp rename to system/autoware_bluetooth_monitor/service/main.cpp index d3aef2c0696dd..eb2bd5f3d5cc0 100644 --- a/system/bluetooth_monitor/service/main.cpp +++ b/system/autoware_bluetooth_monitor/service/main.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "bluetooth_monitor/service/l2ping_interface.hpp" -#include "bluetooth_monitor/service/l2ping_service.hpp" +#include "autoware/bluetooth_monitor/service/l2ping_interface.hpp" +#include "autoware/bluetooth_monitor/service/l2ping_service.hpp" #include @@ -47,7 +47,7 @@ int main(int argc, char ** argv) // Parse command-line options int c = 0; int option_index = 0; - int port = DEFAULT_PORT; + int port = autoware::bluetooth_monitor::DEFAULT_PORT; while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { switch (c) { case 'h': @@ -78,7 +78,7 @@ int main(int argc, char ** argv) openlog(nullptr, LOG_PID, LOG_DAEMON); // Initialize l2ping service - L2pingService service(port); + autoware::bluetooth_monitor::L2pingService service(port); if (!service.initialize()) { service.shutdown(); diff --git a/system/bluetooth_monitor/src/bluetooth_monitor.cpp b/system/autoware_bluetooth_monitor/src/bluetooth_monitor.cpp similarity index 96% rename from system/bluetooth_monitor/src/bluetooth_monitor.cpp rename to system/autoware_bluetooth_monitor/src/bluetooth_monitor.cpp index b9c74870c558d..e9fcb56d41622 100644 --- a/system/bluetooth_monitor/src/bluetooth_monitor.cpp +++ b/system/autoware_bluetooth_monitor/src/bluetooth_monitor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "bluetooth_monitor/bluetooth_monitor.hpp" +#include "autoware/bluetooth_monitor/bluetooth_monitor.hpp" #include #include @@ -28,6 +28,9 @@ #define FMT_HEADER_ONLY #include +namespace autoware::bluetooth_monitor +{ + BluetoothMonitor::BluetoothMonitor(const rclcpp::NodeOptions & options) : Node("bluetooth_monitor", options), updater_(this), @@ -199,5 +202,7 @@ void BluetoothMonitor::checkConnection(diagnostic_updater::DiagnosticStatusWrapp setErrorLevel(stat); } +} // namespace autoware::bluetooth_monitor + #include -RCLCPP_COMPONENTS_REGISTER_NODE(BluetoothMonitor) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::bluetooth_monitor::BluetoothMonitor) diff --git a/system/autoware_component_monitor/CHANGELOG.rst b/system/autoware_component_monitor/CHANGELOG.rst index 339f30f9f9f80..ad3fbeac76c35 100644 --- a/system/autoware_component_monitor/CHANGELOG.rst +++ b/system/autoware_component_monitor/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_component_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/autoware_component_monitor/package.xml b/system/autoware_component_monitor/package.xml index 27432bc1fc0d6..965cef269d971 100644 --- a/system/autoware_component_monitor/package.xml +++ b/system/autoware_component_monitor/package.xml @@ -2,7 +2,7 @@ autoware_component_monitor - 0.40.0 + 0.41.0 A ROS 2 package to monitor system usage of component containers. Mehmet Emin Başoğlu Barış Zeren diff --git a/system/component_state_monitor/CHANGELOG.rst b/system/autoware_component_state_monitor/CHANGELOG.rst similarity index 93% rename from system/component_state_monitor/CHANGELOG.rst rename to system/autoware_component_state_monitor/CHANGELOG.rst index 40843314ddf4a..f7c729718af15 100644 --- a/system/component_state_monitor/CHANGELOG.rst +++ b/system/autoware_component_state_monitor/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package component_state_monitor -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_component_state_monitor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware` prefix for `component_state_monitor` and its dependencies (`#9961 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/system/component_state_monitor/CMakeLists.txt b/system/autoware_component_state_monitor/CMakeLists.txt similarity index 73% rename from system/component_state_monitor/CMakeLists.txt rename to system/autoware_component_state_monitor/CMakeLists.txt index 23037b1400e2d..3ca6e4f5c4e1a 100644 --- a/system/component_state_monitor/CMakeLists.txt +++ b/system/autoware_component_state_monitor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(component_state_monitor) +project(autoware_component_state_monitor) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_nodes(${PROJECT_NAME} - "component_state_monitor::StateMonitor" + "autoware::component_state_monitor::StateMonitor" ) ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/component_state_monitor/README.md b/system/autoware_component_state_monitor/README.md similarity index 87% rename from system/component_state_monitor/README.md rename to system/autoware_component_state_monitor/README.md index 327c7b7651caf..a7ea12624da0d 100644 --- a/system/component_state_monitor/README.md +++ b/system/autoware_component_state_monitor/README.md @@ -1,4 +1,4 @@ -# component_state_monitor +# autoware_component_state_monitor The component state monitor checks the state of each component using topic state monitor. This is an implementation for backward compatibility with the AD service state monitor. diff --git a/system/component_state_monitor/config/topics.yaml b/system/autoware_component_state_monitor/config/topics.yaml similarity index 100% rename from system/component_state_monitor/config/topics.yaml rename to system/autoware_component_state_monitor/config/topics.yaml diff --git a/system/component_state_monitor/launch/component_state_monitor.launch.py b/system/autoware_component_state_monitor/launch/component_state_monitor.launch.py similarity index 94% rename from system/component_state_monitor/launch/component_state_monitor.launch.py rename to system/autoware_component_state_monitor/launch/component_state_monitor.launch.py index 6fcb4e6f13020..0fd166bd80d37 100644 --- a/system/component_state_monitor/launch/component_state_monitor.launch.py +++ b/system/autoware_component_state_monitor/launch/component_state_monitor.launch.py @@ -39,7 +39,7 @@ def create_topic_monitor_name(row): def create_topic_monitor_node(row): tf_mode = "" if "topic_type" in row["args"] else "_tf" - package = FindPackageShare("topic_state_monitor") + package = FindPackageShare("autoware_topic_state_monitor") include = PathJoinSubstitution([package, f"launch/topic_state_monitor{tf_mode}.launch.xml"]) diag_name = create_diagnostic_name(row) arguments = [("diag_name", diag_name)] + [(k, str(v)) for k, v in row["args"].items()] @@ -62,8 +62,8 @@ def launch_setup(context, *args, **kwargs): component = ComposableNode( namespace="component_state_monitor", name="component", - package="component_state_monitor", - plugin="component_state_monitor::StateMonitor", + package="autoware_component_state_monitor", + plugin="autoware::component_state_monitor::StateMonitor", parameters=[{"topic_monitor_names": topic_monitor_names}, topic_monitor_param], ) container = ComposableNodeContainer( diff --git a/system/component_state_monitor/package.xml b/system/autoware_component_state_monitor/package.xml similarity index 71% rename from system/component_state_monitor/package.xml rename to system/autoware_component_state_monitor/package.xml index b92dd4af9517f..91e129f0b8cf0 100644 --- a/system/component_state_monitor/package.xml +++ b/system/autoware_component_state_monitor/package.xml @@ -1,10 +1,11 @@ - component_state_monitor - 0.40.0 - The component_state_monitor package + autoware_component_state_monitor + 0.41.0 + The autoware_component_state_monitor package Takagi, Isamu + Junya Sasaki Apache License 2.0 ament_cmake_auto @@ -15,7 +16,7 @@ rclcpp_components tier4_system_msgs - topic_state_monitor + autoware_topic_state_monitor ament_lint_auto autoware_lint_common diff --git a/system/component_state_monitor/src/main.cpp b/system/autoware_component_state_monitor/src/main.cpp similarity index 95% rename from system/component_state_monitor/src/main.cpp rename to system/autoware_component_state_monitor/src/main.cpp index c87963b1b21c0..43278c7ae894c 100644 --- a/system/component_state_monitor/src/main.cpp +++ b/system/autoware_component_state_monitor/src/main.cpp @@ -18,7 +18,7 @@ #include #include -namespace component_state_monitor +namespace autoware::component_state_monitor { // clang-format off @@ -129,7 +129,7 @@ void StateMonitor::on_diag(const DiagnosticArray::ConstSharedPtr msg) } } -} // namespace component_state_monitor +} // namespace autoware::component_state_monitor #include -RCLCPP_COMPONENTS_REGISTER_NODE(component_state_monitor::StateMonitor) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::component_state_monitor::StateMonitor) diff --git a/system/component_state_monitor/src/main.hpp b/system/autoware_component_state_monitor/src/main.hpp similarity index 95% rename from system/component_state_monitor/src/main.hpp rename to system/autoware_component_state_monitor/src/main.hpp index 73f57f56b4f87..81904553bb272 100644 --- a/system/component_state_monitor/src/main.hpp +++ b/system/autoware_component_state_monitor/src/main.hpp @@ -24,7 +24,7 @@ #include #include -namespace component_state_monitor +namespace autoware::component_state_monitor { // clang-format off @@ -73,6 +73,6 @@ class StateMonitor : public rclcpp::Node void on_diag(const DiagnosticArray::ConstSharedPtr msg); }; -} // namespace component_state_monitor +} // namespace autoware::component_state_monitor #endif // MAIN_HPP_ diff --git a/system/autoware_default_adapi/CHANGELOG.rst b/system/autoware_default_adapi/CHANGELOG.rst index 438ee15f16f61..0bfb5137f9f23 100644 --- a/system/autoware_default_adapi/CHANGELOG.rst +++ b/system/autoware_default_adapi/CHANGELOG.rst @@ -2,6 +2,34 @@ Changelog for package autoware_default_adapi ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware` prefix for `component_state_monitor` and its dependencies (`#9961 `_) +* feat: apply `autoware\_` prefix for `diagnostic_graph_utils` (`#9968 `_) +* feat: apply `autoware\_` prefix for `default_ad_api_helpers` (`#9965 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Takagi, Isamu +* feat(autoware_component_interface_specs_universe!): rename package (`#9753 `_) +* fix(obstacle_stop_planner): migrate planning factor (`#9939 `_) + * fix(obstacle_stop_planner): migrate planning factor + * fix(autoware_default_adapi): add coversion map + --------- +* feat(planning_factor)!: remove velocity_factor, steering_factor and introduce planning_factor (`#9927 `_) + Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> + Co-authored-by: satoshi-ota +* feat(autoware_default_adapi): release adapi v1.6.0 (`#9704 `_) + * feat: reject clearing route during autonomous mode + * feat: modify check and relay door service + * fix door condition + * fix error and add option + * update v1.6.0 + --------- +* fix(autoware_default_adapi): fix bugprone-branch-clone (`#9726 `_) + fix: bugprone-error +* Contributors: Fumiya Watanabe, Junya Sasaki, Mamoru Sobue, Ryohsuke Mitsudome, Satoshi OTA, Takagi, Isamu, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/autoware_default_adapi/config/default_adapi.param.yaml b/system/autoware_default_adapi/config/default_adapi.param.yaml index ddbf103878953..f2782e313e785 100644 --- a/system/autoware_default_adapi/config/default_adapi.param.yaml +++ b/system/autoware_default_adapi/config/default_adapi.param.yaml @@ -6,3 +6,7 @@ ros__parameters: require_accept_start: false stop_check_duration: 1.0 + +/adapi/node/vehicle_door: + ros__parameters: + check_autoware_control: true diff --git a/system/autoware_default_adapi/document/autoware-state.md b/system/autoware_default_adapi/document/autoware-state.md index eb31a4d41bc1d..cb4d95af14c68 100644 --- a/system/autoware_default_adapi/document/autoware-state.md +++ b/system/autoware_default_adapi/document/autoware-state.md @@ -3,7 +3,7 @@ ## Overview Since `/autoware/state` was so widely used, this packages creates it from the states of AD API for backwards compatibility. -The diagnostic checks that ad_service_state_monitor used to perform have been replaced by component_state_monitor. +The diagnostic checks that ad_service_state_monitor used to perform have been replaced by autoware_component_state_monitor. The service `/autoware/shutdown` to change autoware state to finalizing is also supported for compatibility. ![autoware-state-architecture](images/autoware-state-architecture.drawio.svg) diff --git a/system/autoware_default_adapi/launch/test_default_adapi.launch.xml b/system/autoware_default_adapi/launch/test_default_adapi.launch.xml new file mode 100644 index 0000000000000..3a00beeb623d4 --- /dev/null +++ b/system/autoware_default_adapi/launch/test_default_adapi.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/system/autoware_default_adapi/package.xml b/system/autoware_default_adapi/package.xml index a24d0c8a30b62..7442710a81741 100644 --- a/system/autoware_default_adapi/package.xml +++ b/system/autoware_default_adapi/package.xml @@ -2,7 +2,7 @@ autoware_default_adapi - 0.40.0 + 0.41.0 The autoware_default_adapi package Takagi, Isamu Ryohsuke Mitsudome @@ -15,15 +15,15 @@ autoware_adapi_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs - autoware_component_interface_specs + autoware_component_interface_specs_universe autoware_component_interface_utils + autoware_diagnostic_graph_utils autoware_geography_utils autoware_motion_utils autoware_planning_msgs autoware_system_msgs autoware_vehicle_info_utils autoware_vehicle_msgs - diagnostic_graph_utils geographic_msgs nav_msgs rclcpp @@ -32,6 +32,7 @@ std_srvs tier4_api_msgs tier4_control_msgs + tier4_planning_msgs tier4_system_msgs python3-flask diff --git a/system/autoware_default_adapi/script/web_server.py b/system/autoware_default_adapi/script/web_server.py index 1cbf30133c46b..bbd3d72bd0e06 100755 --- a/system/autoware_default_adapi/script/web_server.py +++ b/system/autoware_default_adapi/script/web_server.py @@ -49,7 +49,7 @@ def convert_dict(msg): def spin_ros_node(): global cli - node = Node("ad_api_default_web_server") + node = Node("adapi_default_web_server") cli = create_service(node, InterfaceVersion, "/api/interface/version") rclpy.spin(node) diff --git a/system/autoware_default_adapi/src/diagnostics.hpp b/system/autoware_default_adapi/src/diagnostics.hpp index b382887aaa694..dfd94d9b06f2a 100644 --- a/system/autoware_default_adapi/src/diagnostics.hpp +++ b/system/autoware_default_adapi/src/diagnostics.hpp @@ -15,7 +15,7 @@ #ifndef DIAGNOSTICS_HPP_ #define DIAGNOSTICS_HPP_ -#include "diagnostic_graph_utils/subscription.hpp" +#include "autoware/diagnostic_graph_utils/subscription.hpp" #include @@ -31,14 +31,14 @@ class DiagnosticsNode : public rclcpp::Node explicit DiagnosticsNode(const rclcpp::NodeOptions & options); private: - using DiagGraph = diagnostic_graph_utils::DiagGraph; - using DiagUnit = diagnostic_graph_utils::DiagUnit; - using DiagLink = diagnostic_graph_utils::DiagLink; + using DiagGraph = autoware::diagnostic_graph_utils::DiagGraph; + using DiagUnit = autoware::diagnostic_graph_utils::DiagUnit; + using DiagLink = autoware::diagnostic_graph_utils::DiagLink; void on_create(DiagGraph::ConstSharedPtr graph); void on_update(DiagGraph::ConstSharedPtr graph); rclcpp::Publisher::SharedPtr pub_struct_; rclcpp::Publisher::SharedPtr pub_status_; - diagnostic_graph_utils::DiagGraphSubscription sub_graph_; + autoware::diagnostic_graph_utils::DiagGraphSubscription sub_graph_; }; } // namespace autoware::default_adapi diff --git a/system/autoware_default_adapi/src/fail_safe.hpp b/system/autoware_default_adapi/src/fail_safe.hpp index c5ee08643e802..61141c2453ebc 100644 --- a/system/autoware_default_adapi/src/fail_safe.hpp +++ b/system/autoware_default_adapi/src/fail_safe.hpp @@ -16,7 +16,7 @@ #define FAIL_SAFE_HPP_ #include -#include +#include #include #include @@ -34,7 +34,7 @@ class FailSafeNode : public rclcpp::Node private: using MrmState = autoware::adapi_specs::fail_safe::MrmState::Message; Pub pub_mrm_state_; - Sub sub_mrm_state_; + Sub sub_mrm_state_; MrmState prev_state_; void on_state(const MrmState::ConstSharedPtr msg); }; diff --git a/system/autoware_default_adapi/src/interface.cpp b/system/autoware_default_adapi/src/interface.cpp index fb924f06f42ea..6535dcfca30fd 100644 --- a/system/autoware_default_adapi/src/interface.cpp +++ b/system/autoware_default_adapi/src/interface.cpp @@ -21,7 +21,7 @@ InterfaceNode::InterfaceNode(const rclcpp::NodeOptions & options) : Node("interf { const auto on_interface_version = [](auto, auto res) { res->major = 1; - res->minor = 5; + res->minor = 6; res->patch = 0; }; diff --git a/system/autoware_default_adapi/src/localization.hpp b/system/autoware_default_adapi/src/localization.hpp index ef4ffd52a6718..3575f6a027f6e 100644 --- a/system/autoware_default_adapi/src/localization.hpp +++ b/system/autoware_default_adapi/src/localization.hpp @@ -16,7 +16,7 @@ #define LOCALIZATION_HPP_ #include -#include +#include #include // This file should be included after messages. @@ -34,8 +34,8 @@ class LocalizationNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_cli_; Srv srv_initialize_; Pub pub_state_; - Cli cli_initialize_; - Sub sub_state_; + Cli cli_initialize_; + Sub sub_state_; void on_initialize( const autoware::adapi_specs::localization::Initialize::Service::Request::SharedPtr req, diff --git a/system/autoware_default_adapi/src/motion.cpp b/system/autoware_default_adapi/src/motion.cpp index dd7d842c3f26f..36a08a5b0857a 100644 --- a/system/autoware_default_adapi/src/motion.cpp +++ b/system/autoware_default_adapi/src/motion.cpp @@ -121,8 +121,8 @@ void MotionNode::update_pause(const State state) void MotionNode::change_pause(bool pause) { if (!is_calling_set_pause_ && cli_set_pause_->service_is_ready()) { - const auto req = - std::make_shared(); + const auto req = std::make_shared< + autoware::component_interface_specs_universe::control::SetPause::Service::Request>(); req->pause = pause; is_calling_set_pause_ = true; cli_set_pause_->async_send_request(req, [this](auto) { is_calling_set_pause_ = false; }); @@ -135,14 +135,15 @@ void MotionNode::on_timer() } void MotionNode::on_is_paused( - const autoware::component_interface_specs::control::IsPaused::Message::ConstSharedPtr msg) + const autoware::component_interface_specs_universe::control::IsPaused::Message::ConstSharedPtr + msg) { is_paused_ = msg->data; update_state(); } -void MotionNode::on_is_start_requested( - const autoware::component_interface_specs::control::IsStartRequested::Message::ConstSharedPtr msg) +void MotionNode::on_is_start_requested(const autoware::component_interface_specs_universe::control:: + IsStartRequested::Message::ConstSharedPtr msg) { is_start_requested_ = msg->data; update_state(); diff --git a/system/autoware_default_adapi/src/motion.hpp b/system/autoware_default_adapi/src/motion.hpp index ce9cab6eb4bc7..6d099be0a5d6a 100644 --- a/system/autoware_default_adapi/src/motion.hpp +++ b/system/autoware_default_adapi/src/motion.hpp @@ -16,7 +16,7 @@ #define MOTION_HPP_ #include -#include +#include #include #include #include @@ -39,9 +39,10 @@ class MotionNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_cli_; Srv srv_accept_; Pub pub_state_; - Cli cli_set_pause_; - Sub sub_is_paused_; - Sub sub_is_start_requested_; + Cli cli_set_pause_; + Sub sub_is_paused_; + Sub + sub_is_start_requested_; enum class State { Unknown, Pausing, Paused, Starting, Resuming, Resumed, Moving }; State state_; @@ -58,10 +59,10 @@ class MotionNode : public rclcpp::Node void change_pause(bool pause); void on_timer(); void on_is_paused( - const autoware::component_interface_specs::control::IsPaused::Message::ConstSharedPtr msg); - void on_is_start_requested( - const autoware::component_interface_specs::control::IsStartRequested::Message::ConstSharedPtr + const autoware::component_interface_specs_universe::control::IsPaused::Message::ConstSharedPtr msg); + void on_is_start_requested(const autoware::component_interface_specs_universe::control:: + IsStartRequested::Message::ConstSharedPtr msg); void on_accept( const autoware::adapi_specs::motion::AcceptStart::Service::Request::SharedPtr req, const autoware::adapi_specs::motion::AcceptStart::Service::Response::SharedPtr res); diff --git a/system/autoware_default_adapi/src/operation_mode.hpp b/system/autoware_default_adapi/src/operation_mode.hpp index fdfe9ce237626..43824b4f55899 100644 --- a/system/autoware_default_adapi/src/operation_mode.hpp +++ b/system/autoware_default_adapi/src/operation_mode.hpp @@ -16,7 +16,7 @@ #define OPERATION_MODE_HPP_ #include -#include +#include #include #include @@ -46,9 +46,9 @@ class OperationModeNode : public rclcpp::Node using ChangeToLocal = autoware::adapi_specs::operation_mode::ChangeToLocal; using ChangeToRemote = autoware::adapi_specs::operation_mode::ChangeToRemote; using OperationModeRequest = - autoware::component_interface_specs::system::ChangeOperationMode::Service::Request; + autoware::component_interface_specs_universe::system::ChangeOperationMode::Service::Request; using AutowareControlRequest = - autoware::component_interface_specs::system::ChangeAutowareControl::Service::Request; + autoware::component_interface_specs_universe::system::ChangeAutowareControl::Service::Request; using OperationModeAvailability = tier4_system_msgs::msg::OperationModeAvailability; OperationModeState::Message curr_state_; @@ -64,9 +64,9 @@ class OperationModeNode : public rclcpp::Node Srv srv_remote_mode_; Srv srv_enable_control_; Srv srv_disable_control_; - Sub sub_state_; - Cli cli_mode_; - Cli cli_control_; + Sub sub_state_; + Cli cli_mode_; + Cli cli_control_; rclcpp::Subscription::SharedPtr sub_availability_; void on_change_to_stop( diff --git a/system/autoware_default_adapi/src/perception.cpp b/system/autoware_default_adapi/src/perception.cpp index 3e858e7763baf..35ad31f437f80 100644 --- a/system/autoware_default_adapi/src/perception.cpp +++ b/system/autoware_default_adapi/src/perception.cpp @@ -50,9 +50,8 @@ uint8_t PerceptionNode::mapping( } } -void PerceptionNode::object_recognize( - const autoware::component_interface_specs::perception::ObjectRecognition::Message::ConstSharedPtr - msg) +void PerceptionNode::object_recognize(const autoware::component_interface_specs_universe:: + perception::ObjectRecognition::Message::ConstSharedPtr msg) { DynamicObjectArray::Message objects; objects.header = msg->header; diff --git a/system/autoware_default_adapi/src/perception.hpp b/system/autoware_default_adapi/src/perception.hpp index 61fb2c4667f1d..4c640894d77b6 100644 --- a/system/autoware_default_adapi/src/perception.hpp +++ b/system/autoware_default_adapi/src/perception.hpp @@ -16,7 +16,7 @@ #define PERCEPTION_HPP_ #include -#include +#include #include #include @@ -41,9 +41,10 @@ class PerceptionNode : public rclcpp::Node private: Pub pub_object_recognized_; - Sub sub_object_recognized_; - void object_recognize(const autoware::component_interface_specs::perception::ObjectRecognition:: - Message::ConstSharedPtr msg); + Sub + sub_object_recognized_; + void object_recognize(const autoware::component_interface_specs_universe::perception:: + ObjectRecognition::Message::ConstSharedPtr msg); uint8_t mapping( std::unordered_map hash_map, uint8_t input, uint8_t default_value); }; diff --git a/system/autoware_default_adapi/src/planning.cpp b/system/autoware_default_adapi/src/planning.cpp index d89ef6c221666..98f1c846d202e 100644 --- a/system/autoware_default_adapi/src/planning.cpp +++ b/system/autoware_default_adapi/src/planning.cpp @@ -18,6 +18,7 @@ #include +#include #include #include #include @@ -25,6 +26,35 @@ namespace autoware::default_adapi { +const std::map direction_map = { + {PlanningFactor::SHIFT_RIGHT, SteeringFactor::RIGHT}, + {PlanningFactor::SHIFT_LEFT, SteeringFactor::LEFT}, + {PlanningFactor::TURN_RIGHT, SteeringFactor::RIGHT}, + {PlanningFactor::TURN_LEFT, SteeringFactor::LEFT}}; + +const std::map conversion_map = { + {"behavior_path_planner", PlanningBehavior::INTERSECTION}, + {"static_obstacle_avoidance", PlanningBehavior::AVOIDANCE}, + {"crosswalk", PlanningBehavior::CROSSWALK}, + {"goal_planner", PlanningBehavior::GOAL_PLANNER}, + {"intersection", PlanningBehavior::INTERSECTION}, + {"lane_change_left", PlanningBehavior::LANE_CHANGE}, + {"lane_change_right", PlanningBehavior::LANE_CHANGE}, + {"merge_from_private", PlanningBehavior::MERGE}, + {"no_stopping_area", PlanningBehavior::NO_STOPPING_AREA}, + {"blind_spot", PlanningBehavior::REAR_CHECK}, + {"obstacle_cruise_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"obstacle_stop_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"motion_velocity_planner", PlanningBehavior::ROUTE_OBSTACLE}, + {"walkway", PlanningBehavior::SIDEWALK}, + {"start_planner", PlanningBehavior::START_PLANNER}, + {"stop_line", PlanningBehavior::STOP_SIGN}, + {"surround_obstacle_checker", PlanningBehavior::SURROUNDING_OBSTACLE}, + {"traffic_light", PlanningBehavior::TRAFFIC_SIGNAL}, + {"detection_area", PlanningBehavior::USER_DEFINED_DETECTION_AREA}, + {"virtual_traffic_light", PlanningBehavior::VIRTUAL_TRAFFIC_LIGHT}, + {"run_out", PlanningBehavior::RUN_OUT}}; + template void concat(std::vector & v1, const std::vector & v2) { @@ -49,16 +79,122 @@ std::vector::SharedPtr> init_factors( } template -T merge_factors(const rclcpp::Time stamp, const std::vector & factors) +std::vector convert([[maybe_unused]] const std::vector & factors) +{ + static_assert(sizeof(T) == 0, "Only specializations of convert can be used."); + throw std::logic_error("Only specializations of convert can be used."); +} + +template <> +std::vector convert(const std::vector & factors) +{ + std::vector velocity_factors; + + for (const auto & factor : factors) { + if (factor.behavior != PlanningFactor::SLOW_DOWN && factor.behavior != PlanningFactor::STOP) { + continue; + } + + if (factor.control_points.empty()) { + continue; + } + + if (conversion_map.count(factor.module) == 0) { + continue; + } + + VelocityFactor velocity_factor; + velocity_factor.behavior = conversion_map.at(factor.module); + velocity_factor.pose = factor.control_points.front().pose; + velocity_factor.distance = factor.control_points.front().distance; + + velocity_factors.push_back(velocity_factor); + } + + return velocity_factors; +} + +template <> +std::vector convert(const std::vector & factors) { - T message; + std::vector steering_factors; + + for (const auto & factor : factors) { + if ( + factor.behavior != PlanningFactor::SHIFT_RIGHT && + factor.behavior != PlanningFactor::SHIFT_LEFT && + factor.behavior != PlanningFactor::TURN_RIGHT && + factor.behavior != PlanningFactor::TURN_LEFT) { + continue; + } + + if (factor.control_points.size() < 2) { + continue; + } + + if (conversion_map.count(factor.module) == 0) { + continue; + } + + if (direction_map.count(factor.behavior) == 0) { + continue; + } + + SteeringFactor steering_factor; + steering_factor.behavior = conversion_map.at(factor.module); + steering_factor.direction = direction_map.at(factor.behavior); + steering_factor.pose = std::array{ + factor.control_points.front().pose, factor.control_points.back().pose}; + steering_factor.distance = std::array{ + factor.control_points.front().distance, factor.control_points.back().distance}; + + steering_factors.push_back(steering_factor); + } + + return steering_factors; +} + +template +T merge_factors( + [[maybe_unused]] const rclcpp::Time stamp, + [[maybe_unused]] const std::vector & factors) +{ + static_assert(sizeof(T) == 0, "Only specializations of merge_factors can be used."); + throw std::logic_error("Only specializations of merge_factors can be used."); +} + +template <> +VelocityFactorArray merge_factors( + const rclcpp::Time stamp, const std::vector & factors) +{ + VelocityFactorArray message; message.header.stamp = stamp; message.header.frame_id = "map"; for (const auto & factor : factors) { - if (factor) { - concat(message.factors, factor->factors); + if (!factor) { + continue; } + + concat(message.factors, convert(factor->factors)); + } + return message; +} + +template <> +SteeringFactorArray merge_factors( + const rclcpp::Time stamp, const std::vector & factors) +{ + SteeringFactorArray message; + message.header.stamp = stamp; + message.header.frame_id = "map"; + + for (const auto & factor : factors) { + if (!factor) { + continue; + } + + concat(message.factors, convert(factor->factors)); } return message; } @@ -70,46 +206,34 @@ PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning stop_duration_ = declare_parameter("stop_duration", 1.0); stop_checker_ = std::make_unique(this, stop_duration_ + 1.0); - std::vector velocity_factor_topics = { - "/planning/velocity_factors/blind_spot", - "/planning/velocity_factors/crosswalk", - "/planning/velocity_factors/detection_area", - "/planning/velocity_factors/dynamic_obstacle_stop", - "/planning/velocity_factors/intersection", - "/planning/velocity_factors/merge_from_private", - "/planning/velocity_factors/no_stopping_area", - "/planning/velocity_factors/obstacle_stop", - "/planning/velocity_factors/obstacle_cruise", - "/planning/velocity_factors/occlusion_spot", - "/planning/velocity_factors/run_out", - "/planning/velocity_factors/stop_line", - "/planning/velocity_factors/surround_obstacle", - "/planning/velocity_factors/traffic_light", - "/planning/velocity_factors/virtual_traffic_light", - "/planning/velocity_factors/walkway", - "/planning/velocity_factors/motion_velocity_planner", - "/planning/velocity_factors/static_obstacle_avoidance", - "/planning/velocity_factors/dynamic_obstacle_avoidance", - "/planning/velocity_factors/avoidance_by_lane_change", - "/planning/velocity_factors/lane_change_left", - "/planning/velocity_factors/lane_change_right", - "/planning/velocity_factors/start_planner", - "/planning/velocity_factors/goal_planner"}; - - std::vector steering_factor_topics = { - "/planning/steering_factor/static_obstacle_avoidance", - "/planning/steering_factor/dynamic_obstacle_avoidance", - "/planning/steering_factor/avoidance_by_lane_change", - "/planning/steering_factor/intersection", - "/planning/steering_factor/lane_change_left", - "/planning/steering_factor/lane_change_right", - "/planning/steering_factor/start_planner", - "/planning/steering_factor/goal_planner"}; - - sub_velocity_factors_ = - init_factors(this, velocity_factors_, velocity_factor_topics); - sub_steering_factors_ = - init_factors(this, steering_factors_, steering_factor_topics); + std::vector factor_topics = { + "/planning/planning_factors/behavior_path_planner", + "/planning/planning_factors/blind_spot", + "/planning/planning_factors/crosswalk", + "/planning/planning_factors/detection_area", + "/planning/planning_factors/dynamic_obstacle_stop", + "/planning/planning_factors/intersection", + "/planning/planning_factors/merge_from_private", + "/planning/planning_factors/no_stopping_area", + "/planning/planning_factors/obstacle_cruise_planner", + "/planning/planning_factors/obstacle_stop_planner", + "/planning/planning_factors/occlusion_spot", + "/planning/planning_factors/run_out", + "/planning/planning_factors/stop_line", + "/planning/planning_factors/surround_obstacle_checker", + "/planning/planning_factors/traffic_light", + "/planning/planning_factors/virtual_traffic_light", + "/planning/planning_factors/walkway", + "/planning/planning_factors/motion_velocity_planner", + "/planning/planning_factors/static_obstacle_avoidance", + "/planning/planning_factors/dynamic_obstacle_avoidance", + "/planning/planning_factors/avoidance_by_lane_change", + "/planning/planning_factors/lane_change_left", + "/planning/planning_factors/lane_change_right", + "/planning/planning_factors/start_planner", + "/planning/planning_factors/goal_planner"}; + + sub_factors_ = init_factors(this, factors_, factor_topics); const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); adaptor.init_pub(pub_velocity_factors_); @@ -139,8 +263,8 @@ void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) void PlanningNode::on_timer() { using autoware_adapi_v1_msgs::msg::VelocityFactor; - auto velocity = merge_factors(now(), velocity_factors_); - auto steering = merge_factors(now(), steering_factors_); + auto velocity = merge_factors(now(), factors_); + auto steering = merge_factors(now(), factors_); // Set the distance if it is nan. if (trajectory_ && kinematic_state_) { @@ -164,6 +288,13 @@ void PlanningNode::on_timer() } } + for (auto & factor : steering.factors) { + if ((factor.status == SteeringFactor::UNKNOWN) && (!std::isnan(factor.distance.front()))) { + const auto is_turning = factor.distance.front() < 0.0; + factor.status = is_turning ? SteeringFactor::TURNING : SteeringFactor::APPROACHING; + } + } + pub_velocity_factors_->publish(velocity); pub_steering_factors_->publish(steering); } diff --git a/system/autoware_default_adapi/src/planning.hpp b/system/autoware_default_adapi/src/planning.hpp index 3a9b99e33a997..2204b92dc3568 100644 --- a/system/autoware_default_adapi/src/planning.hpp +++ b/system/autoware_default_adapi/src/planning.hpp @@ -16,12 +16,16 @@ #define PLANNING_HPP_ #include -#include -#include +#include +#include #include #include +#include +#include + #include +#include #include // This file should be included after messages. @@ -30,27 +34,33 @@ namespace autoware::default_adapi { +using autoware_adapi_v1_msgs::msg::PlanningBehavior; +using autoware_adapi_v1_msgs::msg::SteeringFactor; +using autoware_adapi_v1_msgs::msg::SteeringFactorArray; +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using tier4_planning_msgs::msg::PlanningFactor; +using tier4_planning_msgs::msg::PlanningFactorArray; + class PlanningNode : public rclcpp::Node { public: explicit PlanningNode(const rclcpp::NodeOptions & options); private: - using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; - using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; Pub pub_velocity_factors_; Pub pub_steering_factors_; - Sub sub_trajectory_; - Sub sub_kinematic_state_; - std::vector::SharedPtr> sub_velocity_factors_; - std::vector::SharedPtr> sub_steering_factors_; - std::vector velocity_factors_; - std::vector steering_factors_; + Sub sub_trajectory_; + Sub + sub_kinematic_state_; + std::vector::SharedPtr> sub_factors_; + std::vector factors_; rclcpp::TimerBase::SharedPtr timer_; using VehicleStopChecker = autoware::motion_utils::VehicleStopCheckerBase; - using Trajectory = autoware::component_interface_specs::planning::Trajectory::Message; - using KinematicState = autoware::component_interface_specs::localization::KinematicState::Message; + using Trajectory = autoware::component_interface_specs_universe::planning::Trajectory::Message; + using KinematicState = + autoware::component_interface_specs_universe::localization::KinematicState::Message; void on_trajectory(const Trajectory::ConstSharedPtr msg); void on_kinematic_state(const KinematicState::ConstSharedPtr msg); void on_timer(); diff --git a/system/autoware_default_adapi/src/routing.cpp b/system/autoware_default_adapi/src/routing.cpp index 4eaa98376b147..66bc7a8f2e357 100644 --- a/system/autoware_default_adapi/src/routing.cpp +++ b/system/autoware_default_adapi/src/routing.cpp @@ -68,6 +68,7 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", adaptor.init_cli(cli_operation_mode_, group_cli_); adaptor.init_sub(sub_operation_mode_, this, &RoutingNode::on_operation_mode); + is_autoware_control_ = false; is_auto_mode_ = false; state_.state = State::Message::UNKNOWN; } @@ -75,7 +76,7 @@ RoutingNode::RoutingNode(const rclcpp::NodeOptions & options) : Node("routing", void RoutingNode::change_stop_mode() { using OperationModeRequest = - autoware::component_interface_specs::system::ChangeOperationMode::Service::Request; + autoware::component_interface_specs_universe::system::ChangeOperationMode::Service::Request; if (is_auto_mode_) { const auto req = std::make_shared(); req->mode = OperationModeRequest::STOP; @@ -85,6 +86,7 @@ void RoutingNode::change_stop_mode() void RoutingNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg) { + is_autoware_control_ = msg->is_autoware_control_enabled; is_auto_mode_ = msg->mode == OperationModeState::Message::AUTONOMOUS; } @@ -119,7 +121,14 @@ void RoutingNode::on_clear_route( const autoware::adapi_specs::routing::ClearRoute::Service::Request::SharedPtr req, const autoware::adapi_specs::routing::ClearRoute::Service::Response::SharedPtr res) { - change_stop_mode(); + // For safety, do not clear the route while it is in use. + // https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/routing/clear_route/ + if (is_auto_mode_ && is_autoware_control_) { + res->status.success = false; + res->status.code = ResponseStatus::UNKNOWN; + res->status.message = "The route cannot be cleared while it is in use."; + return; + } res->status = conversion::convert_call(cli_clear_route_, req); } diff --git a/system/autoware_default_adapi/src/routing.hpp b/system/autoware_default_adapi/src/routing.hpp index 10eed606a5f6b..34cea7b558196 100644 --- a/system/autoware_default_adapi/src/routing.hpp +++ b/system/autoware_default_adapi/src/routing.hpp @@ -16,8 +16,8 @@ #define ROUTING_HPP_ #include -#include -#include +#include +#include #include #include @@ -33,9 +33,10 @@ class RoutingNode : public rclcpp::Node explicit RoutingNode(const rclcpp::NodeOptions & options); private: - using OperationModeState = autoware::component_interface_specs::system::OperationModeState; - using State = autoware::component_interface_specs::planning::RouteState; - using Route = autoware::component_interface_specs::planning::LaneletRoute; + using OperationModeState = + autoware::component_interface_specs_universe::system::OperationModeState; + using State = autoware::component_interface_specs_universe::planning::RouteState; + using Route = autoware::component_interface_specs_universe::planning::LaneletRoute; rclcpp::CallbackGroup::SharedPtr group_cli_; Pub pub_state_; @@ -45,13 +46,17 @@ class RoutingNode : public rclcpp::Node Srv srv_change_route_points_; Srv srv_change_route_; Srv srv_clear_route_; - Sub sub_state_; - Sub sub_route_; - Cli cli_set_waypoint_route_; - Cli cli_set_lanelet_route_; - Cli cli_clear_route_; - Cli cli_operation_mode_; - Sub sub_operation_mode_; + Sub sub_state_; + Sub sub_route_; + Cli + cli_set_waypoint_route_; + Cli + cli_set_lanelet_route_; + Cli cli_clear_route_; + Cli + cli_operation_mode_; + Sub sub_operation_mode_; + bool is_autoware_control_; bool is_auto_mode_; State::Message state_; diff --git a/system/autoware_default_adapi/src/utils/route_conversion.cpp b/system/autoware_default_adapi/src/utils/route_conversion.cpp index e8a748d52edee..25e8204770f8a 100644 --- a/system/autoware_default_adapi/src/utils/route_conversion.cpp +++ b/system/autoware_default_adapi/src/utils/route_conversion.cpp @@ -115,13 +115,13 @@ ExternalState convert_state(const InternalState & internal) const auto convert = [](InternalState::_state_type state) { switch(state) { // TODO(Takagi, Isamu): Add adapi state. - case InternalState::INITIALIZING: return ExternalState::UNSET; + case InternalState::INITIALIZING: return ExternalState::UNSET; // NOLINT case InternalState::UNSET: return ExternalState::UNSET; case InternalState::ROUTING: return ExternalState::UNSET; case InternalState::SET: return ExternalState::SET; case InternalState::REROUTING: return ExternalState::CHANGING; case InternalState::ARRIVED: return ExternalState::ARRIVED; - case InternalState::ABORTED: return ExternalState::SET; + case InternalState::ABORTED: return ExternalState::SET; // NOLINT case InternalState::INTERRUPTED: return ExternalState::SET; default: return ExternalState::UNKNOWN; } diff --git a/system/autoware_default_adapi/src/vehicle.cpp b/system/autoware_default_adapi/src/vehicle.cpp index fc35efff523c2..eb6f7afb03ff3 100644 --- a/system/autoware_default_adapi/src/vehicle.cpp +++ b/system/autoware_default_adapi/src/vehicle.cpp @@ -25,14 +25,16 @@ namespace autoware::default_adapi { -using GearReport = autoware::component_interface_specs::vehicle::GearStatus::Message; +using GearReport = autoware::component_interface_specs_universe::vehicle::GearStatus::Message; using ApiGear = autoware_adapi_v1_msgs::msg::Gear; using TurnIndicatorsReport = - autoware::component_interface_specs::vehicle::TurnIndicatorStatus::Message; + autoware::component_interface_specs_universe::vehicle::TurnIndicatorStatus::Message; using ApiTurnIndicator = autoware_adapi_v1_msgs::msg::TurnIndicators; -using HazardLightsReport = autoware::component_interface_specs::vehicle::HazardLightStatus::Message; +using HazardLightsReport = + autoware::component_interface_specs_universe::vehicle::HazardLightStatus::Message; using ApiHazardLight = autoware_adapi_v1_msgs::msg::HazardLights; -using MapProjectorInfo = autoware::component_interface_specs::map::MapProjectorInfo::Message; +using MapProjectorInfo = + autoware::component_interface_specs_universe::map::MapProjectorInfo::Message; std::unordered_map gear_type_ = { {GearReport::NONE, ApiGear::UNKNOWN}, {GearReport::NEUTRAL, ApiGear::NEUTRAL}, @@ -90,23 +92,20 @@ uint8_t VehicleNode::mapping( } } -void VehicleNode::kinematic_state( - const autoware::component_interface_specs::localization::KinematicState::Message::ConstSharedPtr - msg_ptr) +void VehicleNode::kinematic_state(const autoware::component_interface_specs_universe::localization:: + KinematicState::Message::ConstSharedPtr msg_ptr) { kinematic_state_msgs_ = msg_ptr; } -void VehicleNode::acceleration_status( - const autoware::component_interface_specs::localization::Acceleration::Message::ConstSharedPtr - msg_ptr) +void VehicleNode::acceleration_status(const autoware::component_interface_specs_universe:: + localization::Acceleration::Message::ConstSharedPtr msg_ptr) { acceleration_msgs_ = msg_ptr; } -void VehicleNode::steering_status( - const autoware::component_interface_specs::vehicle::SteeringStatus::Message::ConstSharedPtr - msg_ptr) +void VehicleNode::steering_status(const autoware::component_interface_specs_universe::vehicle:: + SteeringStatus::Message::ConstSharedPtr msg_ptr) { steering_status_msgs_ = msg_ptr; } @@ -127,7 +126,8 @@ void VehicleNode::hazard_light_status(const HazardLightsReport::ConstSharedPtr m } void VehicleNode::energy_status( - const autoware::component_interface_specs::vehicle::EnergyStatus::Message::ConstSharedPtr msg_ptr) + const autoware::component_interface_specs_universe::vehicle::EnergyStatus::Message::ConstSharedPtr + msg_ptr) { energy_status_msgs_ = msg_ptr; } diff --git a/system/autoware_default_adapi/src/vehicle.hpp b/system/autoware_default_adapi/src/vehicle.hpp index 9bb83cb1613e9..750ef9dd14c57 100644 --- a/system/autoware_default_adapi/src/vehicle.hpp +++ b/system/autoware_default_adapi/src/vehicle.hpp @@ -16,9 +16,9 @@ #define VEHICLE_HPP_ #include -#include -#include -#include +#include +#include +#include #include #include @@ -42,57 +42,52 @@ class VehicleNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_cli_; Pub pub_kinematics_; Pub pub_status_; - Sub sub_kinematic_state_; - Sub sub_acceleration_; - Sub sub_steering_; - Sub sub_gear_state_; - Sub sub_turn_indicator_; - Sub sub_hazard_light_; - Sub sub_energy_level_; - Sub sub_map_projector_info_; + Sub + sub_kinematic_state_; + Sub sub_acceleration_; + Sub sub_steering_; + Sub sub_gear_state_; + Sub + sub_turn_indicator_; + Sub sub_hazard_light_; + Sub sub_energy_level_; + Sub sub_map_projector_info_; rclcpp::TimerBase::SharedPtr timer_; - autoware::component_interface_specs::localization::KinematicState::Message::ConstSharedPtr - kinematic_state_msgs_; - autoware::component_interface_specs::localization::Acceleration::Message::ConstSharedPtr + autoware::component_interface_specs_universe::localization::KinematicState::Message:: + ConstSharedPtr kinematic_state_msgs_; + autoware::component_interface_specs_universe::localization::Acceleration::Message::ConstSharedPtr acceleration_msgs_; - autoware::component_interface_specs::vehicle::SteeringStatus::Message::ConstSharedPtr + autoware::component_interface_specs_universe::vehicle::SteeringStatus::Message::ConstSharedPtr steering_status_msgs_; - autoware::component_interface_specs::vehicle::GearStatus::Message::ConstSharedPtr + autoware::component_interface_specs_universe::vehicle::GearStatus::Message::ConstSharedPtr gear_status_msgs_; - autoware::component_interface_specs::vehicle::TurnIndicatorStatus::Message::ConstSharedPtr - turn_indicator_status_msgs_; - autoware::component_interface_specs::vehicle::HazardLightStatus::Message::ConstSharedPtr + autoware::component_interface_specs_universe::vehicle::TurnIndicatorStatus::Message:: + ConstSharedPtr turn_indicator_status_msgs_; + autoware::component_interface_specs_universe::vehicle::HazardLightStatus::Message::ConstSharedPtr hazard_light_status_msgs_; - autoware::component_interface_specs::vehicle::EnergyStatus::Message::ConstSharedPtr + autoware::component_interface_specs_universe::vehicle::EnergyStatus::Message::ConstSharedPtr energy_status_msgs_; - autoware::component_interface_specs::map::MapProjectorInfo::Message::ConstSharedPtr + autoware::component_interface_specs_universe::map::MapProjectorInfo::Message::ConstSharedPtr map_projector_info_; - void kinematic_state( - const autoware::component_interface_specs::localization::KinematicState::Message::ConstSharedPtr - msg_ptr); - void acceleration_status( - const autoware::component_interface_specs::localization::Acceleration::Message::ConstSharedPtr - msg_ptr); - void steering_status( - const autoware::component_interface_specs::vehicle::SteeringStatus::Message::ConstSharedPtr - msg_ptr); + void kinematic_state(const autoware::component_interface_specs_universe::localization:: + KinematicState::Message::ConstSharedPtr msg_ptr); + void acceleration_status(const autoware::component_interface_specs_universe::localization:: + Acceleration::Message::ConstSharedPtr msg_ptr); + void steering_status(const autoware::component_interface_specs_universe::vehicle::SteeringStatus:: + Message::ConstSharedPtr msg_ptr); void gear_status( - const autoware::component_interface_specs::vehicle::GearStatus::Message::ConstSharedPtr - msg_ptr); - void turn_indicator_status( - const autoware::component_interface_specs::vehicle::TurnIndicatorStatus::Message::ConstSharedPtr - msg_ptr); - void map_projector_info( - const autoware::component_interface_specs::map::MapProjectorInfo::Message::ConstSharedPtr - msg_ptr); - void hazard_light_status( - const autoware::component_interface_specs::vehicle::HazardLightStatus::Message::ConstSharedPtr - msg_ptr); - void energy_status( - const autoware::component_interface_specs::vehicle::EnergyStatus::Message::ConstSharedPtr + const autoware::component_interface_specs_universe::vehicle::GearStatus::Message::ConstSharedPtr msg_ptr); + void turn_indicator_status(const autoware::component_interface_specs_universe::vehicle:: + TurnIndicatorStatus::Message::ConstSharedPtr msg_ptr); + void map_projector_info(const autoware::component_interface_specs_universe::map:: + MapProjectorInfo::Message::ConstSharedPtr msg_ptr); + void hazard_light_status(const autoware::component_interface_specs_universe::vehicle:: + HazardLightStatus::Message::ConstSharedPtr msg_ptr); + void energy_status(const autoware::component_interface_specs_universe::vehicle::EnergyStatus:: + Message::ConstSharedPtr msg_ptr); uint8_t mapping( std::unordered_map hash_map, uint8_t input, uint8_t default_value); void publish_kinematics(); diff --git a/system/autoware_default_adapi/src/vehicle_door.cpp b/system/autoware_default_adapi/src/vehicle_door.cpp index 23588bdf4597a..d3ad5d8ddccee 100644 --- a/system/autoware_default_adapi/src/vehicle_door.cpp +++ b/system/autoware_default_adapi/src/vehicle_door.cpp @@ -24,18 +24,50 @@ VehicleDoorNode::VehicleDoorNode(const rclcpp::NodeOptions & options) { const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - adaptor.relay_service(cli_command_, srv_command_, group_cli_, 3.0); - adaptor.relay_service(cli_layout_, srv_layout_, group_cli_, 3.0); + adaptor.relay_service(cli_layout_, srv_layout_, group_cli_); + adaptor.init_cli(cli_command_, group_cli_); + adaptor.init_srv(srv_command_, this, &VehicleDoorNode::on_command); adaptor.init_pub(pub_status_); adaptor.init_sub(sub_status_, this, &VehicleDoorNode::on_status); + adaptor.init_sub(sub_operation_mode_, this, &VehicleDoorNode::on_operation_mode); + + check_autoware_control_ = declare_parameter("check_autoware_control"); + is_autoware_control_ = false; + is_stop_mode_ = false; +} + +void VehicleDoorNode::on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg) +{ + is_autoware_control_ = msg->is_autoware_control_enabled; + is_stop_mode_ = msg->mode == OperationModeState::Message::STOP; +} +void VehicleDoorNode::on_status(InternalDoorStatus::Message::ConstSharedPtr msg) +{ + utils::notify(pub_status_, status_, *msg, utils::ignore_stamp); } -void VehicleDoorNode::on_status( - autoware::component_interface_specs::vehicle::DoorStatus::Message::ConstSharedPtr msg) +void VehicleDoorNode::on_command( + const ExternalDoorCommand::Service::Request::SharedPtr req, + const ExternalDoorCommand::Service::Response::SharedPtr res) { - utils::notify( - pub_status_, status_, *msg, - utils::ignore_stamp); + // For safety, do not open the door if the vehicle is not stopped. + // https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/ad-api/list/api/vehicle/doors/command/ + if (!is_stop_mode_ || (!is_autoware_control_ && check_autoware_control_)) { + bool is_open = false; + for (const auto & door : req->doors) { + if (door.command == autoware_adapi_v1_msgs::msg::DoorCommand::OPEN) { + is_open = true; + break; + } + } + if (is_open) { + res->status.success = false; + res->status.code = autoware_adapi_v1_msgs::msg::ResponseStatus::UNKNOWN; + res->status.message = "Doors cannot be opened if the vehicle is not stopped."; + return; + } + } + autoware::component_interface_utils::status::copy(cli_command_->call(req), res); } } // namespace autoware::default_adapi diff --git a/system/autoware_default_adapi/src/vehicle_door.hpp b/system/autoware_default_adapi/src/vehicle_door.hpp index 50312a38a4cb7..b0d6151ca3e8b 100644 --- a/system/autoware_default_adapi/src/vehicle_door.hpp +++ b/system/autoware_default_adapi/src/vehicle_door.hpp @@ -16,7 +16,9 @@ #define VEHICLE_DOOR_HPP_ #include -#include +#include +#include +#include #include #include @@ -33,16 +35,34 @@ class VehicleDoorNode : public rclcpp::Node explicit VehicleDoorNode(const rclcpp::NodeOptions & options); private: - void on_status( - autoware::component_interface_specs::vehicle::DoorStatus::Message::ConstSharedPtr msg); + using OperationModeState = + autoware::component_interface_specs_universe::system::OperationModeState; + using InternalDoorStatus = autoware::component_interface_specs_universe::vehicle::DoorStatus; + using InternalDoorLayout = autoware::component_interface_specs_universe::vehicle::DoorLayout; + using InternalDoorCommand = autoware::component_interface_specs_universe::vehicle::DoorCommand; + using ExternalDoorStatus = autoware::adapi_specs::vehicle::DoorStatus; + using ExternalDoorLayout = autoware::adapi_specs::vehicle::DoorLayout; + using ExternalDoorCommand = autoware::adapi_specs::vehicle::DoorCommand; + + void on_operation_mode(const OperationModeState::Message::ConstSharedPtr msg); + void on_status(InternalDoorStatus::Message::ConstSharedPtr msg); + void on_command( + const ExternalDoorCommand::Service::Request::SharedPtr req, + const ExternalDoorCommand::Service::Response::SharedPtr res); + rclcpp::CallbackGroup::SharedPtr group_cli_; - Srv srv_command_; - Srv srv_layout_; - Pub pub_status_; - Cli cli_command_; - Cli cli_layout_; - Sub sub_status_; - std::optional status_; + Srv srv_command_; + Srv srv_layout_; + Pub pub_status_; + Cli cli_command_; + Cli cli_layout_; + Sub sub_status_; + std::optional status_; + + bool check_autoware_control_; + bool is_autoware_control_; + bool is_stop_mode_; + Sub sub_operation_mode_; }; } // namespace autoware::default_adapi diff --git a/system/autoware_default_adapi/test/node/interface_version.py b/system/autoware_default_adapi/test/node/interface_version.py index c91889149cb0e..8db70ca5cba9a 100644 --- a/system/autoware_default_adapi/test/node/interface_version.py +++ b/system/autoware_default_adapi/test/node/interface_version.py @@ -30,7 +30,7 @@ if response.major != 1: exit(1) -if response.minor != 5: +if response.minor != 6: exit(1) if response.patch != 0: exit(1) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/CHANGELOG.rst b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/CHANGELOG.rst similarity index 95% rename from system/default_ad_api_helpers/ad_api_adaptors/CHANGELOG.rst rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/CHANGELOG.rst index b0d8f9b49cd59..488e0dc01c50e 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/CHANGELOG.rst +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/CHANGELOG.rst @@ -1,7 +1,15 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package ad_api_adaptors +Changelog for package autoware_ad_api_adaptors ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `default_ad_api_helpers` (`#9965 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Takagi, Isamu +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/CMakeLists.txt similarity index 77% rename from system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/CMakeLists.txt index c14f71571d272..a9f443e919411 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(ad_api_adaptors) +project(autoware_adapi_adaptors) find_package(autoware_cmake REQUIRED) autoware_package() @@ -10,13 +10,13 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "ad_api_adaptors::InitialPoseAdaptor" + PLUGIN "autoware::adapi_adaptors::InitialPoseAdaptor" EXECUTABLE initial_pose_adaptor_node EXECUTOR MultiThreadedExecutor ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "ad_api_adaptors::RoutingAdaptor" + PLUGIN "autoware::adapi_adaptors::RoutingAdaptor" EXECUTABLE routing_adaptor_node EXECUTOR SingleThreadedExecutor ) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/README.md b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/README.md similarity index 94% rename from system/default_ad_api_helpers/ad_api_adaptors/README.md rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/README.md index 74c5e6f84e80b..24f2de82e564f 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/README.md +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/README.md @@ -1,4 +1,4 @@ -# ad_api_adaptors +# adapi_adaptors ## initial_pose_adaptor @@ -32,4 +32,4 @@ The clear API is called automatically before setting the route. ## parameters -{{ json_to_markdown("/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json") }} +{{ json_to_markdown("/system/autoware_default_adapi_helpers/adapi_adaptors/schema/adapi_adaptors.schema.json") }} diff --git a/system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/config/initial_pose.param.yaml similarity index 100% rename from system/default_ad_api_helpers/ad_api_adaptors/config/initial_pose.param.yaml rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/config/initial_pose.param.yaml diff --git a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/launch/rviz_adaptors.launch.xml similarity index 74% rename from system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/launch/rviz_adaptors.launch.xml index 855f57345ed15..253d786cd59e7 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/launch/rviz_adaptors.launch.xml @@ -2,9 +2,9 @@ - - - + + + @@ -12,7 +12,7 @@ - + diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/package.xml similarity index 83% rename from system/default_ad_api_helpers/ad_api_adaptors/package.xml rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/package.xml index 4fe6390df77e0..2febc33a7db47 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/package.xml @@ -1,12 +1,13 @@ - ad_api_adaptors - 0.40.0 - The ad_api_adaptors package + autoware_adapi_adaptors + 0.41.0 + The adapi_adaptors package Takagi, Isamu Ryohsuke Mitsudome Yukihiro Saito + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/schema/adapi_adaptors.schema.json similarity index 93% rename from system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/schema/adapi_adaptors.schema.json index bdc921e31dd23..dd3bb2b4a25ed 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/schema/ad_api_adaptors.schema.json +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/schema/adapi_adaptors.schema.json @@ -1,9 +1,9 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "ad_api_adaptors parameter", + "title": "adapi_adaptors parameter", "type": "object", "definitions": { - "ad_api_adaptors": { + "adapi_adaptors": { "type": "object", "properties": { "initial_pose_particle_covariance": { diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/initial_pose_adaptor.cpp similarity index 90% rename from system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/initial_pose_adaptor.cpp index 18235d966aa5e..f4aaada70b4a6 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/initial_pose_adaptor.cpp @@ -18,7 +18,7 @@ #include #include -namespace ad_api_adaptors +namespace autoware::adapi_adaptors { template using Future = typename rclcpp::Client::SharedFuture; @@ -35,7 +35,7 @@ std::array get_covariance_parameter(rclcpp::Node * node, const std:: } InitialPoseAdaptor::InitialPoseAdaptor(const rclcpp::NodeOptions & options) -: Node("initial_pose_adaptor", options), fitter_(this) +: Node("autoware_initial_pose_adaptor", options), fitter_(this) { rviz_particle_covariance_ = get_covariance_parameter(this, "initial_pose_particle_covariance"); sub_initial_pose_ = create_subscription( @@ -60,7 +60,7 @@ void InitialPoseAdaptor::on_initial_pose(const PoseWithCovarianceStamped::ConstS cli_initialize_->async_send_request(req); } -} // namespace ad_api_adaptors +} // namespace autoware::adapi_adaptors #include -RCLCPP_COMPONENTS_REGISTER_NODE(ad_api_adaptors::InitialPoseAdaptor) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::adapi_adaptors::InitialPoseAdaptor) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/initial_pose_adaptor.hpp similarity index 95% rename from system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/initial_pose_adaptor.hpp index 922c4c30b6254..1ea99394f25fc 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/initial_pose_adaptor.hpp @@ -22,7 +22,7 @@ #include -namespace ad_api_adaptors +namespace autoware::adapi_adaptors { class InitialPoseAdaptor : public rclcpp::Node @@ -41,6 +41,6 @@ class InitialPoseAdaptor : public rclcpp::Node void on_initial_pose(const PoseWithCovarianceStamped::ConstSharedPtr msg); }; -} // namespace ad_api_adaptors +} // namespace autoware::adapi_adaptors #endif // INITIAL_POSE_ADAPTOR_HPP_ diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/routing_adaptor.cpp similarity index 95% rename from system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/routing_adaptor.cpp index 1de86a621a9e4..a84d8604f7ce6 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/routing_adaptor.cpp @@ -16,11 +16,11 @@ #include -namespace ad_api_adaptors +namespace autoware::adapi_adaptors { RoutingAdaptor::RoutingAdaptor(const rclcpp::NodeOptions & options) -: Node("routing_adaptor", options) +: Node("autoware_routing_adaptor", options) { using std::placeholders::_1; @@ -109,7 +109,7 @@ void RoutingAdaptor::on_reroute(const PoseStamped::ConstSharedPtr pose) cli_reroute_->async_send_request(route); } -} // namespace ad_api_adaptors +} // namespace autoware::adapi_adaptors #include -RCLCPP_COMPONENTS_REGISTER_NODE(ad_api_adaptors::RoutingAdaptor) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::adapi_adaptors::RoutingAdaptor) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/routing_adaptor.hpp similarity index 96% rename from system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp rename to system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/routing_adaptor.hpp index b4577573b665b..4f8a18af5c066 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp +++ b/system/autoware_default_adapi_helpers/autoware_adapi_adaptors/src/routing_adaptor.hpp @@ -23,7 +23,7 @@ #include -namespace ad_api_adaptors +namespace autoware::adapi_adaptors { class RoutingAdaptor : public rclcpp::Node @@ -59,6 +59,6 @@ class RoutingAdaptor : public rclcpp::Node void on_reroute(const PoseStamped::ConstSharedPtr pose); }; -} // namespace ad_api_adaptors +} // namespace autoware::adapi_adaptors #endif // ROUTING_ADAPTOR_HPP_ diff --git a/system/default_ad_api_helpers/ad_api_visualizers/CHANGELOG.rst b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/CHANGELOG.rst similarity index 87% rename from system/default_ad_api_helpers/ad_api_visualizers/CHANGELOG.rst rename to system/autoware_default_adapi_helpers/autoware_adapi_visualizers/CHANGELOG.rst index d44fe830e75db..85f8701a228a7 100644 --- a/system/default_ad_api_helpers/ad_api_visualizers/CHANGELOG.rst +++ b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/CHANGELOG.rst @@ -1,7 +1,15 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package ad_api_visualizers +Changelog for package autoware_ad_api_visualizers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `default_ad_api_helpers` (`#9965 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Takagi, Isamu +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/__init__.py b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/autoware_adapi_visualizers/__init__.py similarity index 100% rename from system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/__init__.py rename to system/autoware_default_adapi_helpers/autoware_adapi_visualizers/autoware_adapi_visualizers/__init__.py diff --git a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/autoware_adapi_visualizers/planning_factors.py similarity index 98% rename from system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py rename to system/autoware_default_adapi_helpers/autoware_adapi_visualizers/autoware_adapi_visualizers/planning_factors.py index b60fa5365f2a2..27ff45276b9ef 100755 --- a/system/default_ad_api_helpers/ad_api_visualizers/ad_api_visualizers/planning_factors.py +++ b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/autoware_adapi_visualizers/planning_factors.py @@ -60,7 +60,7 @@ class PlanningFactorVisualizer(rclpy.node.Node): def __init__(self): - super().__init__("planning_factor_visualizer") + super().__init__("autoware_planning_factor_visualizer") self.front_offset = self.declare_parameter("front_offset", 0.0).value self.pub_velocity = self.create_publisher(MarkerArray, "/visualizer/velocity_factors", 1) self.pub_steering = self.create_publisher(MarkerArray, "/visualizer/steering_factors", 1) diff --git a/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/launch/autoware_adapi_visualizers.launch.xml b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/launch/autoware_adapi_visualizers.launch.xml new file mode 100644 index 0000000000000..90559864b9469 --- /dev/null +++ b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/launch/autoware_adapi_visualizers.launch.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/system/default_ad_api_helpers/ad_api_visualizers/package.xml b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/package.xml similarity index 81% rename from system/default_ad_api_helpers/ad_api_visualizers/package.xml rename to system/autoware_default_adapi_helpers/autoware_adapi_visualizers/package.xml index 2392cfaaface1..26da09537239e 100644 --- a/system/default_ad_api_helpers/ad_api_visualizers/package.xml +++ b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/package.xml @@ -1,12 +1,13 @@ - ad_api_visualizers - 0.40.0 - The ad_api_visualizers package + autoware_adapi_visualizers + 0.41.0 + The adapi_visualizers package Takagi, Isamu Ryohsuke Mitsudome Yukihiro Saito + Junya Sasaki Apache License 2.0 autoware_adapi_v1_msgs diff --git a/system/default_ad_api_helpers/ad_api_visualizers/resource/ad_api_visualizers b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/resource/autoware_adapi_visualizers similarity index 100% rename from system/default_ad_api_helpers/ad_api_visualizers/resource/ad_api_visualizers rename to system/autoware_default_adapi_helpers/autoware_adapi_visualizers/resource/autoware_adapi_visualizers diff --git a/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/setup.cfg b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/setup.cfg new file mode 100644 index 0000000000000..0eed433d69c43 --- /dev/null +++ b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/adapi_visualizers +[install] +install_scripts=$base/lib/adapi_visualizers diff --git a/system/default_ad_api_helpers/ad_api_visualizers/setup.py b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/setup.py similarity index 79% rename from system/default_ad_api_helpers/ad_api_visualizers/setup.py rename to system/autoware_default_adapi_helpers/autoware_adapi_visualizers/setup.py index ee4cf253b5288..57d7adc3ddd35 100644 --- a/system/default_ad_api_helpers/ad_api_visualizers/setup.py +++ b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/setup.py @@ -7,11 +7,11 @@ simplefilter("ignore", category=SetuptoolsDeprecationWarning) simplefilter("ignore", category=PkgResourcesDeprecationWarning) -package_name = "ad_api_visualizers" +package_name = "autoware_adapi_visualizers" setup( name=package_name, - version="0.40.0", + version="0.41.0", packages=[package_name], data_files=[ ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), @@ -22,10 +22,10 @@ zip_safe=True, maintainer="Takagi, Isamu", maintainer_email="isamu.takagi@tier4.jp", - description="The ad_api_visualizers package", + description="The adapi_visualizers package", license="Apache License 2.0", tests_require=["pytest"], entry_points={ - "console_scripts": ["planning_factors = ad_api_visualizers.planning_factors:main"], + "console_scripts": ["planning_factors = autoware_adapi_visualizers.planning_factors:main"], }, ) diff --git a/system/default_ad_api_helpers/ad_api_visualizers/test/test_copyright.py b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/test/test_copyright.py similarity index 100% rename from system/default_ad_api_helpers/ad_api_visualizers/test/test_copyright.py rename to system/autoware_default_adapi_helpers/autoware_adapi_visualizers/test/test_copyright.py diff --git a/system/default_ad_api_helpers/ad_api_visualizers/test/test_pep257.py b/system/autoware_default_adapi_helpers/autoware_adapi_visualizers/test/test_pep257.py similarity index 100% rename from system/default_ad_api_helpers/ad_api_visualizers/test/test_pep257.py rename to system/autoware_default_adapi_helpers/autoware_adapi_visualizers/test/test_pep257.py diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/CHANGELOG.rst b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/CHANGELOG.rst similarity index 93% rename from system/default_ad_api_helpers/automatic_pose_initializer/CHANGELOG.rst rename to system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/CHANGELOG.rst index 819a580c87f43..2028d4b4cdbcc 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/CHANGELOG.rst +++ b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/CHANGELOG.rst @@ -1,7 +1,15 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package automatic_pose_initializer +Changelog for package autoware_automatic_pose_initializer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `default_ad_api_helpers` (`#9965 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: Takagi, Isamu +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Revert "chore(package.xml): bump version to 0.39.0 (`#9587 `_)" diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/CMakeLists.txt similarity index 74% rename from system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt rename to system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/CMakeLists.txt index b777df8675bef..0e0b604d57935 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt +++ b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(automatic_pose_initializer) +project(autoware_automatic_pose_initializer) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "automatic_pose_initializer::AutomaticPoseInitializer" + PLUGIN "autoware::automatic_pose_initializer::AutomaticPoseInitializer" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR MultiThreadedExecutor ) diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/README.md b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/README.md similarity index 100% rename from system/default_ad_api_helpers/automatic_pose_initializer/README.md rename to system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/README.md diff --git a/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml new file mode 100644 index 0000000000000..0c4f62891c67a --- /dev/null +++ b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/package.xml similarity index 86% rename from system/default_ad_api_helpers/automatic_pose_initializer/package.xml rename to system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/package.xml index 13f15d7dc63fd..7a0613dd1168d 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml +++ b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/package.xml @@ -1,12 +1,13 @@ - automatic_pose_initializer - 0.40.0 + autoware_automatic_pose_initializer + 0.41.0 The automatic_pose_initializer package Takagi, Isamu Ryohsuke Mitsudome Yukihiro Saito + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/src/automatic_pose_initializer.cpp similarity index 86% rename from system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp rename to system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/src/automatic_pose_initializer.cpp index 53943af1a3f31..192eeae356e51 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp +++ b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/src/automatic_pose_initializer.cpp @@ -16,11 +16,11 @@ #include -namespace automatic_pose_initializer +namespace autoware::automatic_pose_initializer { AutomaticPoseInitializer::AutomaticPoseInitializer(const rclcpp::NodeOptions & options) -: Node("automatic_pose_initializer", options) +: Node("autoware_automatic_pose_initializer", options) { const auto adaptor = autoware::component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -47,7 +47,7 @@ void AutomaticPoseInitializer::on_timer() timer_->reset(); } -} // namespace automatic_pose_initializer +} // namespace autoware::automatic_pose_initializer #include -RCLCPP_COMPONENTS_REGISTER_NODE(automatic_pose_initializer::AutomaticPoseInitializer) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::automatic_pose_initializer::AutomaticPoseInitializer) diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/src/automatic_pose_initializer.hpp similarity index 93% rename from system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp rename to system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/src/automatic_pose_initializer.hpp index a22a19cc592d2..407a23a6d01e5 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp +++ b/system/autoware_default_adapi_helpers/autoware_automatic_pose_initializer/src/automatic_pose_initializer.hpp @@ -19,7 +19,7 @@ #include #include -namespace automatic_pose_initializer +namespace autoware::automatic_pose_initializer { class AutomaticPoseInitializer : public rclcpp::Node @@ -38,6 +38,6 @@ class AutomaticPoseInitializer : public rclcpp::Node State::Message state_; }; -} // namespace automatic_pose_initializer +} // namespace autoware::automatic_pose_initializer #endif // AUTOMATIC_POSE_INITIALIZER_HPP_ diff --git a/system/diagnostic_graph_aggregator/CHANGELOG.rst b/system/autoware_diagnostic_graph_aggregator/CHANGELOG.rst similarity index 93% rename from system/diagnostic_graph_aggregator/CHANGELOG.rst rename to system/autoware_diagnostic_graph_aggregator/CHANGELOG.rst index 51abe33dcfbdf..79b2dd9ff7345 100644 --- a/system/diagnostic_graph_aggregator/CHANGELOG.rst +++ b/system/autoware_diagnostic_graph_aggregator/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package diagnostic_graph_aggregator -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_diagnostic_graph_aggregator +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware` prefix for `component_state_monitor` and its dependencies (`#9961 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/system/diagnostic_graph_aggregator/CMakeLists.txt b/system/autoware_diagnostic_graph_aggregator/CMakeLists.txt similarity index 89% rename from system/diagnostic_graph_aggregator/CMakeLists.txt rename to system/autoware_diagnostic_graph_aggregator/CMakeLists.txt index 4f18407e2a108..69882ad10bd81 100644 --- a/system/diagnostic_graph_aggregator/CMakeLists.txt +++ b/system/autoware_diagnostic_graph_aggregator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(diagnostic_graph_aggregator) +project(autoware_diagnostic_graph_aggregator) find_package(autoware_cmake REQUIRED) autoware_package() @@ -30,8 +30,8 @@ ament_auto_add_library(aggregator SHARED target_include_directories(aggregator PRIVATE src/common) rclcpp_components_register_node(aggregator - PLUGIN "diagnostic_graph_aggregator::AggregatorNode" - EXECUTABLE aggregator_node + PLUGIN "autoware::diagnostic_graph_aggregator::AggregatorNode" + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) diff --git a/system/diagnostic_graph_aggregator/README.md b/system/autoware_diagnostic_graph_aggregator/README.md similarity index 94% rename from system/diagnostic_graph_aggregator/README.md rename to system/autoware_diagnostic_graph_aggregator/README.md index c0cd78e0610c2..892a2a381f324 100644 --- a/system/diagnostic_graph_aggregator/README.md +++ b/system/autoware_diagnostic_graph_aggregator/README.md @@ -1,4 +1,4 @@ -# diagnostic_graph_aggregator +# autoware_diagnostic_graph_aggregator ## Overview @@ -66,7 +66,7 @@ This is an example of a diagnostic graph configuration. The configuration can be - [module2.yaml](./example/graph/module2.yaml) ```bash -ros2 launch diagnostic_graph_aggregator example-main.launch.xml +ros2 launch autoware_diagnostic_graph_aggregator example-main.launch.xml ``` You can reuse the graph by making partial edits. For example, disable hardware checks for simulation. @@ -74,13 +74,13 @@ You can reuse the graph by making partial edits. For example, disable hardware c - [edit.yaml](./example/graph/edit.yaml) ```bash -ros2 launch diagnostic_graph_aggregator example-edit.launch.xml +ros2 launch autoware_diagnostic_graph_aggregator example-edit.launch.xml ``` ## Debug tools - [tree](./doc/tool/tree.md) -- [diagnostic_graph_utils](../diagnostic_graph_utils/README.md) +- [autoware_diagnostic_graph_utils](../autoware_diagnostic_graph_utils/README.md) ## Graph file format diff --git a/system/diagnostic_graph_aggregator/config/default.param.yaml b/system/autoware_diagnostic_graph_aggregator/config/default.param.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/config/default.param.yaml rename to system/autoware_diagnostic_graph_aggregator/config/default.param.yaml diff --git a/system/diagnostic_graph_aggregator/doc/format/edit.md b/system/autoware_diagnostic_graph_aggregator/doc/format/edit.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/edit.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/edit.md diff --git a/system/diagnostic_graph_aggregator/doc/format/edit/remove.md b/system/autoware_diagnostic_graph_aggregator/doc/format/edit/remove.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/edit/remove.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/edit/remove.md diff --git a/system/diagnostic_graph_aggregator/doc/format/graph.md b/system/autoware_diagnostic_graph_aggregator/doc/format/graph.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/graph.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/graph.md diff --git a/system/diagnostic_graph_aggregator/doc/format/path.md b/system/autoware_diagnostic_graph_aggregator/doc/format/path.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/path.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/path.md diff --git a/system/diagnostic_graph_aggregator/doc/format/unit.md b/system/autoware_diagnostic_graph_aggregator/doc/format/unit.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/unit.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/unit.md diff --git a/system/diagnostic_graph_aggregator/doc/format/unit/and.md b/system/autoware_diagnostic_graph_aggregator/doc/format/unit/and.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/unit/and.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/unit/and.md diff --git a/system/diagnostic_graph_aggregator/doc/format/unit/const.md b/system/autoware_diagnostic_graph_aggregator/doc/format/unit/const.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/unit/const.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/unit/const.md diff --git a/system/diagnostic_graph_aggregator/doc/format/unit/diag.md b/system/autoware_diagnostic_graph_aggregator/doc/format/unit/diag.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/unit/diag.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/unit/diag.md diff --git a/system/diagnostic_graph_aggregator/doc/format/unit/link.md b/system/autoware_diagnostic_graph_aggregator/doc/format/unit/link.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/unit/link.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/unit/link.md diff --git a/system/diagnostic_graph_aggregator/doc/format/unit/or.md b/system/autoware_diagnostic_graph_aggregator/doc/format/unit/or.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/unit/or.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/unit/or.md diff --git a/system/diagnostic_graph_aggregator/doc/format/unit/remap.md b/system/autoware_diagnostic_graph_aggregator/doc/format/unit/remap.md similarity index 100% rename from system/diagnostic_graph_aggregator/doc/format/unit/remap.md rename to system/autoware_diagnostic_graph_aggregator/doc/format/unit/remap.md diff --git a/system/diagnostic_graph_aggregator/doc/overview.drawio.svg b/system/autoware_diagnostic_graph_aggregator/doc/overview.drawio.svg similarity index 100% rename from system/diagnostic_graph_aggregator/doc/overview.drawio.svg rename to system/autoware_diagnostic_graph_aggregator/doc/overview.drawio.svg diff --git a/system/diagnostic_graph_aggregator/doc/tool/tree.md b/system/autoware_diagnostic_graph_aggregator/doc/tool/tree.md similarity index 83% rename from system/diagnostic_graph_aggregator/doc/tool/tree.md rename to system/autoware_diagnostic_graph_aggregator/doc/tool/tree.md index 12b664ecc48d6..2bbedcce4aeb4 100644 --- a/system/diagnostic_graph_aggregator/doc/tool/tree.md +++ b/system/autoware_diagnostic_graph_aggregator/doc/tool/tree.md @@ -5,13 +5,13 @@ This tool displays the graph structure of the configuration file in tree format. ## Usage ```bash -ros2 run diagnostic_graph_aggregator tree +ros2 run autoware_diagnostic_graph_aggregator tree ``` ## Examples ```bash -ros2 run diagnostic_graph_aggregator tree '$(find-pkg-share diagnostic_graph_aggregator)/example/graph/main.yaml' +ros2 run autoware_diagnostic_graph_aggregator tree '$(find-pkg-share autoware_diagnostic_graph_aggregator)/example/graph/main.yaml' ``` ```txt diff --git a/system/diagnostic_graph_aggregator/example/dummy-diags.py b/system/autoware_diagnostic_graph_aggregator/example/dummy-diags.py similarity index 100% rename from system/diagnostic_graph_aggregator/example/dummy-diags.py rename to system/autoware_diagnostic_graph_aggregator/example/dummy-diags.py diff --git a/system/autoware_diagnostic_graph_aggregator/example/example-edit.launch.xml b/system/autoware_diagnostic_graph_aggregator/example/example-edit.launch.xml new file mode 100644 index 0000000000000..1a15bbe929f6e --- /dev/null +++ b/system/autoware_diagnostic_graph_aggregator/example/example-edit.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/autoware_diagnostic_graph_aggregator/example/example-main.launch.xml b/system/autoware_diagnostic_graph_aggregator/example/example-main.launch.xml new file mode 100644 index 0000000000000..712da5128d09b --- /dev/null +++ b/system/autoware_diagnostic_graph_aggregator/example/example-main.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/diagnostic_graph_aggregator/example/graph/edit.yaml b/system/autoware_diagnostic_graph_aggregator/example/graph/edit.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/example/graph/edit.yaml rename to system/autoware_diagnostic_graph_aggregator/example/graph/edit.yaml diff --git a/system/diagnostic_graph_aggregator/example/graph/main.yaml b/system/autoware_diagnostic_graph_aggregator/example/graph/main.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/example/graph/main.yaml rename to system/autoware_diagnostic_graph_aggregator/example/graph/main.yaml diff --git a/system/diagnostic_graph_aggregator/example/graph/module1.yaml b/system/autoware_diagnostic_graph_aggregator/example/graph/module1.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/example/graph/module1.yaml rename to system/autoware_diagnostic_graph_aggregator/example/graph/module1.yaml diff --git a/system/diagnostic_graph_aggregator/example/graph/module2.yaml b/system/autoware_diagnostic_graph_aggregator/example/graph/module2.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/example/graph/module2.yaml rename to system/autoware_diagnostic_graph_aggregator/example/graph/module2.yaml diff --git a/system/autoware_diagnostic_graph_aggregator/launch/aggregator.launch.xml b/system/autoware_diagnostic_graph_aggregator/launch/aggregator.launch.xml new file mode 100644 index 0000000000000..89dac8e5f439b --- /dev/null +++ b/system/autoware_diagnostic_graph_aggregator/launch/aggregator.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/system/diagnostic_graph_aggregator/package.xml b/system/autoware_diagnostic_graph_aggregator/package.xml similarity index 84% rename from system/diagnostic_graph_aggregator/package.xml rename to system/autoware_diagnostic_graph_aggregator/package.xml index ae9266a3bf302..24a850760153a 100644 --- a/system/diagnostic_graph_aggregator/package.xml +++ b/system/autoware_diagnostic_graph_aggregator/package.xml @@ -1,10 +1,11 @@ - diagnostic_graph_aggregator - 0.40.0 + autoware_diagnostic_graph_aggregator + 0.41.0 The diagnostic_graph_aggregator package Takagi, Isamu + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/diagnostic_graph_aggregator/script/dump.py b/system/autoware_diagnostic_graph_aggregator/script/dump.py similarity index 100% rename from system/diagnostic_graph_aggregator/script/dump.py rename to system/autoware_diagnostic_graph_aggregator/script/dump.py diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/config.cpp similarity index 98% rename from system/diagnostic_graph_aggregator/src/common/graph/config.cpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/config.cpp index e622d089109f4..3b95d06befb8e 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/config.cpp @@ -29,7 +29,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { std::string resolve_substitution(const std::string & substitution, const TreeData & data) @@ -309,4 +309,4 @@ FileConfig TreeLoader::construct() return config; } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/common/graph/config.hpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/config.hpp similarity index 96% rename from system/diagnostic_graph_aggregator/src/common/graph/config.hpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/config.hpp index 935b65203afbe..f74f2ef721ee0 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/config.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/config.hpp @@ -23,7 +23,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { struct PathConfig @@ -98,6 +98,6 @@ class TreeLoader std::vector files_; }; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // COMMON__GRAPH__CONFIG_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/common/graph/data.cpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/data.cpp similarity index 95% rename from system/diagnostic_graph_aggregator/src/common/graph/data.cpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/data.cpp index e1d053e6d259c..60fb356d41910 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/data.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/data.cpp @@ -16,7 +16,7 @@ #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { TreeData TreeData::Load(const std::string & path) @@ -96,4 +96,4 @@ double TreeData::real(double fail) return yaml_.as(fail); } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/common/graph/data.hpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/data.hpp similarity index 93% rename from system/diagnostic_graph_aggregator/src/common/graph/data.hpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/data.hpp index 437e3793df398..6ddac7897746d 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/data.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/data.hpp @@ -23,7 +23,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { class TreeData @@ -51,6 +51,6 @@ class TreeData TreePath path_; }; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // COMMON__GRAPH__DATA_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/common/graph/error.cpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/error.cpp similarity index 93% rename from system/diagnostic_graph_aggregator/src/common/graph/error.cpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/error.cpp index 7a4336e62d8a3..976fc63aab8ab 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/error.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/error.cpp @@ -16,7 +16,7 @@ #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { TreePath::TreePath(const std::string & file) @@ -57,4 +57,4 @@ std::string TreePath::text() const return " (" + file_ + sep + node_ + ")"; } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/common/graph/error.hpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/error.hpp similarity index 97% rename from system/diagnostic_graph_aggregator/src/common/graph/error.hpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/error.hpp index d923e12caf783..cef345a1243ca 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/error.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/error.hpp @@ -18,7 +18,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { struct TreePath @@ -127,6 +127,6 @@ struct GraphStructure : public Exception using Exception::Exception; }; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // COMMON__GRAPH__ERROR_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/graph.cpp similarity index 95% rename from system/diagnostic_graph_aggregator/src/common/graph/graph.cpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/graph.cpp index 7977209b4905c..ff6c5a8db21ed 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/graph.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/graph.cpp @@ -22,7 +22,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { void Graph::create(const std::string & file, const std::string & id) @@ -77,4 +77,4 @@ DiagGraphStatus Graph::create_status(const rclcpp::Time & stamp) const Graph::Graph() = default; Graph::~Graph() = default; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/graph.hpp similarity index 94% rename from system/diagnostic_graph_aggregator/src/common/graph/graph.hpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/graph.hpp index 14a27dae9a539..13362613d9952 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/graph.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/graph.hpp @@ -24,7 +24,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { class Graph @@ -51,6 +51,6 @@ class Graph std::string id_; }; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // COMMON__GRAPH__GRAPH_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/loader.cpp similarity index 98% rename from system/diagnostic_graph_aggregator/src/common/graph/loader.cpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/loader.cpp index 5edef61340fe8..8a21a4f47951e 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/loader.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/loader.cpp @@ -26,7 +26,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { struct UnitLoader::GraphLinks @@ -190,4 +190,4 @@ std::unique_ptr GraphLoader::create_node(const UnitLoader & unit) throw UnknownUnitType(unit.data().path(), unit.type()); } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/common/graph/loader.hpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/loader.hpp similarity index 94% rename from system/diagnostic_graph_aggregator/src/common/graph/loader.hpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/loader.hpp index 226b3ab279b22..c9f6923c5bcfd 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/loader.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/loader.hpp @@ -22,7 +22,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { class UnitLoader @@ -62,6 +62,6 @@ class GraphLoader std::vector> links_; }; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // COMMON__GRAPH__LOADER_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/common/graph/names.hpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/names.hpp similarity index 83% rename from system/diagnostic_graph_aggregator/src/common/graph/names.hpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/names.hpp index aa5a9c46e7b37..36645b320f496 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/names.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/names.hpp @@ -15,7 +15,7 @@ #ifndef COMMON__GRAPH__NAMES_HPP_ #define COMMON__GRAPH__NAMES_HPP_ -namespace diagnostic_graph_aggregator::unit_name +namespace autoware::diagnostic_graph_aggregator::unit_name { constexpr char const * link = "link"; @@ -30,13 +30,13 @@ constexpr char const * warn = "warn"; constexpr char const * error = "error"; constexpr char const * stale = "stale"; -} // namespace diagnostic_graph_aggregator::unit_name +} // namespace autoware::diagnostic_graph_aggregator::unit_name -namespace diagnostic_graph_aggregator::edit_name +namespace autoware::diagnostic_graph_aggregator::edit_name { constexpr char const * remove = "remove"; -} // namespace diagnostic_graph_aggregator::edit_name +} // namespace autoware::diagnostic_graph_aggregator::edit_name #endif // COMMON__GRAPH__NAMES_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/common/graph/types.hpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/types.hpp similarity index 95% rename from system/diagnostic_graph_aggregator/src/common/graph/types.hpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/types.hpp index 6eae26a1f98c7..f7844b3d1b327 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/types.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/types.hpp @@ -31,7 +31,7 @@ #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { using diagnostic_msgs::msg::DiagnosticArray; @@ -59,6 +59,6 @@ class DiagUnit; class Graph; class UnitLoader; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // COMMON__GRAPH__TYPES_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/units.cpp similarity index 98% rename from system/diagnostic_graph_aggregator/src/common/graph/units.cpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/units.cpp index 5ebd603964add..3d7919027aae5 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/units.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/units.cpp @@ -20,7 +20,7 @@ #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { void UnitLink::initialize_object(BaseUnit * parent, BaseUnit * child) @@ -226,4 +226,4 @@ StaleUnit::StaleUnit(const UnitLoader & unit) : ConstUnit(unit) status_.level = DiagnosticStatus::STALE; } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/common/graph/units.hpp b/system/autoware_diagnostic_graph_aggregator/src/common/graph/units.hpp similarity index 98% rename from system/diagnostic_graph_aggregator/src/common/graph/units.hpp rename to system/autoware_diagnostic_graph_aggregator/src/common/graph/units.hpp index 323053db08385..248f4694c17ae 100644 --- a/system/diagnostic_graph_aggregator/src/common/graph/units.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/common/graph/units.hpp @@ -24,7 +24,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { class UnitLink @@ -225,6 +225,6 @@ class StaleUnit : public ConstUnit std::string type() const override { return unit_name::stale; } }; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // COMMON__GRAPH__UNITS_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/node/aggregator.cpp b/system/autoware_diagnostic_graph_aggregator/src/node/aggregator.cpp similarity index 94% rename from system/diagnostic_graph_aggregator/src/node/aggregator.cpp rename to system/autoware_diagnostic_graph_aggregator/src/node/aggregator.cpp index 4d2ec73bfceca..ac0fa587895e5 100644 --- a/system/diagnostic_graph_aggregator/src/node/aggregator.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/node/aggregator.cpp @@ -18,7 +18,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { AggregatorNode::AggregatorNode(const rclcpp::NodeOptions & options) : Node("aggregator", options) @@ -97,7 +97,7 @@ void AggregatorNode::on_diag(const DiagnosticArray & msg) // pub_status_->publish(); } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #include -RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_aggregator::AggregatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::diagnostic_graph_aggregator::AggregatorNode) diff --git a/system/diagnostic_graph_aggregator/src/node/aggregator.hpp b/system/autoware_diagnostic_graph_aggregator/src/node/aggregator.hpp similarity index 93% rename from system/diagnostic_graph_aggregator/src/node/aggregator.hpp rename to system/autoware_diagnostic_graph_aggregator/src/node/aggregator.hpp index f71780f19a5c7..53e2ba3562aaa 100644 --- a/system/diagnostic_graph_aggregator/src/node/aggregator.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/node/aggregator.hpp @@ -24,7 +24,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { class AggregatorNode : public rclcpp::Node @@ -48,6 +48,6 @@ class AggregatorNode : public rclcpp::Node std::unordered_map unknown_diags_; }; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // NODE__AGGREGATOR_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/node/availability.cpp b/system/autoware_diagnostic_graph_aggregator/src/node/availability.cpp similarity index 96% rename from system/diagnostic_graph_aggregator/src/node/availability.cpp rename to system/autoware_diagnostic_graph_aggregator/src/node/availability.cpp index 60ad418799716..df232143aa005 100644 --- a/system/diagnostic_graph_aggregator/src/node/availability.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/node/availability.cpp @@ -22,7 +22,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { ModesAvailability::ModesAvailability(rclcpp::Node & node, const Graph & graph) @@ -73,4 +73,4 @@ void ModesAvailability::update(const rclcpp::Time & stamp) const pub_->publish(message); } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator diff --git a/system/diagnostic_graph_aggregator/src/node/availability.hpp b/system/autoware_diagnostic_graph_aggregator/src/node/availability.hpp similarity index 93% rename from system/diagnostic_graph_aggregator/src/node/availability.hpp rename to system/autoware_diagnostic_graph_aggregator/src/node/availability.hpp index c91db089266f5..f93b19fd55046 100644 --- a/system/diagnostic_graph_aggregator/src/node/availability.hpp +++ b/system/autoware_diagnostic_graph_aggregator/src/node/availability.hpp @@ -21,7 +21,7 @@ #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { class ModesAvailability @@ -44,6 +44,6 @@ class ModesAvailability BaseUnit * pull_over_mrm_; }; -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator #endif // NODE__AVAILABILITY_HPP_ diff --git a/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp b/system/autoware_diagnostic_graph_aggregator/src/tool/plantuml.cpp similarity index 88% rename from system/diagnostic_graph_aggregator/src/tool/plantuml.cpp rename to system/autoware_diagnostic_graph_aggregator/src/tool/plantuml.cpp index 68c0082908430..a87df3c47a19f 100644 --- a/system/diagnostic_graph_aggregator/src/tool/plantuml.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/tool/plantuml.cpp @@ -18,7 +18,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { void dump_root(const std::string & path) @@ -40,7 +40,7 @@ void dump_root(const std::string & path) } } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator int main(int argc, char ** argv) { @@ -48,5 +48,5 @@ int main(int argc, char ** argv) std::cerr << "usage: plantuml " << std::endl; return 1; } - diagnostic_graph_aggregator::dump_root(argv[1]); + autoware::diagnostic_graph_aggregator::dump_root(argv[1]); } diff --git a/system/diagnostic_graph_aggregator/src/tool/tree.cpp b/system/autoware_diagnostic_graph_aggregator/src/tool/tree.cpp similarity index 92% rename from system/diagnostic_graph_aggregator/src/tool/tree.cpp rename to system/autoware_diagnostic_graph_aggregator/src/tool/tree.cpp index f367ec2113808..f37f0c3e575a7 100644 --- a/system/diagnostic_graph_aggregator/src/tool/tree.cpp +++ b/system/autoware_diagnostic_graph_aggregator/src/tool/tree.cpp @@ -18,7 +18,7 @@ #include #include -namespace diagnostic_graph_aggregator +namespace autoware::diagnostic_graph_aggregator { void dump_unit(const BaseUnit * unit, const std::string & indent = "", bool root = true) @@ -60,7 +60,7 @@ void dump_root(const std::string & path) } } -} // namespace diagnostic_graph_aggregator +} // namespace autoware::diagnostic_graph_aggregator int main(int argc, char ** argv) { @@ -68,5 +68,5 @@ int main(int argc, char ** argv) std::cerr << "usage: tree " << std::endl; return 1; } - diagnostic_graph_aggregator::dump_root(argv[1]); + autoware::diagnostic_graph_aggregator::dump_root(argv[1]); } diff --git a/system/diagnostic_graph_aggregator/test/files/test1/field-not-found.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/field-not-found.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/field-not-found.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/field-not-found.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test1/file-not-found.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/file-not-found.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/file-not-found.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/file-not-found.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test1/graph-circulation.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/graph-circulation.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/graph-circulation.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/graph-circulation.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test1/invalid-dict-type.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/invalid-dict-type.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/invalid-dict-type.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/invalid-dict-type.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test1/invalid-list-type.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/invalid-list-type.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/invalid-list-type.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/invalid-list-type.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test1/path-conflict.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/path-conflict.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/path-conflict.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/path-conflict.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test1/path-not-found.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/path-not-found.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/path-not-found.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/path-not-found.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test1/unknown-substitution.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/unknown-substitution.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/unknown-substitution.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/unknown-substitution.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test1/unknown-unit-type.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test1/unknown-unit-type.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test1/unknown-unit-type.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test1/unknown-unit-type.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test2/and.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test2/and.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test2/and.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test2/and.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test2/or.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test2/or.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test2/or.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test2/or.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test2/warn-to-error.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test2/warn-to-error.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test2/warn-to-error.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test2/warn-to-error.yaml diff --git a/system/diagnostic_graph_aggregator/test/files/test2/warn-to-ok.yaml b/system/autoware_diagnostic_graph_aggregator/test/files/test2/warn-to-ok.yaml similarity index 100% rename from system/diagnostic_graph_aggregator/test/files/test2/warn-to-ok.yaml rename to system/autoware_diagnostic_graph_aggregator/test/files/test2/warn-to-ok.yaml diff --git a/system/diagnostic_graph_aggregator/test/src/test1.cpp b/system/autoware_diagnostic_graph_aggregator/test/src/test1.cpp similarity index 96% rename from system/diagnostic_graph_aggregator/test/src/test1.cpp rename to system/autoware_diagnostic_graph_aggregator/test/src/test1.cpp index ea157852675ec..30182314df48f 100644 --- a/system/diagnostic_graph_aggregator/test/src/test1.cpp +++ b/system/autoware_diagnostic_graph_aggregator/test/src/test1.cpp @@ -18,7 +18,7 @@ #include -using namespace diagnostic_graph_aggregator; // NOLINT(build/namespaces) +using namespace autoware::diagnostic_graph_aggregator; // NOLINT(build/namespaces) TEST(ConfigFile, RootNotFound) { diff --git a/system/diagnostic_graph_aggregator/test/src/test2.cpp b/system/autoware_diagnostic_graph_aggregator/test/src/test2.cpp similarity index 97% rename from system/diagnostic_graph_aggregator/test/src/test2.cpp rename to system/autoware_diagnostic_graph_aggregator/test/src/test2.cpp index 4a0199a89f37e..d0f9a8c2e2557 100644 --- a/system/diagnostic_graph_aggregator/test/src/test2.cpp +++ b/system/autoware_diagnostic_graph_aggregator/test/src/test2.cpp @@ -25,11 +25,10 @@ #include #include -using namespace diagnostic_graph_aggregator; // NOLINT(build/namespaces) +using namespace autoware::diagnostic_graph_aggregator; // NOLINT(build/namespaces) using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; -using tier4_system_msgs::msg::DiagnosticGraph; constexpr auto OK = DiagnosticStatus::OK; constexpr auto WARN = DiagnosticStatus::WARN; diff --git a/system/diagnostic_graph_aggregator/test/src/utils.cpp b/system/autoware_diagnostic_graph_aggregator/test/src/utils.cpp similarity index 100% rename from system/diagnostic_graph_aggregator/test/src/utils.cpp rename to system/autoware_diagnostic_graph_aggregator/test/src/utils.cpp diff --git a/system/diagnostic_graph_aggregator/test/src/utils.hpp b/system/autoware_diagnostic_graph_aggregator/test/src/utils.hpp similarity index 100% rename from system/diagnostic_graph_aggregator/test/src/utils.hpp rename to system/autoware_diagnostic_graph_aggregator/test/src/utils.hpp diff --git a/system/diagnostic_graph_utils/CHANGELOG.rst b/system/autoware_diagnostic_graph_utils/CHANGELOG.rst similarity index 92% rename from system/diagnostic_graph_utils/CHANGELOG.rst rename to system/autoware_diagnostic_graph_utils/CHANGELOG.rst index 857671f77fd94..e4c93b3283b6c 100644 --- a/system/diagnostic_graph_utils/CHANGELOG.rst +++ b/system/autoware_diagnostic_graph_utils/CHANGELOG.rst @@ -1,7 +1,13 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package diagnostic_graph_utils +Changelog for package autoware_diagnostic_graph_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `diagnostic_graph_utils` (`#9968 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/diagnostic_graph_utils/CMakeLists.txt b/system/autoware_diagnostic_graph_utils/CMakeLists.txt similarity index 74% rename from system/diagnostic_graph_utils/CMakeLists.txt rename to system/autoware_diagnostic_graph_utils/CMakeLists.txt index 0c36964f49237..b1de417b1bc7d 100644 --- a/system/diagnostic_graph_utils/CMakeLists.txt +++ b/system/autoware_diagnostic_graph_utils/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(diagnostic_graph_utils) +project(autoware_diagnostic_graph_utils) find_package(autoware_cmake REQUIRED) autoware_package() @@ -16,17 +16,17 @@ ament_auto_add_library(${PROJECT_NAME}_tools SHARED ) rclcpp_components_register_node(${PROJECT_NAME}_tools - PLUGIN "diagnostic_graph_utils::DumpNode" + PLUGIN "autoware::diagnostic_graph_utils::DumpNode" EXECUTABLE dump_node ) rclcpp_components_register_node(${PROJECT_NAME}_tools - PLUGIN "diagnostic_graph_utils::ConverterNode" + PLUGIN "autoware::diagnostic_graph_utils::ConverterNode" EXECUTABLE converter_node ) rclcpp_components_register_node(${PROJECT_NAME}_tools - PLUGIN "diagnostic_graph_utils::LoggingNode" + PLUGIN "autoware::diagnostic_graph_utils::LoggingNode" EXECUTABLE logging_node ) diff --git a/system/diagnostic_graph_utils/README.md b/system/autoware_diagnostic_graph_utils/README.md similarity index 55% rename from system/diagnostic_graph_utils/README.md rename to system/autoware_diagnostic_graph_utils/README.md index a06d664622bff..ac941a3575445 100644 --- a/system/diagnostic_graph_utils/README.md +++ b/system/autoware_diagnostic_graph_utils/README.md @@ -1,4 +1,4 @@ -# diagnostic_graph_utils +# autoware_diagnostic_graph_utils This package is a utility for diagnostic graph published by [diagnostic_graph_aggregator](../diagnostic_graph_aggregator/README.md). @@ -9,5 +9,5 @@ This package is a utility for diagnostic graph published by [diagnostic_graph_ag ## C++ library -- [DiagGraph](./include/diagnostic_graph_utils/graph.hpp) -- [DiagGraphSubscription](./include/diagnostic_graph_utils/subscription.hpp) +- [DiagGraph](./include/autoware/diagnostic_graph_utils/graph.hpp) +- [DiagGraphSubscription](./include/autoware/diagnostic_graph_utils/subscription.hpp) diff --git a/system/diagnostic_graph_utils/doc/node/converter.md b/system/autoware_diagnostic_graph_utils/doc/node/converter.md similarity index 80% rename from system/diagnostic_graph_utils/doc/node/converter.md rename to system/autoware_diagnostic_graph_utils/doc/node/converter.md index 407a99c87f73e..a7793b815c0b7 100644 --- a/system/diagnostic_graph_utils/doc/node/converter.md +++ b/system/autoware_diagnostic_graph_utils/doc/node/converter.md @@ -5,7 +5,7 @@ This tool converts `/diagnostics_graph` to `/diagnostics_array` so it can be rea ## Usage ```bash -ros2 run diagnostic_graph_utils converter_node +ros2 run autoware_diagnostic_graph_utils converter_node ``` ## Examples @@ -19,7 +19,7 @@ ros2 launch diagnostic_graph_aggregator example-main.launch.xml Terminal 2: ```bash -ros2 run diagnostic_graph_utils converter_node +ros2 run autoware_diagnostic_graph_utils converter_node ``` Terminal 3: diff --git a/system/diagnostic_graph_utils/doc/node/dump.md b/system/autoware_diagnostic_graph_utils/doc/node/dump.md similarity index 93% rename from system/diagnostic_graph_utils/doc/node/dump.md rename to system/autoware_diagnostic_graph_utils/doc/node/dump.md index c76bb85ed75cb..fac23d655ed64 100644 --- a/system/diagnostic_graph_utils/doc/node/dump.md +++ b/system/autoware_diagnostic_graph_utils/doc/node/dump.md @@ -5,7 +5,7 @@ This tool displays `/diagnostics_graph` in table format. ## Usage ```bash -ros2 run diagnostic_graph_utils dump_node +ros2 run autoware_diagnostic_graph_utils dump_node ``` ## Examples @@ -19,7 +19,7 @@ ros2 launch diagnostic_graph_aggregator example-main.launch.xml Terminal 2: ```bash -ros2 run diagnostic_graph_utils dump_node +ros2 run autoware_diagnostic_graph_utils dump_node ``` Output: diff --git a/system/diagnostic_graph_utils/doc/node/images/rqt_runtime_monitor.png b/system/autoware_diagnostic_graph_utils/doc/node/images/rqt_runtime_monitor.png similarity index 100% rename from system/diagnostic_graph_utils/doc/node/images/rqt_runtime_monitor.png rename to system/autoware_diagnostic_graph_utils/doc/node/images/rqt_runtime_monitor.png diff --git a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp b/system/autoware_diagnostic_graph_utils/include/autoware/diagnostic_graph_utils/graph.hpp similarity index 94% rename from system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp rename to system/autoware_diagnostic_graph_utils/include/autoware/diagnostic_graph_utils/graph.hpp index 275e4ffcb1c7e..3114ffe108b5b 100644 --- a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/graph.hpp +++ b/system/autoware_diagnostic_graph_utils/include/autoware/diagnostic_graph_utils/graph.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DIAGNOSTIC_GRAPH_UTILS__GRAPH_HPP_ -#define DIAGNOSTIC_GRAPH_UTILS__GRAPH_HPP_ +#ifndef AUTOWARE__DIAGNOSTIC_GRAPH_UTILS__GRAPH_HPP_ +#define AUTOWARE__DIAGNOSTIC_GRAPH_UTILS__GRAPH_HPP_ #include @@ -27,7 +27,7 @@ #include #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { class DiagLink; @@ -139,6 +139,6 @@ class DiagGraph std::vector> links_; }; -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils -#endif // DIAGNOSTIC_GRAPH_UTILS__GRAPH_HPP_ +#endif // AUTOWARE__DIAGNOSTIC_GRAPH_UTILS__GRAPH_HPP_ diff --git a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/subscription.hpp b/system/autoware_diagnostic_graph_utils/include/autoware/diagnostic_graph_utils/subscription.hpp similarity index 82% rename from system/diagnostic_graph_utils/include/diagnostic_graph_utils/subscription.hpp rename to system/autoware_diagnostic_graph_utils/include/autoware/diagnostic_graph_utils/subscription.hpp index 5aebde7ea3a38..97a110d0305b0 100644 --- a/system/diagnostic_graph_utils/include/diagnostic_graph_utils/subscription.hpp +++ b/system/autoware_diagnostic_graph_utils/include/autoware/diagnostic_graph_utils/subscription.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DIAGNOSTIC_GRAPH_UTILS__SUBSCRIPTION_HPP_ -#define DIAGNOSTIC_GRAPH_UTILS__SUBSCRIPTION_HPP_ +#ifndef AUTOWARE__DIAGNOSTIC_GRAPH_UTILS__SUBSCRIPTION_HPP_ +#define AUTOWARE__DIAGNOSTIC_GRAPH_UTILS__SUBSCRIPTION_HPP_ -#include "diagnostic_graph_utils/graph.hpp" +#include "autoware/diagnostic_graph_utils/graph.hpp" #include #include #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { class DiagGraphSubscription @@ -48,6 +48,6 @@ class DiagGraphSubscription CallbackType update_callback_; }; -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils -#endif // DIAGNOSTIC_GRAPH_UTILS__SUBSCRIPTION_HPP_ +#endif // AUTOWARE__DIAGNOSTIC_GRAPH_UTILS__SUBSCRIPTION_HPP_ diff --git a/system/diagnostic_graph_utils/launch/logging.launch.xml b/system/autoware_diagnostic_graph_utils/launch/logging.launch.xml similarity index 79% rename from system/diagnostic_graph_utils/launch/logging.launch.xml rename to system/autoware_diagnostic_graph_utils/launch/logging.launch.xml index f14a045919599..fd1d998ac9f22 100644 --- a/system/diagnostic_graph_utils/launch/logging.launch.xml +++ b/system/autoware_diagnostic_graph_utils/launch/logging.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/system/diagnostic_graph_utils/package.xml b/system/autoware_diagnostic_graph_utils/package.xml similarity index 72% rename from system/diagnostic_graph_utils/package.xml rename to system/autoware_diagnostic_graph_utils/package.xml index 03cf1fa369774..527fa1a201e05 100644 --- a/system/diagnostic_graph_utils/package.xml +++ b/system/autoware_diagnostic_graph_utils/package.xml @@ -1,19 +1,20 @@ - diagnostic_graph_utils - 0.40.0 - The diagnostic_graph_utils package + autoware_diagnostic_graph_utils + 0.41.0 + The autoware_diagnostic_graph_utils package Takagi, Isamu + Junya Sasaki Apache License 2.0 ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs diagnostic_msgs rclcpp rclcpp_components - tier4_debug_msgs tier4_system_msgs ament_lint_auto diff --git a/system/diagnostic_graph_utils/src/lib/graph.cpp b/system/autoware_diagnostic_graph_utils/src/lib/graph.cpp similarity index 95% rename from system/diagnostic_graph_utils/src/lib/graph.cpp rename to system/autoware_diagnostic_graph_utils/src/lib/graph.cpp index 007b42547bee1..6bee048f23400 100644 --- a/system/diagnostic_graph_utils/src/lib/graph.cpp +++ b/system/autoware_diagnostic_graph_utils/src/lib/graph.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "diagnostic_graph_utils/graph.hpp" +#include "autoware/diagnostic_graph_utils/graph.hpp" #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { DiagUnit::DiagnosticStatus DiagNode::create_diagnostic_status() const @@ -112,4 +112,4 @@ std::vector DiagGraph::links() const return create_ptrs(links_); } -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils diff --git a/system/diagnostic_graph_utils/src/lib/subscription.cpp b/system/autoware_diagnostic_graph_utils/src/lib/subscription.cpp similarity index 92% rename from system/diagnostic_graph_utils/src/lib/subscription.cpp rename to system/autoware_diagnostic_graph_utils/src/lib/subscription.cpp index c10481ef8f16e..b1464fd2a6ccc 100644 --- a/system/diagnostic_graph_utils/src/lib/subscription.cpp +++ b/system/autoware_diagnostic_graph_utils/src/lib/subscription.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "diagnostic_graph_utils/subscription.hpp" +#include "autoware/diagnostic_graph_utils/subscription.hpp" #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { DiagGraphSubscription::DiagGraphSubscription() @@ -66,4 +66,4 @@ void DiagGraphSubscription::on_status(const DiagGraphStatus & msg) } } -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils diff --git a/system/diagnostic_graph_utils/src/node/converter.cpp b/system/autoware_diagnostic_graph_utils/src/node/converter.cpp similarity index 88% rename from system/diagnostic_graph_utils/src/node/converter.cpp rename to system/autoware_diagnostic_graph_utils/src/node/converter.cpp index 159cc6e0c3cab..8ab7f98457cae 100644 --- a/system/diagnostic_graph_utils/src/node/converter.cpp +++ b/system/autoware_diagnostic_graph_utils/src/node/converter.cpp @@ -16,7 +16,7 @@ #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { ConverterNode::ConverterNode(const rclcpp::NodeOptions & options) : Node("converter", options) @@ -38,7 +38,7 @@ void ConverterNode::on_update(DiagGraph::ConstSharedPtr graph) pub_array_->publish(array); } -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils #include -RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::ConverterNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::diagnostic_graph_utils::ConverterNode) diff --git a/system/diagnostic_graph_utils/src/node/converter.hpp b/system/autoware_diagnostic_graph_utils/src/node/converter.hpp similarity index 89% rename from system/diagnostic_graph_utils/src/node/converter.hpp rename to system/autoware_diagnostic_graph_utils/src/node/converter.hpp index 19364a8ff8240..d47d27c9626ca 100644 --- a/system/diagnostic_graph_utils/src/node/converter.hpp +++ b/system/autoware_diagnostic_graph_utils/src/node/converter.hpp @@ -15,14 +15,14 @@ #ifndef NODE__CONVERTER_HPP_ #define NODE__CONVERTER_HPP_ -#include "diagnostic_graph_utils/subscription.hpp" +#include "autoware/diagnostic_graph_utils/subscription.hpp" #include #include #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { class ConverterNode : public rclcpp::Node @@ -38,6 +38,6 @@ class ConverterNode : public rclcpp::Node DiagGraphSubscription sub_graph_; }; -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils #endif // NODE__CONVERTER_HPP_ diff --git a/system/diagnostic_graph_utils/src/node/dump.cpp b/system/autoware_diagnostic_graph_utils/src/node/dump.cpp similarity index 96% rename from system/diagnostic_graph_utils/src/node/dump.cpp rename to system/autoware_diagnostic_graph_utils/src/node/dump.cpp index 42c66224b2c37..3999c2698a087 100644 --- a/system/diagnostic_graph_utils/src/node/dump.cpp +++ b/system/autoware_diagnostic_graph_utils/src/node/dump.cpp @@ -22,7 +22,7 @@ #include #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { DumpNode::DumpNode(const rclcpp::NodeOptions & options) : Node("dump", options) @@ -131,7 +131,7 @@ void DumpNode::on_update(DiagGraph::ConstSharedPtr graph) } } -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils #include -RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::DumpNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::diagnostic_graph_utils::DumpNode) diff --git a/system/diagnostic_graph_utils/src/node/dump.hpp b/system/autoware_diagnostic_graph_utils/src/node/dump.hpp similarity index 88% rename from system/diagnostic_graph_utils/src/node/dump.hpp rename to system/autoware_diagnostic_graph_utils/src/node/dump.hpp index c990fb77a53db..9412692fa530c 100644 --- a/system/diagnostic_graph_utils/src/node/dump.hpp +++ b/system/autoware_diagnostic_graph_utils/src/node/dump.hpp @@ -15,14 +15,14 @@ #ifndef NODE__DUMP_HPP_ #define NODE__DUMP_HPP_ -#include "diagnostic_graph_utils/subscription.hpp" +#include "autoware/diagnostic_graph_utils/subscription.hpp" #include #include #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { class DumpNode : public rclcpp::Node @@ -47,6 +47,6 @@ class DumpNode : public rclcpp::Node std::string border_; }; -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils #endif // NODE__DUMP_HPP_ diff --git a/system/diagnostic_graph_utils/src/node/logging.cpp b/system/autoware_diagnostic_graph_utils/src/node/logging.cpp similarity index 89% rename from system/diagnostic_graph_utils/src/node/logging.cpp rename to system/autoware_diagnostic_graph_utils/src/node/logging.cpp index 5d360a00aee82..abe59ac28031d 100644 --- a/system/diagnostic_graph_utils/src/node/logging.cpp +++ b/system/autoware_diagnostic_graph_utils/src/node/logging.cpp @@ -22,7 +22,7 @@ #include #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging", options) @@ -34,7 +34,7 @@ LoggingNode::LoggingNode(const rclcpp::NodeOptions & options) : Node("logging", sub_graph_.register_create_callback(std::bind(&LoggingNode::on_create, this, _1)); sub_graph_.subscribe(*this, 1); - pub_error_graph_text_ = create_publisher( + pub_error_graph_text_ = create_publisher( "~/debug/error_graph_text", rclcpp::QoS(1)); const auto period = rclcpp::Rate(declare_parameter("show_rate")).period(); @@ -68,12 +68,12 @@ void LoggingNode::on_timer() RCLCPP_WARN_STREAM(get_logger(), prefix_message << std::endl << dump_text_.str()); } - tier4_debug_msgs::msg::StringStamped message; + autoware_internal_debug_msgs::msg::StringStamped message; message.stamp = now(); message.data = dump_text_.str(); pub_error_graph_text_->publish(message); } else { - tier4_debug_msgs::msg::StringStamped message; + autoware_internal_debug_msgs::msg::StringStamped message; message.stamp = now(); pub_error_graph_text_->publish(message); } @@ -107,7 +107,7 @@ void LoggingNode::dump_unit(DiagUnit * unit, int depth, const std::string & inde } } -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils #include -RCLCPP_COMPONENTS_REGISTER_NODE(diagnostic_graph_utils::LoggingNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::diagnostic_graph_utils::LoggingNode) diff --git a/system/diagnostic_graph_utils/src/node/logging.hpp b/system/autoware_diagnostic_graph_utils/src/node/logging.hpp similarity index 79% rename from system/diagnostic_graph_utils/src/node/logging.hpp rename to system/autoware_diagnostic_graph_utils/src/node/logging.hpp index 17d33499a1513..cbf28c3908105 100644 --- a/system/diagnostic_graph_utils/src/node/logging.hpp +++ b/system/autoware_diagnostic_graph_utils/src/node/logging.hpp @@ -15,16 +15,16 @@ #ifndef NODE__LOGGING_HPP_ #define NODE__LOGGING_HPP_ -#include "diagnostic_graph_utils/subscription.hpp" +#include "autoware/diagnostic_graph_utils/subscription.hpp" #include -#include "tier4_debug_msgs/msg/string_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/string_stamped.hpp" #include #include -namespace diagnostic_graph_utils +namespace autoware::diagnostic_graph_utils { class LoggingNode : public rclcpp::Node @@ -37,7 +37,8 @@ class LoggingNode : public rclcpp::Node void on_timer(); void dump_unit(DiagUnit * unit, int depth, const std::string & indent); DiagGraphSubscription sub_graph_; - rclcpp::Publisher::SharedPtr pub_error_graph_text_; + rclcpp::Publisher::SharedPtr + pub_error_graph_text_; rclcpp::TimerBase::SharedPtr timer_; DiagUnit * root_unit_; @@ -47,6 +48,6 @@ class LoggingNode : public rclcpp::Node bool enable_terminal_log_; }; -} // namespace diagnostic_graph_utils +} // namespace autoware::diagnostic_graph_utils #endif // NODE__LOGGING_HPP_ diff --git a/system/dummy_diag_publisher/CHANGELOG.rst b/system/autoware_dummy_diag_publisher/CHANGELOG.rst similarity index 97% rename from system/dummy_diag_publisher/CHANGELOG.rst rename to system/autoware_dummy_diag_publisher/CHANGELOG.rst index 4652837056220..dfbb265f511e1 100644 --- a/system/dummy_diag_publisher/CHANGELOG.rst +++ b/system/autoware_dummy_diag_publisher/CHANGELOG.rst @@ -1,7 +1,18 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package dummy_diag_publisher +Changelog for package autoware_dummy_diag_publisher ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_dummy_diag_publisher): add autowre prefix (`#9958 `_) + * fic: add autoare\_ prefix + * fix: add autoare\_ prefix codeowner + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, TetsuKawa + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/dummy_diag_publisher/CMakeLists.txt b/system/autoware_dummy_diag_publisher/CMakeLists.txt similarity index 76% rename from system/dummy_diag_publisher/CMakeLists.txt rename to system/autoware_dummy_diag_publisher/CMakeLists.txt index 794e7d35e1194..73e354b7d4d84 100644 --- a/system/dummy_diag_publisher/CMakeLists.txt +++ b/system/autoware_dummy_diag_publisher/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(dummy_diag_publisher) +project(autoware_dummy_diag_publisher) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "DummyDiagPublisher" + PLUGIN "autoware::dummy_diag_publisher::DummyDiagPublisher" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/system/dummy_diag_publisher/README.md b/system/autoware_dummy_diag_publisher/README.md similarity index 96% rename from system/dummy_diag_publisher/README.md rename to system/autoware_dummy_diag_publisher/README.md index 9704cc4af9bc5..759ae509c29a1 100644 --- a/system/dummy_diag_publisher/README.md +++ b/system/autoware_dummy_diag_publisher/README.md @@ -42,7 +42,7 @@ TBD. ### launch ```sh -ros2 launch dummy_diag_publisher dummy_diag_publisher.launch.xml +ros2 launch autoware_dummy_diag_publisher dummy_diag_publisher.launch.xml ``` ### reconfigure diff --git a/system/dummy_diag_publisher/config/_empty.param.yaml b/system/autoware_dummy_diag_publisher/config/_empty.param.yaml similarity index 100% rename from system/dummy_diag_publisher/config/_empty.param.yaml rename to system/autoware_dummy_diag_publisher/config/_empty.param.yaml diff --git a/system/dummy_diag_publisher/config/dummy_diag_publisher.param.yaml b/system/autoware_dummy_diag_publisher/config/dummy_diag_publisher.param.yaml similarity index 100% rename from system/dummy_diag_publisher/config/dummy_diag_publisher.param.yaml rename to system/autoware_dummy_diag_publisher/config/dummy_diag_publisher.param.yaml diff --git a/system/dummy_diag_publisher/config/extra.param.yaml b/system/autoware_dummy_diag_publisher/config/extra.param.yaml similarity index 100% rename from system/dummy_diag_publisher/config/extra.param.yaml rename to system/autoware_dummy_diag_publisher/config/extra.param.yaml diff --git a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp b/system/autoware_dummy_diag_publisher/include/autoware/dummy_diag_publisher/dummy_diag_publisher_core.hpp similarity index 74% rename from system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp rename to system/autoware_dummy_diag_publisher/include/autoware/dummy_diag_publisher/dummy_diag_publisher_core.hpp index bbca397fb47ec..e94085fcff560 100644 --- a/system/dummy_diag_publisher/include/dummy_diag_publisher/dummy_diag_publisher_core.hpp +++ b/system/autoware_dummy_diag_publisher/include/autoware/dummy_diag_publisher/dummy_diag_publisher_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ -#define DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ +#ifndef AUTOWARE__DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ +#define AUTOWARE__DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ #include @@ -23,6 +23,8 @@ #include #include +namespace autoware::dummy_diag_publisher +{ struct DiagConfig { std::string hardware_id; @@ -60,9 +62,13 @@ class DummyDiagPublisher : public rclcpp::Node DummyDiagConfig config_; RequiredDiags required_diags_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + void loadRequiredDiags(); + rcl_interfaces::msg::SetParametersResult onSetParams( + const std::vector & parameters); - std::optional convertStrToStatus(std::string & status_str); + std::optional convertStrToStatus(const std::string & status_str); std::string convertStatusToStr(const Status & status); diagnostic_msgs::msg::DiagnosticStatus::_level_type convertStatusToLevel(const Status & status); @@ -71,5 +77,6 @@ class DummyDiagPublisher : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr pub_; }; +} // namespace autoware::dummy_diag_publisher -#endif // DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ +#endif // AUTOWARE__DUMMY_DIAG_PUBLISHER__DUMMY_DIAG_PUBLISHER_CORE_HPP_ diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml b/system/autoware_dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml similarity index 81% rename from system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml rename to system/autoware_dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml index ae4436fdc9a6d..0c5ec80815c94 100644 --- a/system/dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml +++ b/system/autoware_dummy_diag_publisher/launch/dummy_diag_publisher.launch.xml @@ -1,6 +1,6 @@ - - + + @@ -26,7 +26,7 @@ - + diff --git a/system/autoware_dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml b/system/autoware_dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml new file mode 100644 index 0000000000000..4527c5fe21c01 --- /dev/null +++ b/system/autoware_dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/system/dummy_diag_publisher/package.xml b/system/autoware_dummy_diag_publisher/package.xml similarity index 86% rename from system/dummy_diag_publisher/package.xml rename to system/autoware_dummy_diag_publisher/package.xml index 56faf31413e77..0b45cc9643df5 100644 --- a/system/dummy_diag_publisher/package.xml +++ b/system/autoware_dummy_diag_publisher/package.xml @@ -1,8 +1,8 @@ - dummy_diag_publisher - 0.40.0 + autoware_dummy_diag_publisher + 0.41.0 The dummy_diag_publisher ROS 2 package Fumihito Ito TetsuKawa @@ -11,8 +11,7 @@ ament_cmake_auto autoware_cmake - autoware_universe_utils - diagnostic_updater + diagnostic_msgs fmt rclcpp rclcpp_components diff --git a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp b/system/autoware_dummy_diag_publisher/src/dummy_diag_publisher_core.cpp similarity index 67% rename from system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp rename to system/autoware_dummy_diag_publisher/src/dummy_diag_publisher_core.cpp index 6bc1378507a37..57473e1562620 100644 --- a/system/dummy_diag_publisher/src/dummy_diag_publisher_core.cpp +++ b/system/autoware_dummy_diag_publisher/src/dummy_diag_publisher_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "dummy_diag_publisher/dummy_diag_publisher_core.hpp" +#include "autoware/dummy_diag_publisher/dummy_diag_publisher_core.hpp" #include #include @@ -22,7 +22,9 @@ #define FMT_HEADER_ONLY #include -namespace +#include + +namespace autoware::dummy_diag_publisher { std::vector split(const std::string & str, const char delim) { @@ -34,10 +36,9 @@ std::vector split(const std::string & str, const char delim) } return elems; } -} // namespace std::optional DummyDiagPublisher::convertStrToStatus( - std::string & status_str) + const std::string & status_str) { static std::unordered_map const table = { {"OK", Status::OK}, {"Warn", Status::WARN}, {"Error", Status::ERROR}, {"Stale", Status::STALE}}; @@ -144,6 +145,61 @@ rclcpp::NodeOptions override_options(rclcpp::NodeOptions options) true); } +rcl_interfaces::msg::SetParametersResult DummyDiagPublisher::onSetParams( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + + for (const auto & parameter : parameters) { + bool param_found = false; + const auto & param_name = parameter.get_name(); + + for (auto & diag : required_diags_) { + if (param_name == diag.name + std::string(".status")) { + param_found = true; + auto new_status = convertStrToStatus(parameter.as_string()); + if (new_status) { + diag.status = *new_status; + RCLCPP_INFO( + this->get_logger(), "Updated %s status to: %s", diag.name.c_str(), + parameter.as_string().c_str()); + } else { + result.successful = false; + result.reason = "Invalid status value for: " + parameter.as_string(); + RCLCPP_WARN( + this->get_logger(), "Invalid status value for %s: %s", diag.name.c_str(), + parameter.as_string().c_str()); + } + } else if (param_name == diag.name + std::string(".is_active")) { + param_found = true; + try { + diag.is_active = parameter.as_bool(); + RCLCPP_INFO( + this->get_logger(), "Updated %s is_active to: %s", diag.name.c_str(), + diag.is_active ? "true" : "false"); + } catch (const rclcpp::ParameterTypeException & e) { + result.successful = false; + result.reason = "Invalid is_active value for: " + parameter.as_string(); + RCLCPP_WARN( + this->get_logger(), "Invalid is_active value for %s: %s", diag.name.c_str(), + parameter.as_string().c_str()); + } + } + } + + if (!param_found) { + result.successful = false; + result.reason = "Parameter not registered: " + parameter.get_name(); + RCLCPP_WARN( + this->get_logger(), "Attempted to set unregistered parameter: %s", + parameter.get_name().c_str()); + } + } + + return result; +} + DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) : Node("dummy_diag_publisher", override_options(options)) @@ -164,7 +220,12 @@ DummyDiagPublisher::DummyDiagPublisher(const rclcpp::NodeOptions & options) // Publisher pub_ = create_publisher("/diagnostics", rclcpp::QoS(1)); + + // Parameter Callback Handle + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&DummyDiagPublisher::onSetParams, this, std::placeholders::_1)); } +} // namespace autoware::dummy_diag_publisher #include -RCLCPP_COMPONENTS_REGISTER_NODE(DummyDiagPublisher) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::dummy_diag_publisher::DummyDiagPublisher) diff --git a/system/dummy_infrastructure/CHANGELOG.rst b/system/autoware_dummy_infrastructure/CHANGELOG.rst similarity index 80% rename from system/dummy_infrastructure/CHANGELOG.rst rename to system/autoware_dummy_infrastructure/CHANGELOG.rst index 50ecdd8d7fa96..f59db749a4bc9 100644 --- a/system/dummy_infrastructure/CHANGELOG.rst +++ b/system/autoware_dummy_infrastructure/CHANGELOG.rst @@ -1,6 +1,31 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package dummy_infrastructure -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_dummy_infrastructure +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `dummy_infrastructure` (`#9969 `_) + * feat(dummy_infrastructure): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(dummy_infrastructure): move a header under `include/autoware` + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(dummy_infrastructure): fix include header path + * To follow the previous commit + * rename: `dummy_infrastructure` => `autoware_dummy_infrastructure` + * bug(autoware_dummy_infrastructure): revert wrongly updated copyrights + * update(autoware_dummy_infrastructure): `README.md` + * update: `CODEOWNERS` + * fix(autoware_dummy_infrastructure): fix package name in CHANGELOG.rst + * docs(autoware_dummy_infrastructure): fix package name in README and package description + --------- + Co-authored-by: Ryohsuke Mitsudome + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/system/autoware_dummy_infrastructure/CMakeLists.txt b/system/autoware_dummy_infrastructure/CMakeLists.txt new file mode 100644 index 0000000000000..dc025c8d643f1 --- /dev/null +++ b/system/autoware_dummy_infrastructure/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_dummy_infrastructure) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_dummy_infrastructure_node_component SHARED + src/dummy_infrastructure_node/dummy_infrastructure_node.cpp +) + +rclcpp_components_register_node(autoware_dummy_infrastructure_node_component + PLUGIN "autoware::dummy_infrastructure::DummyInfrastructureNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/system/dummy_infrastructure/README.md b/system/autoware_dummy_infrastructure/README.md similarity index 94% rename from system/dummy_infrastructure/README.md rename to system/autoware_dummy_infrastructure/README.md index d032bfa1a7e3d..a070489ec0513 100644 --- a/system/dummy_infrastructure/README.md +++ b/system/autoware_dummy_infrastructure/README.md @@ -1,11 +1,11 @@ -# dummy_infrastructure +# autoware_dummy_infrastructure This is a debug node for infrastructure communication. ## Usage ```sh -ros2 launch dummy_infrastructure dummy_infrastructure.launch.xml +ros2 launch autoware_dummy_infrastructure dummy_infrastructure.launch.xml ros2 run rqt_reconfigure rqt_reconfigure ``` diff --git a/system/dummy_infrastructure/config/dummy_infrastructure.param.yaml b/system/autoware_dummy_infrastructure/config/dummy_infrastructure.param.yaml similarity index 100% rename from system/dummy_infrastructure/config/dummy_infrastructure.param.yaml rename to system/autoware_dummy_infrastructure/config/dummy_infrastructure.param.yaml diff --git a/system/dummy_infrastructure/include/dummy_infrastructure/dummy_infrastructure_node.hpp b/system/autoware_dummy_infrastructure/include/autoware/dummy_infrastructure/dummy_infrastructure_node.hpp similarity index 87% rename from system/dummy_infrastructure/include/dummy_infrastructure/dummy_infrastructure_node.hpp rename to system/autoware_dummy_infrastructure/include/autoware/dummy_infrastructure/dummy_infrastructure_node.hpp index 38d41750610b1..10f7f81f4e7ff 100644 --- a/system/dummy_infrastructure/include/dummy_infrastructure/dummy_infrastructure_node.hpp +++ b/system/autoware_dummy_infrastructure/include/autoware/dummy_infrastructure/dummy_infrastructure_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP_ -#define DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP_ +#ifndef AUTOWARE__DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP_ +#define AUTOWARE__DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP_ #include @@ -25,7 +25,7 @@ #include #include -namespace dummy_infrastructure +namespace autoware::dummy_infrastructure { using tier4_v2x_msgs::msg::InfrastructureCommand; using tier4_v2x_msgs::msg::InfrastructureCommandArray; @@ -75,6 +75,6 @@ class DummyInfrastructureNode : public rclcpp::Node NodeParam node_param_{}; }; -} // namespace dummy_infrastructure +} // namespace autoware::dummy_infrastructure -#endif // DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP_ +#endif // AUTOWARE__DUMMY_INFRASTRUCTURE__DUMMY_INFRASTRUCTURE_NODE_HPP_ diff --git a/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml b/system/autoware_dummy_infrastructure/launch/dummy_infrastructure.launch.xml similarity index 69% rename from system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml rename to system/autoware_dummy_infrastructure/launch/dummy_infrastructure.launch.xml index bc45abdebec70..8dea215e4a546 100644 --- a/system/dummy_infrastructure/launch/dummy_infrastructure.launch.xml +++ b/system/autoware_dummy_infrastructure/launch/dummy_infrastructure.launch.xml @@ -7,10 +7,10 @@ - + - + diff --git a/system/dummy_infrastructure/package.xml b/system/autoware_dummy_infrastructure/package.xml similarity index 77% rename from system/dummy_infrastructure/package.xml rename to system/autoware_dummy_infrastructure/package.xml index 49c1b00757094..4bfb322dd7fc3 100644 --- a/system/dummy_infrastructure/package.xml +++ b/system/autoware_dummy_infrastructure/package.xml @@ -1,10 +1,11 @@ - dummy_infrastructure - 0.40.0 - dummy_infrastructure + autoware_dummy_infrastructure + 0.41.0 + autoware_dummy_infrastructure Ryohsuke Mitsudome + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json b/system/autoware_dummy_infrastructure/schema/dummy_infrastructure.schema.json similarity index 100% rename from system/dummy_infrastructure/schema/dummy_infrastructure_node.schema.json rename to system/autoware_dummy_infrastructure/schema/dummy_infrastructure.schema.json diff --git a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp b/system/autoware_dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp similarity index 95% rename from system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp rename to system/autoware_dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp index 485d85c926268..2f1500e7db1e0 100644 --- a/system/dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp +++ b/system/autoware_dummy_infrastructure/src/dummy_infrastructure_node/dummy_infrastructure_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "dummy_infrastructure/dummy_infrastructure_node.hpp" +#include "autoware/dummy_infrastructure/dummy_infrastructure_node.hpp" #include @@ -24,9 +24,8 @@ using namespace std::literals; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::nanoseconds; -using std::placeholders::_1; -namespace dummy_infrastructure +namespace autoware::dummy_infrastructure { namespace { @@ -84,6 +83,7 @@ boost::optional findCommand( DummyInfrastructureNode::DummyInfrastructureNode(const rclcpp::NodeOptions & node_options) : Node("dummy_infrastructure", node_options) { + using std::placeholders::_1; // Parameter Server set_param_res_ = this->add_on_set_parameters_callback(std::bind(&DummyInfrastructureNode::onSetParam, this, _1)); @@ -192,7 +192,7 @@ void DummyInfrastructureNode::onTimer() pub_state_array_->publish(state_array); } -} // namespace dummy_infrastructure +} // namespace autoware::dummy_infrastructure #include -RCLCPP_COMPONENTS_REGISTER_NODE(dummy_infrastructure::DummyInfrastructureNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::dummy_infrastructure::DummyInfrastructureNode) diff --git a/system/duplicated_node_checker/CHANGELOG.rst b/system/autoware_duplicated_node_checker/CHANGELOG.rst similarity index 77% rename from system/duplicated_node_checker/CHANGELOG.rst rename to system/autoware_duplicated_node_checker/CHANGELOG.rst index 605012c898598..8bad33913194f 100644 --- a/system/duplicated_node_checker/CHANGELOG.rst +++ b/system/autoware_duplicated_node_checker/CHANGELOG.rst @@ -1,7 +1,30 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package duplicated_node_checker +Changelog for package autoware_duplicated_node_checker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `duplicated_node_checker` (`#9970 `_) + * feat(duplicated_node_checker): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(duplicated_node_checker): move a header under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(duplicated_node_checker): fix include header path + * To follow the previous commit + * rename: `duplicated_node_checker` => `autoware_duplicated_node_checker` + * style(pre-commit): autofix + * bug(autoware_duplicated_node_checker): revert wrongly updated copyrights + * update(autoware_duplicated_node_checker): `README.md` + * update: `CODEOWNERS` + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/duplicated_node_checker/CMakeLists.txt b/system/autoware_duplicated_node_checker/CMakeLists.txt similarity index 69% rename from system/duplicated_node_checker/CMakeLists.txt rename to system/autoware_duplicated_node_checker/CMakeLists.txt index 6a8089fa85c96..25f7e8bd16d92 100644 --- a/system/duplicated_node_checker/CMakeLists.txt +++ b/system/autoware_duplicated_node_checker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(duplicated_node_checker) +project(autoware_duplicated_node_checker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -10,8 +10,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "duplicated_node_checker::DuplicatedNodeChecker" - EXECUTABLE duplicated_node_checker_node + PLUGIN "autoware::duplicated_node_checker::DuplicatedNodeChecker" + EXECUTABLE ${PROJECT_NAME}_node ) ament_auto_package(INSTALL_TO_SHARE diff --git a/system/duplicated_node_checker/README.md b/system/autoware_duplicated_node_checker/README.md similarity index 83% rename from system/duplicated_node_checker/README.md rename to system/autoware_duplicated_node_checker/README.md index 81b53e36c9f6d..42cbf8f7633ac 100644 --- a/system/duplicated_node_checker/README.md +++ b/system/autoware_duplicated_node_checker/README.md @@ -8,7 +8,7 @@ The result is published as diagnostics. ### Standalone Startup ```bash -ros2 launch duplicated_node_checker duplicated_node_checker.launch.xml +ros2 launch autoware_duplicated_node_checker duplicated_node_checker.launch.xml ``` ## Inner-workings / Algorithms @@ -30,7 +30,7 @@ The types of topic status and corresponding diagnostic status are following. ## Parameters -{{ json_to_markdown("system/duplicated_node_checker/schema/duplicated_node_checker.schema.json") }} +{{ json_to_markdown("system/autoware_duplicated_node_checker/schema/duplicated_node_checker.schema.json") }} ## Assumptions / Known limits diff --git a/system/duplicated_node_checker/config/duplicated_node_checker.param.yaml b/system/autoware_duplicated_node_checker/config/duplicated_node_checker.param.yaml similarity index 100% rename from system/duplicated_node_checker/config/duplicated_node_checker.param.yaml rename to system/autoware_duplicated_node_checker/config/duplicated_node_checker.param.yaml diff --git a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp b/system/autoware_duplicated_node_checker/include/autoware/duplicated_node_checker/duplicated_node_checker_core.hpp similarity index 83% rename from system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp rename to system/autoware_duplicated_node_checker/include/autoware/duplicated_node_checker/duplicated_node_checker_core.hpp index 66e125de666b7..91683ff46c265 100644 --- a/system/duplicated_node_checker/include/duplicated_node_checker/duplicated_node_checker_core.hpp +++ b/system/autoware_duplicated_node_checker/include/autoware/duplicated_node_checker/duplicated_node_checker_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ -#define DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ +#ifndef AUTOWARE__DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ +#define AUTOWARE__DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ #include #include @@ -22,7 +22,7 @@ #include #include -namespace duplicated_node_checker +namespace autoware::duplicated_node_checker { class DuplicatedNodeChecker : public rclcpp::Node { @@ -54,6 +54,6 @@ class DuplicatedNodeChecker : public rclcpp::Node bool add_duplicated_node_names_to_msg_; std::unordered_set nodes_to_ignore_; }; -} // namespace duplicated_node_checker +} // namespace autoware::duplicated_node_checker -#endif // DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ +#endif // AUTOWARE__DUPLICATED_NODE_CHECKER__DUPLICATED_NODE_CHECKER_CORE_HPP_ diff --git a/system/autoware_duplicated_node_checker/launch/duplicated_node_checker.launch.xml b/system/autoware_duplicated_node_checker/launch/duplicated_node_checker.launch.xml new file mode 100644 index 0000000000000..ec7c417f671c1 --- /dev/null +++ b/system/autoware_duplicated_node_checker/launch/duplicated_node_checker.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/system/duplicated_node_checker/package.xml b/system/autoware_duplicated_node_checker/package.xml similarity index 85% rename from system/duplicated_node_checker/package.xml rename to system/autoware_duplicated_node_checker/package.xml index ee2f938cfb442..eab99982cccf6 100644 --- a/system/duplicated_node_checker/package.xml +++ b/system/autoware_duplicated_node_checker/package.xml @@ -1,12 +1,13 @@ - duplicated_node_checker - 0.40.0 + autoware_duplicated_node_checker + 0.41.0 A package to find out nodes with common names Shumpei Wakabayashi yliuhb Mamoru Sobue + Junya Sasaki Apache 2.0 ament_cmake diff --git a/system/duplicated_node_checker/schema/duplicated_node_checker.schema.json b/system/autoware_duplicated_node_checker/schema/duplicated_node_checker.schema.json similarity index 100% rename from system/duplicated_node_checker/schema/duplicated_node_checker.schema.json rename to system/autoware_duplicated_node_checker/schema/duplicated_node_checker.schema.json diff --git a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp b/system/autoware_duplicated_node_checker/src/duplicated_node_checker_core.cpp similarity index 91% rename from system/duplicated_node_checker/src/duplicated_node_checker_core.cpp rename to system/autoware_duplicated_node_checker/src/duplicated_node_checker_core.cpp index 40ced8f886005..d68b893b075c1 100644 --- a/system/duplicated_node_checker/src/duplicated_node_checker_core.cpp +++ b/system/autoware_duplicated_node_checker/src/duplicated_node_checker_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "duplicated_node_checker/duplicated_node_checker_core.hpp" +#include "autoware/duplicated_node_checker/duplicated_node_checker_core.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace duplicated_node_checker +namespace autoware::duplicated_node_checker { DuplicatedNodeChecker::DuplicatedNodeChecker(const rclcpp::NodeOptions & node_options) @@ -77,7 +77,7 @@ void DuplicatedNodeChecker::produceDiagnostics(diagnostic_updater::DiagnosticSta stat.summary(level, msg); } -} // namespace duplicated_node_checker +} // namespace autoware::duplicated_node_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(duplicated_node_checker::DuplicatedNodeChecker) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::duplicated_node_checker::DuplicatedNodeChecker) diff --git a/system/hazard_status_converter/CHANGELOG.rst b/system/autoware_hazard_status_converter/CHANGELOG.rst similarity index 87% rename from system/hazard_status_converter/CHANGELOG.rst rename to system/autoware_hazard_status_converter/CHANGELOG.rst index a0c8e0ae193d1..176d49e24579c 100644 --- a/system/hazard_status_converter/CHANGELOG.rst +++ b/system/autoware_hazard_status_converter/CHANGELOG.rst @@ -1,6 +1,13 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package hazard_status_converter -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_hazard_status_converter +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `diagnostic_graph_utils` (`#9968 `_) +* feat: apply `autoware\_` prefix for `hazard_status_converter` (`#9971 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/system/hazard_status_converter/CMakeLists.txt b/system/autoware_hazard_status_converter/CMakeLists.txt similarity index 66% rename from system/hazard_status_converter/CMakeLists.txt rename to system/autoware_hazard_status_converter/CMakeLists.txt index b253f9fc51f2b..b49a67f24a693 100644 --- a/system/hazard_status_converter/CMakeLists.txt +++ b/system/autoware_hazard_status_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(hazard_status_converter) +project(autoware_hazard_status_converter) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,8 +9,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "hazard_status_converter::Converter" - EXECUTABLE converter + PLUGIN "autoware::hazard_status_converter::Converter" + EXECUTABLE ${PROJECT_NAME}_node ) ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml b/system/autoware_hazard_status_converter/launch/hazard_status_converter.launch.xml similarity index 66% rename from system/hazard_status_converter/launch/hazard_status_converter.launch.xml rename to system/autoware_hazard_status_converter/launch/hazard_status_converter.launch.xml index 84318a921d4ce..26164c5ad50b0 100644 --- a/system/hazard_status_converter/launch/hazard_status_converter.launch.xml +++ b/system/autoware_hazard_status_converter/launch/hazard_status_converter.launch.xml @@ -1,5 +1,5 @@ - + diff --git a/system/hazard_status_converter/package.xml b/system/autoware_hazard_status_converter/package.xml similarity index 75% rename from system/hazard_status_converter/package.xml rename to system/autoware_hazard_status_converter/package.xml index 80cc53c08e16d..ba501e70003d5 100644 --- a/system/hazard_status_converter/package.xml +++ b/system/autoware_hazard_status_converter/package.xml @@ -1,19 +1,20 @@ - hazard_status_converter - 0.40.0 - The hazard_status_converter package + autoware_hazard_status_converter + 0.41.0 + The autoware_hazard_status_converter package Takagi, Isamu + Junya Sasaki Apache License 2.0 ament_cmake_auto autoware_cmake autoware_adapi_v1_msgs + autoware_diagnostic_graph_utils autoware_system_msgs autoware_universe_utils - diagnostic_graph_utils diagnostic_msgs rclcpp rclcpp_components diff --git a/system/hazard_status_converter/src/converter.cpp b/system/autoware_hazard_status_converter/src/converter.cpp similarity index 96% rename from system/hazard_status_converter/src/converter.cpp rename to system/autoware_hazard_status_converter/src/converter.cpp index 52cfef93aa522..9faf4e186e06c 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/autoware_hazard_status_converter/src/converter.cpp @@ -18,7 +18,7 @@ #include #include -namespace hazard_status_converter +namespace autoware::hazard_status_converter { Converter::Converter(const rclcpp::NodeOptions & options) : Node("converter", options) @@ -125,7 +125,7 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph) pub_hazard_->publish(hazard); } -} // namespace hazard_status_converter +} // namespace autoware::hazard_status_converter #include -RCLCPP_COMPONENTS_REGISTER_NODE(hazard_status_converter::Converter) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::hazard_status_converter::Converter) diff --git a/system/hazard_status_converter/src/converter.hpp b/system/autoware_hazard_status_converter/src/converter.hpp similarity index 81% rename from system/hazard_status_converter/src/converter.hpp rename to system/autoware_hazard_status_converter/src/converter.hpp index 8011b911f3d42..01b28a54c4413 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/autoware_hazard_status_converter/src/converter.hpp @@ -15,8 +15,8 @@ #ifndef CONVERTER_HPP_ #define CONVERTER_HPP_ +#include #include -#include #include #include @@ -24,7 +24,7 @@ #include -namespace hazard_status_converter +namespace autoware::hazard_status_converter { class Converter : public rclcpp::Node @@ -34,11 +34,11 @@ class Converter : public rclcpp::Node private: using HazardStatusStamped = autoware_system_msgs::msg::HazardStatusStamped; - using DiagGraph = diagnostic_graph_utils::DiagGraph; - using DiagUnit = diagnostic_graph_utils::DiagUnit; + using DiagGraph = autoware::diagnostic_graph_utils::DiagGraph; + using DiagUnit = autoware::diagnostic_graph_utils::DiagUnit; void on_create(DiagGraph::ConstSharedPtr graph); void on_update(DiagGraph::ConstSharedPtr graph); - diagnostic_graph_utils::DiagGraphSubscription sub_graph_; + autoware::diagnostic_graph_utils::DiagGraphSubscription sub_graph_; rclcpp::Publisher::SharedPtr pub_hazard_; autoware::universe_utils::InterProcessPollingSubscriber< tier4_system_msgs::msg::EmergencyHoldingState> @@ -48,6 +48,6 @@ class Converter : public rclcpp::Node std::unordered_set auto_mode_tree_; }; -} // namespace hazard_status_converter +} // namespace autoware::hazard_status_converter #endif // CONVERTER_HPP_ diff --git a/system/mrm_comfortable_stop_operator/CHANGELOG.rst b/system/autoware_mrm_comfortable_stop_operator/CHANGELOG.rst similarity index 88% rename from system/mrm_comfortable_stop_operator/CHANGELOG.rst rename to system/autoware_mrm_comfortable_stop_operator/CHANGELOG.rst index cf423a3d7b158..d4d3af9f3cf56 100644 --- a/system/mrm_comfortable_stop_operator/CHANGELOG.rst +++ b/system/autoware_mrm_comfortable_stop_operator/CHANGELOG.rst @@ -1,7 +1,26 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mrm_comfortable_stop_operator +Changelog for package autoware_mrm_comfortable_stop_operator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `mrm_comfortable_stop_operator` (`#10011 `_) + * feat(mrm_comfortable_stop_operator): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(mrm_comfortable_stop_operator): move a header under `include/autoware` + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(mrm_comfortable_stop_operator): fix include header path + * To follow the previous commit + * rename: `mrm_comfortable_stop_operator` => `autoware_mrm_comfortable_stop_operator` + * update: `CODEOWNERS` + --------- +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/autoware_mrm_comfortable_stop_operator/CMakeLists.txt b/system/autoware_mrm_comfortable_stop_operator/CMakeLists.txt new file mode 100644 index 0000000000000..3ee028cd31608 --- /dev/null +++ b/system/autoware_mrm_comfortable_stop_operator/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_mrm_comfortable_stop_operator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_component SHARED + src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_component + PLUGIN "autoware::mrm_comfortable_stop_operator::MrmComfortableStopOperator" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/mrm_comfortable_stop_operator/README.md b/system/autoware_mrm_comfortable_stop_operator/README.md similarity index 100% rename from system/mrm_comfortable_stop_operator/README.md rename to system/autoware_mrm_comfortable_stop_operator/README.md diff --git a/system/mrm_comfortable_stop_operator/config/mrm_comfortable_stop_operator.param.yaml b/system/autoware_mrm_comfortable_stop_operator/config/mrm_comfortable_stop_operator.param.yaml similarity index 100% rename from system/mrm_comfortable_stop_operator/config/mrm_comfortable_stop_operator.param.yaml rename to system/autoware_mrm_comfortable_stop_operator/config/mrm_comfortable_stop_operator.param.yaml diff --git a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp b/system/autoware_mrm_comfortable_stop_operator/include/autoware/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp similarity index 86% rename from system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp rename to system/autoware_mrm_comfortable_stop_operator/include/autoware/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp index eb84191f481f9..0dbaef1f1df21 100644 --- a/system/mrm_comfortable_stop_operator/include/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp +++ b/system/autoware_mrm_comfortable_stop_operator/include/autoware/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ -#define MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ +#ifndef AUTOWARE__MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ +#define AUTOWARE__MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ // Core #include @@ -29,7 +29,7 @@ // ROS 2 core #include -namespace mrm_comfortable_stop_operator +namespace autoware::mrm_comfortable_stop_operator { struct Parameters @@ -81,6 +81,6 @@ class MrmComfortableStopOperator : public rclcpp::Node OnSetParametersCallbackHandle::SharedPtr set_param_res_; }; -} // namespace mrm_comfortable_stop_operator +} // namespace autoware::mrm_comfortable_stop_operator -#endif // MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ +#endif // AUTOWARE__MRM_COMFORTABLE_STOP_OPERATOR__MRM_COMFORTABLE_STOP_OPERATOR_CORE_HPP_ diff --git a/system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py b/system/autoware_mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py similarity index 91% rename from system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py rename to system/autoware_mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py index 9181af398e3d6..50dabf74206af 100644 --- a/system/mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py +++ b/system/autoware_mrm_comfortable_stop_operator/launch/mrm_comfortable_stop_operator.launch.py @@ -28,8 +28,8 @@ def launch_setup(context, *args, **kwargs): params = yaml.safe_load(f)["/**"]["ros__parameters"] component = ComposableNode( - package="mrm_comfortable_stop_operator", - plugin="mrm_comfortable_stop_operator::MrmComfortableStopOperator", + package="autoware_mrm_comfortable_stop_operator", + plugin="autoware::mrm_comfortable_stop_operator::MrmComfortableStopOperator", name="mrm_comfortable_stop_operator", parameters=[ params, @@ -61,7 +61,7 @@ def generate_launch_description(): DeclareLaunchArgument( "config_file", default_value=[ - FindPackageShare("mrm_comfortable_stop_operator"), + FindPackageShare("autoware_mrm_comfortable_stop_operator"), "/config/mrm_comfortable_stop_operator.param.yaml", ], description="path to the parameter file of mrm_comfortable_stop_operator", diff --git a/system/mrm_comfortable_stop_operator/package.xml b/system/autoware_mrm_comfortable_stop_operator/package.xml similarity index 86% rename from system/mrm_comfortable_stop_operator/package.xml rename to system/autoware_mrm_comfortable_stop_operator/package.xml index 58ff309d825f1..33746a7d2dda0 100644 --- a/system/mrm_comfortable_stop_operator/package.xml +++ b/system/autoware_mrm_comfortable_stop_operator/package.xml @@ -1,11 +1,12 @@ - mrm_comfortable_stop_operator - 0.40.0 + autoware_mrm_comfortable_stop_operator + 0.41.0 The MRM comfortable stop operator package Makoto Kurihara Tomohito Ando + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp b/system/autoware_mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp similarity index 94% rename from system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp rename to system/autoware_mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp index fc703ab16e259..397367e568a25 100644 --- a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp +++ b/system/autoware_mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp" +#include "autoware/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp" #include #include -namespace mrm_comfortable_stop_operator +namespace autoware::mrm_comfortable_stop_operator { MrmComfortableStopOperator::MrmComfortableStopOperator(const rclcpp::NodeOptions & node_options) @@ -123,7 +123,7 @@ void MrmComfortableStopOperator::onTimer() const publishStatus(); } -} // namespace mrm_comfortable_stop_operator +} // namespace autoware::mrm_comfortable_stop_operator #include -RCLCPP_COMPONENTS_REGISTER_NODE(mrm_comfortable_stop_operator::MrmComfortableStopOperator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mrm_comfortable_stop_operator::MrmComfortableStopOperator) diff --git a/system/mrm_emergency_stop_operator/CHANGELOG.rst b/system/autoware_mrm_emergency_stop_operator/CHANGELOG.rst similarity index 86% rename from system/mrm_emergency_stop_operator/CHANGELOG.rst rename to system/autoware_mrm_emergency_stop_operator/CHANGELOG.rst index f6dc5499bcb74..125eed07658a4 100644 --- a/system/mrm_emergency_stop_operator/CHANGELOG.rst +++ b/system/autoware_mrm_emergency_stop_operator/CHANGELOG.rst @@ -1,7 +1,28 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mrm_emergency_stop_operator +Changelog for package autoware_mrm_emergency_stop_operator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `mrm_emergency_stop_operator` (`#9973 `_) + * feat(mrm_emergency_stop_operator): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(mrm_emergency_stop_operator): move a header under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(mrm_emergency_stop_operator): fix include header path + * To follow the previous commit + * rename: `mrm_emergency_stop_operator` => `autoware_mrm_emergency_stop_operator` + * bug(autoware_mrm_emergency_stop_operator): revert wrongly updated copyrights + * bug(tier4_system_launch): fix a missing `autoware\_` for `mrm_emergency_stop_operator` + * bug(autoware_mrm_emergency_stop_operator): fix critical bugs that contaminate topic names + --------- +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/autoware_mrm_emergency_stop_operator/CMakeLists.txt b/system/autoware_mrm_emergency_stop_operator/CMakeLists.txt new file mode 100644 index 0000000000000..a4403aaf11ed1 --- /dev/null +++ b/system/autoware_mrm_emergency_stop_operator/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_mrm_emergency_stop_operator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_component SHARED + src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_component + PLUGIN "autoware::mrm_emergency_stop_operator::MrmEmergencyStopOperator" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/mrm_emergency_stop_operator/README.md b/system/autoware_mrm_emergency_stop_operator/README.md similarity index 100% rename from system/mrm_emergency_stop_operator/README.md rename to system/autoware_mrm_emergency_stop_operator/README.md diff --git a/system/mrm_emergency_stop_operator/config/mrm_emergency_stop_operator.param.yaml b/system/autoware_mrm_emergency_stop_operator/config/mrm_emergency_stop_operator.param.yaml similarity index 100% rename from system/mrm_emergency_stop_operator/config/mrm_emergency_stop_operator.param.yaml rename to system/autoware_mrm_emergency_stop_operator/config/mrm_emergency_stop_operator.param.yaml diff --git a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp b/system/autoware_mrm_emergency_stop_operator/include/autoware/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp similarity index 86% rename from system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp rename to system/autoware_mrm_emergency_stop_operator/include/autoware/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp index d7995a51ac8fb..93df3be01cae8 100644 --- a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp +++ b/system/autoware_mrm_emergency_stop_operator/include/autoware/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ -#define MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ +#ifndef AUTOWARE__MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ +#define AUTOWARE__MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ // Core #include @@ -28,7 +28,7 @@ #include #include -namespace mrm_emergency_stop_operator +namespace autoware::mrm_emergency_stop_operator { using autoware_control_msgs::msg::Control; using tier4_system_msgs::msg::MrmBehaviorStatus; @@ -86,6 +86,6 @@ class MrmEmergencyStopOperator : public rclcpp::Node Control calcTargetAcceleration(const Control & prev_control_cmd) const; }; -} // namespace mrm_emergency_stop_operator +} // namespace autoware::mrm_emergency_stop_operator -#endif // MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ +#endif // AUTOWARE__MRM_EMERGENCY_STOP_OPERATOR__MRM_EMERGENCY_STOP_OPERATOR_CORE_HPP_ diff --git a/system/mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py b/system/autoware_mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py similarity index 91% rename from system/mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py rename to system/autoware_mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py index 769c121c18ac9..c00494a0f1b88 100644 --- a/system/mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py +++ b/system/autoware_mrm_emergency_stop_operator/launch/mrm_emergency_stop_operator.launch.py @@ -28,8 +28,8 @@ def launch_setup(context, *args, **kwargs): params = yaml.safe_load(f)["/**"]["ros__parameters"] component = ComposableNode( - package="mrm_emergency_stop_operator", - plugin="mrm_emergency_stop_operator::MrmEmergencyStopOperator", + package="autoware_mrm_emergency_stop_operator", + plugin="autoware::mrm_emergency_stop_operator::MrmEmergencyStopOperator", name="mrm_emergency_stop_operator", parameters=[ params, @@ -61,7 +61,7 @@ def generate_launch_description(): DeclareLaunchArgument( "config_file", default_value=[ - FindPackageShare("mrm_emergency_stop_operator"), + FindPackageShare("autoware_mrm_emergency_stop_operator"), "/config/mrm_emergency_stop_operator.param.yaml", ], description="path to the parameter file of mrm_emergency_stop_operator", diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/autoware_mrm_emergency_stop_operator/package.xml similarity index 86% rename from system/mrm_emergency_stop_operator/package.xml rename to system/autoware_mrm_emergency_stop_operator/package.xml index 7614373672f31..169c3569b5da2 100644 --- a/system/mrm_emergency_stop_operator/package.xml +++ b/system/autoware_mrm_emergency_stop_operator/package.xml @@ -1,11 +1,12 @@ - mrm_emergency_stop_operator - 0.40.0 + autoware_mrm_emergency_stop_operator + 0.41.0 The MRM emergency stop operator package Makoto Kurihara Tomohito Ando + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/autoware_mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp similarity index 94% rename from system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp rename to system/autoware_mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index 9c941888a3545..7db4ddaef9a9c 100644 --- a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/autoware_mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp" +#include "autoware/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp" #include #include -namespace mrm_emergency_stop_operator +namespace autoware::mrm_emergency_stop_operator { MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & node_options) @@ -150,7 +150,7 @@ Control MrmEmergencyStopOperator::calcTargetAcceleration(const Control & prev_co return control_cmd; } -} // namespace mrm_emergency_stop_operator +} // namespace autoware::mrm_emergency_stop_operator #include -RCLCPP_COMPONENTS_REGISTER_NODE(mrm_emergency_stop_operator::MrmEmergencyStopOperator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mrm_emergency_stop_operator::MrmEmergencyStopOperator) diff --git a/system/mrm_handler/CHANGELOG.rst b/system/autoware_mrm_handler/CHANGELOG.rst similarity index 86% rename from system/mrm_handler/CHANGELOG.rst rename to system/autoware_mrm_handler/CHANGELOG.rst index d9e6990373f6f..9d39ce3ed10c2 100644 --- a/system/mrm_handler/CHANGELOG.rst +++ b/system/autoware_mrm_handler/CHANGELOG.rst @@ -1,7 +1,29 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package mrm_handler +Changelog for package autoware_mrm_handler ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `mrm_handler` (`#9974 `_) + * feat(mrm_handler): apply `autoware\_` prefix (see below): + Note: + * In this commit, I did not organize a folder structure. + The folder structure will be organized in the next some commits. + * The changes will follow the Autoware's guideline as below: + - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder + * rename(mrm_handler): move a header under `include/autoware`: + * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit + * fix(mrm_handler): fix include header path + * To follow the previous commit + * rename: `mrm_handler` => `autoware_mrm_handler` + * bug(tier4_system_launch): fix a missing `autoware\_` for `mrm_handler` + * bug(mrm_handler): revert wrongly updated copyrights + * update(mrm_handler): `README.md` + * bug(autoware_mrm_handler): fix a critical bug that contaminates topic name + --------- +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/mrm_handler/CMakeLists.txt b/system/autoware_mrm_handler/CMakeLists.txt similarity index 82% rename from system/mrm_handler/CMakeLists.txt rename to system/autoware_mrm_handler/CMakeLists.txt index 93e03e7f20ead..ba960c05924d5 100644 --- a/system/mrm_handler/CMakeLists.txt +++ b/system/autoware_mrm_handler/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(mrm_handler) +project(autoware_mrm_handler) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "MrmHandler" + PLUGIN "autoware::mrm_handler::MrmHandler" EXECUTABLE ${PROJECT_NAME}_node EXECUTOR MultiThreadedExecutor ) diff --git a/system/mrm_handler/README.md b/system/autoware_mrm_handler/README.md similarity index 97% rename from system/mrm_handler/README.md rename to system/autoware_mrm_handler/README.md index 8ccb95e6ca8d3..ea2a63e51cd4b 100644 --- a/system/mrm_handler/README.md +++ b/system/autoware_mrm_handler/README.md @@ -37,7 +37,7 @@ MRM Handler is a node to select a proper MRM from a system failure state contain ## Parameters -{{ json_to_markdown("system/mrm_handler/schema/mrm_handler.schema.json") }} +{{ json_to_markdown("system/autoware_mrm_handler/schema/mrm_handler.schema.json") }} ## Assumptions / Known limits diff --git a/system/mrm_handler/config/mrm_handler.param.yaml b/system/autoware_mrm_handler/config/mrm_handler.param.yaml similarity index 100% rename from system/mrm_handler/config/mrm_handler.param.yaml rename to system/autoware_mrm_handler/config/mrm_handler.param.yaml diff --git a/system/mrm_handler/image/mrm-state.svg b/system/autoware_mrm_handler/image/mrm-state.svg similarity index 100% rename from system/mrm_handler/image/mrm-state.svg rename to system/autoware_mrm_handler/image/mrm-state.svg diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp similarity index 96% rename from system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp rename to system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp index b292ab1d874d3..8d74db4e5c843 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/autoware_mrm_handler/include/autoware/mrm_handler/mrm_handler_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MRM_HANDLER__MRM_HANDLER_CORE_HPP_ -#define MRM_HANDLER__MRM_HANDLER_CORE_HPP_ +#ifndef AUTOWARE__MRM_HANDLER__MRM_HANDLER_CORE_HPP_ +#define AUTOWARE__MRM_HANDLER__MRM_HANDLER_CORE_HPP_ // Core #include @@ -41,6 +41,9 @@ #include #include +namespace autoware::mrm_handler +{ + struct HazardLampPolicy { bool emergency; @@ -162,4 +165,6 @@ class MrmHandler : public rclcpp::Node bool isArrivedAtGoal(); }; -#endif // MRM_HANDLER__MRM_HANDLER_CORE_HPP_ +} // namespace autoware::mrm_handler + +#endif // AUTOWARE__MRM_HANDLER__MRM_HANDLER_CORE_HPP_ diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/autoware_mrm_handler/launch/mrm_handler.launch.xml similarity index 92% rename from system/mrm_handler/launch/mrm_handler.launch.xml rename to system/autoware_mrm_handler/launch/mrm_handler.launch.xml index 51a22cf92bebc..d3ed4fb499729 100644 --- a/system/mrm_handler/launch/mrm_handler.launch.xml +++ b/system/autoware_mrm_handler/launch/mrm_handler.launch.xml @@ -17,10 +17,10 @@ - + - + diff --git a/system/mrm_handler/package.xml b/system/autoware_mrm_handler/package.xml similarity index 88% rename from system/mrm_handler/package.xml rename to system/autoware_mrm_handler/package.xml index d330680e87049..92cfef1634bc1 100644 --- a/system/mrm_handler/package.xml +++ b/system/autoware_mrm_handler/package.xml @@ -1,12 +1,13 @@ - mrm_handler - 0.40.0 + autoware_mrm_handler + 0.41.0 The mrm_handler ROS 2 package Makoto Kurihara Ryuta Kambe Tetsuhiro Kawaguchi + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/mrm_handler/schema/mrm_handler.schema.json b/system/autoware_mrm_handler/schema/mrm_handler.schema.json similarity index 100% rename from system/mrm_handler/schema/mrm_handler.schema.json rename to system/autoware_mrm_handler/schema/mrm_handler.schema.json diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp similarity index 97% rename from system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp rename to system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 4022bdaadebef..e9429300de573 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/autoware_mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -11,13 +11,16 @@ // CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language // governing permissions and limitations under the License. -#include "mrm_handler/mrm_handler_core.hpp" +#include "autoware/mrm_handler/mrm_handler_core.hpp" #include #include #include #include +namespace autoware::mrm_handler +{ + MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler", options) { // Parameter @@ -115,12 +118,7 @@ void MrmHandler::publishHazardCmd() HazardLightsCommand msg; msg.stamp = this->now(); - if (is_emergency_holding_) { - // turn hazard on during emergency holding - msg.command = HazardLightsCommand::ENABLE; - } else if (isEmergency() && param_.turning_hazard_on.emergency) { - // turn hazard on if vehicle is in emergency state and - // turning hazard on if emergency flag is true + if (param_.turning_hazard_on.emergency && isEmergency()) { msg.command = HazardLightsCommand::ENABLE; } else { msg.command = HazardLightsCommand::NO_COMMAND; @@ -588,5 +586,7 @@ bool MrmHandler::isArrivedAtGoal() return state->mode == OperationModeState::STOP; } +} // namespace autoware::mrm_handler + #include -RCLCPP_COMPONENTS_REGISTER_NODE(MrmHandler) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::mrm_handler::MrmHandler) diff --git a/system/autoware_processing_time_checker/CHANGELOG.rst b/system/autoware_processing_time_checker/CHANGELOG.rst index 39bf68af5f415..71e10ad7c579e 100644 --- a/system/autoware_processing_time_checker/CHANGELOG.rst +++ b/system/autoware_processing_time_checker/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package autoware_processing_time_checker ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_processing_time_checker)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_processing_time_checker (`#9921 `_) + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* fix(autoware_processing_time_checker): fix bugprone-exception-escape (`#9780 `_) + * fix: bugprone-error + * fix: cpplint + --------- +* Contributors: Fumiya Watanabe, Vishal Chauhan, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/autoware_processing_time_checker/README.md b/system/autoware_processing_time_checker/README.md index 1cb6a289bc54f..e7d70f9378f10 100644 --- a/system/autoware_processing_time_checker/README.md +++ b/system/autoware_processing_time_checker/README.md @@ -17,9 +17,9 @@ ros2 launch autoware_processing_time_checker processing_time_checker.launch.xml ### Input -| Name | Type | Description | -| ------------------------- | --------------------------------- | ------------------------------ | -| `/.../processing_time_ms` | `tier4_debug_msgs/Float64Stamped` | processing time of each module | +| Name | Type | Description | +| ------------------------- | --------------------------------------------- | ------------------------------ | +| `/.../processing_time_ms` | `autoware_internal_debug_msgs/Float64Stamped` | processing time of each module | ### Output diff --git a/system/autoware_processing_time_checker/package.xml b/system/autoware_processing_time_checker/package.xml index 4a18857663cc1..ffbdfc85c3cbe 100644 --- a/system/autoware_processing_time_checker/package.xml +++ b/system/autoware_processing_time_checker/package.xml @@ -2,7 +2,7 @@ autoware_processing_time_checker - 0.40.0 + 0.41.0 A package to find out nodes with common names Takayuki Murooka Kosuke Takeuchi @@ -12,11 +12,11 @@ ament_cmake autoware_cmake + autoware_internal_debug_msgs autoware_universe_utils nlohmann-json-dev rclcpp rclcpp_components - tier4_debug_msgs tier4_metric_msgs ament_lint_auto diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.cpp b/system/autoware_processing_time_checker/src/processing_time_checker.cpp index 1e3f04af8fa89..5455cdab422d5 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.cpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -103,45 +104,51 @@ ProcessingTimeChecker::~ProcessingTimeChecker() return; } - // generate json data - nlohmann::json j; - for (const auto & accumulator_iterator : processing_time_accumulator_map_) { - const auto module_name = accumulator_iterator.first; - const auto processing_time_accumulator = accumulator_iterator.second; - j[module_name + "/min"] = processing_time_accumulator.min(); - j[module_name + "/max"] = processing_time_accumulator.max(); - j[module_name + "/mean"] = processing_time_accumulator.mean(); - j[module_name + "/count"] = processing_time_accumulator.count(); - j[module_name + "/description"] = "processing time of " + module_name + "[ms]"; - } + try { + // generate json data + nlohmann::json j; + for (const auto & accumulator_iterator : processing_time_accumulator_map_) { + const auto module_name = accumulator_iterator.first; + const auto processing_time_accumulator = accumulator_iterator.second; + j[module_name + "/min"] = processing_time_accumulator.min(); + j[module_name + "/max"] = processing_time_accumulator.max(); + j[module_name + "/mean"] = processing_time_accumulator.mean(); + j[module_name + "/count"] = processing_time_accumulator.count(); + j[module_name + "/description"] = "processing time of " + module_name + "[ms]"; + } - // get output folder - const std::string output_folder_str = - rclcpp::get_logging_directory().string() + "/autoware_metrics"; - if (!std::filesystem::exists(output_folder_str)) { - if (!std::filesystem::create_directories(output_folder_str)) { - RCLCPP_ERROR( - this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); - return; + // get output folder + const std::string output_folder_str = + rclcpp::get_logging_directory().string() + "/autoware_metrics"; + if (!std::filesystem::exists(output_folder_str)) { + if (!std::filesystem::create_directories(output_folder_str)) { + RCLCPP_ERROR( + this->get_logger(), "Failed to create directories: %s", output_folder_str.c_str()); + return; + } } - } - // get time stamp - std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); - std::tm * local_time = std::localtime(&now_time_t); - std::ostringstream oss; - oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); - std::string cur_time_str = oss.str(); - - // Write metrics .json to file - const std::string output_file_str = - output_folder_str + "/autoware_processing_time_checker-" + cur_time_str + ".json"; - std::ofstream f(output_file_str); - if (f.is_open()) { - f << j.dump(4); - f.close(); - } else { - RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + // get time stamp + std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::tm * local_time = std::localtime(&now_time_t); + std::ostringstream oss; + oss << std::put_time(local_time, "%Y-%m-%d-%H-%M-%S"); + std::string cur_time_str = oss.str(); + + // Write metrics .json to file + const std::string output_file_str = + output_folder_str + "/autoware_processing_time_checker-" + cur_time_str + ".json"; + std::ofstream f(output_file_str); + if (f.is_open()) { + f << j.dump(4); + f.close(); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to open file: %s", output_file_str.c_str()); + } + } catch (const std::exception & e) { + std::cerr << "Exception in ProcessingTimeChecker: " << e.what() << std::endl; + } catch (...) { + std::cerr << "Unknown exception in ProcessingTimeChecker" << std::endl; } } diff --git a/system/autoware_processing_time_checker/src/processing_time_checker.hpp b/system/autoware_processing_time_checker/src/processing_time_checker.hpp index 77450923509f2..f6664b901f890 100644 --- a/system/autoware_processing_time_checker/src/processing_time_checker.hpp +++ b/system/autoware_processing_time_checker/src/processing_time_checker.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include #include @@ -32,7 +32,7 @@ namespace autoware::processing_time_checker using autoware::universe_utils::Accumulator; using MetricMsg = tier4_metric_msgs::msg::Metric; using MetricArrayMsg = tier4_metric_msgs::msg::MetricArray; -using tier4_debug_msgs::msg::Float64Stamped; +using autoware_internal_debug_msgs::msg::Float64Stamped; class ProcessingTimeChecker : public rclcpp::Node { diff --git a/system/system_diagnostic_monitor/CHANGELOG.rst b/system/autoware_system_diagnostic_monitor/CHANGELOG.rst similarity index 89% rename from system/system_diagnostic_monitor/CHANGELOG.rst rename to system/autoware_system_diagnostic_monitor/CHANGELOG.rst index 9a5721b05d7ee..59495adc1b2fa 100644 --- a/system/system_diagnostic_monitor/CHANGELOG.rst +++ b/system/autoware_system_diagnostic_monitor/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package system_diagnostic_monitor -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_system_diagnostic_monitor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware` prefix for `component_state_monitor` and its dependencies (`#9961 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/autoware_system_diagnostic_monitor/CMakeLists.txt similarity index 86% rename from system/system_diagnostic_monitor/CMakeLists.txt rename to system/autoware_system_diagnostic_monitor/CMakeLists.txt index b253b5dbc0529..562079abf84fb 100644 --- a/system/system_diagnostic_monitor/CMakeLists.txt +++ b/system/autoware_system_diagnostic_monitor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(system_diagnostic_monitor) +project(autoware_system_diagnostic_monitor) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/system/system_diagnostic_monitor/README.md b/system/autoware_system_diagnostic_monitor/README.md similarity index 100% rename from system/system_diagnostic_monitor/README.md rename to system/autoware_system_diagnostic_monitor/README.md diff --git a/system/system_diagnostic_monitor/config/autoware-main.yaml b/system/autoware_system_diagnostic_monitor/config/autoware-main.yaml similarity index 78% rename from system/system_diagnostic_monitor/config/autoware-main.yaml rename to system/autoware_system_diagnostic_monitor/config/autoware-main.yaml index 26f3fee2863d0..30b6ce9bad9f4 100644 --- a/system/system_diagnostic_monitor/config/autoware-main.yaml +++ b/system/autoware_system_diagnostic_monitor/config/autoware-main.yaml @@ -1,11 +1,11 @@ files: - - { path: $(find-pkg-share system_diagnostic_monitor)/config/map.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/localization.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/planning.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/perception.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/control.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/vehicle.yaml } - - { path: $(find-pkg-share system_diagnostic_monitor)/config/system.yaml } + - { path: $(dirname)/map.yaml } + - { path: $(dirname)/localization.yaml } + - { path: $(dirname)/planning.yaml } + - { path: $(dirname)/perception.yaml } + - { path: $(dirname)/control.yaml } + - { path: $(dirname)/vehicle.yaml } + - { path: $(dirname)/system.yaml } units: - path: /autoware/modes/stop diff --git a/system/system_diagnostic_monitor/config/autoware-psim.yaml b/system/autoware_system_diagnostic_monitor/config/autoware-psim.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/autoware-psim.yaml rename to system/autoware_system_diagnostic_monitor/config/autoware-psim.yaml diff --git a/system/system_diagnostic_monitor/config/control.yaml b/system/autoware_system_diagnostic_monitor/config/control.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/control.yaml rename to system/autoware_system_diagnostic_monitor/config/control.yaml diff --git a/system/system_diagnostic_monitor/config/hardware.yaml b/system/autoware_system_diagnostic_monitor/config/hardware.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/hardware.yaml rename to system/autoware_system_diagnostic_monitor/config/hardware.yaml diff --git a/system/system_diagnostic_monitor/config/localization.yaml b/system/autoware_system_diagnostic_monitor/config/localization.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/localization.yaml rename to system/autoware_system_diagnostic_monitor/config/localization.yaml diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/autoware_system_diagnostic_monitor/config/map.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/map.yaml rename to system/autoware_system_diagnostic_monitor/config/map.yaml diff --git a/system/system_diagnostic_monitor/config/perception.yaml b/system/autoware_system_diagnostic_monitor/config/perception.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/perception.yaml rename to system/autoware_system_diagnostic_monitor/config/perception.yaml diff --git a/system/system_diagnostic_monitor/config/planning.yaml b/system/autoware_system_diagnostic_monitor/config/planning.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/planning.yaml rename to system/autoware_system_diagnostic_monitor/config/planning.yaml diff --git a/system/system_diagnostic_monitor/config/system.yaml b/system/autoware_system_diagnostic_monitor/config/system.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/system.yaml rename to system/autoware_system_diagnostic_monitor/config/system.yaml diff --git a/system/system_diagnostic_monitor/config/vehicle.yaml b/system/autoware_system_diagnostic_monitor/config/vehicle.yaml similarity index 100% rename from system/system_diagnostic_monitor/config/vehicle.yaml rename to system/autoware_system_diagnostic_monitor/config/vehicle.yaml diff --git a/system/autoware_system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/autoware_system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml new file mode 100644 index 0000000000000..9a9fcb494d4e2 --- /dev/null +++ b/system/autoware_system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/system/system_diagnostic_monitor/package.xml b/system/autoware_system_diagnostic_monitor/package.xml similarity index 71% rename from system/system_diagnostic_monitor/package.xml rename to system/autoware_system_diagnostic_monitor/package.xml index 095d7cf040517..92e869001ddab 100644 --- a/system/system_diagnostic_monitor/package.xml +++ b/system/autoware_system_diagnostic_monitor/package.xml @@ -1,17 +1,18 @@ - system_diagnostic_monitor - 0.40.0 - The system_diagnostic_monitor package + autoware_system_diagnostic_monitor + 0.41.0 + The autoware_system_diagnostic_monitor package Takagi, Isamu + Junya Sasaki Apache License 2.0 ament_cmake_auto autoware_cmake autoware_adapi_v1_msgs - diagnostic_graph_aggregator + autoware_diagnostic_graph_aggregator diagnostic_msgs rclpy tier4_system_msgs diff --git a/system/system_diagnostic_monitor/script/component_state_diagnostics.py b/system/autoware_system_diagnostic_monitor/script/component_state_diagnostics.py similarity index 100% rename from system/system_diagnostic_monitor/script/component_state_diagnostics.py rename to system/autoware_system_diagnostic_monitor/script/component_state_diagnostics.py diff --git a/system/system_monitor/CHANGELOG.rst b/system/autoware_system_monitor/CHANGELOG.rst similarity index 98% rename from system/system_monitor/CHANGELOG.rst rename to system/autoware_system_monitor/CHANGELOG.rst index 0ce1977dfdfa8..226e1c44a292b 100644 --- a/system/system_monitor/CHANGELOG.rst +++ b/system/autoware_system_monitor/CHANGELOG.rst @@ -1,7 +1,27 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package system_monitor +Changelog for package autoware_system_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `system_monitor` (`#10017 `_) + * feat(system_monitor): apply `autoware\_` prefix (see below): + * The `system_monitor` operates independently from other modules in `autoware.universe`, so the `autoware\_` prefix is added only to the package name. + * The `autoware::` namespace is not used because C language does not support namespaces. + * Headers are not moved under `include/autoware` to maintain compatibility for use outside the `autoware` context. + * For users utilizing this package within `autoware.universe`, only the package name includes the `autoware\_` prefix. + This approach explains the unique namespacing and naming conventions for `system_monitor` compared to other packages. + * bug(system_monitor): fix missing package name update + * rename: `system_monitor` => `autoware_system_monitor` + * style(pre-commit): autofix + * update: `CODEOWNERS` + * bug(autoware_system_monitor): apply missing fix + * update: `CODEOWNERS` + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Junya Sasaki + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/system/system_monitor/CMakeLists.txt b/system/autoware_system_monitor/CMakeLists.txt similarity index 99% rename from system/system_monitor/CMakeLists.txt rename to system/autoware_system_monitor/CMakeLists.txt index be3f8fadc8a81..cb6b8197ae03a 100644 --- a/system/system_monitor/CMakeLists.txt +++ b/system/autoware_system_monitor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(system_monitor) +project(autoware_system_monitor) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/system/system_monitor/README.md b/system/autoware_system_monitor/README.md similarity index 97% rename from system/system_monitor/README.md rename to system/autoware_system_monitor/README.md index 6203ad45d4b3a..06b79422f833e 100644 --- a/system/system_monitor/README.md +++ b/system/autoware_system_monitor/README.md @@ -33,7 +33,7 @@ Use colcon build and launch in the same way as other packages. ```sh colcon build source install/setup.bash -ros2 launch system_monitor system_monitor.launch.xml +ros2 launch autoware_system_monitor system_monitor.launch.xml ``` CPU and GPU monitoring method differs depending on platform.
@@ -77,6 +77,7 @@ Every topic is published in 1 minute interval. | | Network Usage | ✓ | ✓ | ✓ | Notification of usage only, normally error not generated. | | | Network CRC Error | ✓ | ✓ | ✓ | Warning occurs when the number of CRC errors in the period reaches the threshold value. The number of CRC errors that occur is the same as the value that can be confirmed with the ip command. | | | IP Packet Reassembles Failed | ✓ | ✓ | ✓ | | +| | UDP Buf Errors | ✓ | ✓ | ✓ | | | NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | | Process Monitor | Tasks Summary | ✓ | ✓ | ✓ | | | | High-load Proc[0-9] | ✓ | ✓ | ✓ | | diff --git a/system/system_monitor/cmake/FindNVML.cmake b/system/autoware_system_monitor/cmake/FindNVML.cmake similarity index 100% rename from system/system_monitor/cmake/FindNVML.cmake rename to system/autoware_system_monitor/cmake/FindNVML.cmake diff --git a/system/system_monitor/config/cpu_monitor.param.yaml b/system/autoware_system_monitor/config/cpu_monitor.param.yaml similarity index 100% rename from system/system_monitor/config/cpu_monitor.param.yaml rename to system/autoware_system_monitor/config/cpu_monitor.param.yaml diff --git a/system/system_monitor/config/gpu_monitor.param.yaml b/system/autoware_system_monitor/config/gpu_monitor.param.yaml similarity index 100% rename from system/system_monitor/config/gpu_monitor.param.yaml rename to system/autoware_system_monitor/config/gpu_monitor.param.yaml diff --git a/system/system_monitor/config/hdd_monitor.param.yaml b/system/autoware_system_monitor/config/hdd_monitor.param.yaml similarity index 100% rename from system/system_monitor/config/hdd_monitor.param.yaml rename to system/autoware_system_monitor/config/hdd_monitor.param.yaml diff --git a/system/system_monitor/config/mem_monitor.param.yaml b/system/autoware_system_monitor/config/mem_monitor.param.yaml similarity index 100% rename from system/system_monitor/config/mem_monitor.param.yaml rename to system/autoware_system_monitor/config/mem_monitor.param.yaml diff --git a/system/system_monitor/config/net_monitor.param.yaml b/system/autoware_system_monitor/config/net_monitor.param.yaml similarity index 78% rename from system/system_monitor/config/net_monitor.param.yaml rename to system/autoware_system_monitor/config/net_monitor.param.yaml index 7a1e5aeff2db1..b366e26395641 100644 --- a/system/system_monitor/config/net_monitor.param.yaml +++ b/system/autoware_system_monitor/config/net_monitor.param.yaml @@ -7,3 +7,5 @@ crc_error_count_threshold: 1 reassembles_failed_check_duration: 1 reassembles_failed_check_count: 1 + udp_buf_errors_check_duration: 1 + udp_buf_errors_check_count: 1 diff --git a/system/system_monitor/config/ntp_monitor.param.yaml b/system/autoware_system_monitor/config/ntp_monitor.param.yaml similarity index 100% rename from system/system_monitor/config/ntp_monitor.param.yaml rename to system/autoware_system_monitor/config/ntp_monitor.param.yaml diff --git a/system/system_monitor/config/process_monitor.param.yaml b/system/autoware_system_monitor/config/process_monitor.param.yaml similarity index 100% rename from system/system_monitor/config/process_monitor.param.yaml rename to system/autoware_system_monitor/config/process_monitor.param.yaml diff --git a/system/system_monitor/config/voltage_monitor.param.yaml b/system/autoware_system_monitor/config/voltage_monitor.param.yaml similarity index 100% rename from system/system_monitor/config/voltage_monitor.param.yaml rename to system/autoware_system_monitor/config/voltage_monitor.param.yaml diff --git a/system/system_monitor/docs/class_diagrams.md b/system/autoware_system_monitor/docs/class_diagrams.md similarity index 100% rename from system/system_monitor/docs/class_diagrams.md rename to system/autoware_system_monitor/docs/class_diagrams.md diff --git a/system/system_monitor/docs/hdd_reader.md b/system/autoware_system_monitor/docs/hdd_reader.md similarity index 100% rename from system/system_monitor/docs/hdd_reader.md rename to system/autoware_system_monitor/docs/hdd_reader.md diff --git a/system/system_monitor/docs/images/class_cpu_monitor.png b/system/autoware_system_monitor/docs/images/class_cpu_monitor.png similarity index 100% rename from system/system_monitor/docs/images/class_cpu_monitor.png rename to system/autoware_system_monitor/docs/images/class_cpu_monitor.png diff --git a/system/system_monitor/docs/images/class_gpu_monitor.png b/system/autoware_system_monitor/docs/images/class_gpu_monitor.png similarity index 100% rename from system/system_monitor/docs/images/class_gpu_monitor.png rename to system/autoware_system_monitor/docs/images/class_gpu_monitor.png diff --git a/system/system_monitor/docs/images/class_hdd_monitor.png b/system/autoware_system_monitor/docs/images/class_hdd_monitor.png similarity index 100% rename from system/system_monitor/docs/images/class_hdd_monitor.png rename to system/autoware_system_monitor/docs/images/class_hdd_monitor.png diff --git a/system/system_monitor/docs/images/class_mem_monitor.png b/system/autoware_system_monitor/docs/images/class_mem_monitor.png similarity index 100% rename from system/system_monitor/docs/images/class_mem_monitor.png rename to system/autoware_system_monitor/docs/images/class_mem_monitor.png diff --git a/system/system_monitor/docs/images/class_net_monitor.png b/system/autoware_system_monitor/docs/images/class_net_monitor.png similarity index 100% rename from system/system_monitor/docs/images/class_net_monitor.png rename to system/autoware_system_monitor/docs/images/class_net_monitor.png diff --git a/system/system_monitor/docs/images/class_ntp_monitor.png b/system/autoware_system_monitor/docs/images/class_ntp_monitor.png similarity index 100% rename from system/system_monitor/docs/images/class_ntp_monitor.png rename to system/autoware_system_monitor/docs/images/class_ntp_monitor.png diff --git a/system/system_monitor/docs/images/class_process_monitor.png b/system/autoware_system_monitor/docs/images/class_process_monitor.png similarity index 100% rename from system/system_monitor/docs/images/class_process_monitor.png rename to system/autoware_system_monitor/docs/images/class_process_monitor.png diff --git a/system/system_monitor/docs/images/seq_cpu_monitor.png b/system/autoware_system_monitor/docs/images/seq_cpu_monitor.png similarity index 100% rename from system/system_monitor/docs/images/seq_cpu_monitor.png rename to system/autoware_system_monitor/docs/images/seq_cpu_monitor.png diff --git a/system/system_monitor/docs/images/seq_gpu_monitor.png b/system/autoware_system_monitor/docs/images/seq_gpu_monitor.png similarity index 100% rename from system/system_monitor/docs/images/seq_gpu_monitor.png rename to system/autoware_system_monitor/docs/images/seq_gpu_monitor.png diff --git a/system/system_monitor/docs/images/seq_hdd_monitor.png b/system/autoware_system_monitor/docs/images/seq_hdd_monitor.png similarity index 100% rename from system/system_monitor/docs/images/seq_hdd_monitor.png rename to system/autoware_system_monitor/docs/images/seq_hdd_monitor.png diff --git a/system/system_monitor/docs/images/seq_mem_monitor.png b/system/autoware_system_monitor/docs/images/seq_mem_monitor.png similarity index 100% rename from system/system_monitor/docs/images/seq_mem_monitor.png rename to system/autoware_system_monitor/docs/images/seq_mem_monitor.png diff --git a/system/system_monitor/docs/images/seq_net_monitor.png b/system/autoware_system_monitor/docs/images/seq_net_monitor.png similarity index 100% rename from system/system_monitor/docs/images/seq_net_monitor.png rename to system/autoware_system_monitor/docs/images/seq_net_monitor.png diff --git a/system/system_monitor/docs/images/seq_ntp_monitor.png b/system/autoware_system_monitor/docs/images/seq_ntp_monitor.png similarity index 100% rename from system/system_monitor/docs/images/seq_ntp_monitor.png rename to system/autoware_system_monitor/docs/images/seq_ntp_monitor.png diff --git a/system/system_monitor/docs/images/seq_process_monitor.png b/system/autoware_system_monitor/docs/images/seq_process_monitor.png similarity index 100% rename from system/system_monitor/docs/images/seq_process_monitor.png rename to system/autoware_system_monitor/docs/images/seq_process_monitor.png diff --git a/system/system_monitor/docs/msr_reader.md b/system/autoware_system_monitor/docs/msr_reader.md similarity index 100% rename from system/system_monitor/docs/msr_reader.md rename to system/autoware_system_monitor/docs/msr_reader.md diff --git a/system/system_monitor/docs/ros_parameters.md b/system/autoware_system_monitor/docs/ros_parameters.md similarity index 95% rename from system/system_monitor/docs/ros_parameters.md rename to system/autoware_system_monitor/docs/ros_parameters.md index 1d081d513c85c..8ec952e60e242 100644 --- a/system/system_monitor/docs/ros_parameters.md +++ b/system/autoware_system_monitor/docs/ros_parameters.md @@ -69,6 +69,8 @@ net_monitor: | crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | | reassembles_failed_check_duration | int | sec | 1 | IP packet reassembles failed check duration. | | reassembles_failed_check_count | int | n/a | 1 | Generates warning when count of IP packet reassembles failed during IP packet reassembles failed check duration reaches a specified value or higher. | +| udp_buf_errors_check_duration | int | sec | 1 | UDP buf errors check duration. | +| udp_buf_errors_check_count | int | n/a | 1 | Generates warning when count of UDP buf errors during udp_buf_errors_check_duration reaches a specified value or higher. | ## NTP Monitor diff --git a/system/system_monitor/docs/seq_diagrams.md b/system/autoware_system_monitor/docs/seq_diagrams.md similarity index 100% rename from system/system_monitor/docs/seq_diagrams.md rename to system/autoware_system_monitor/docs/seq_diagrams.md diff --git a/system/system_monitor/docs/topics_cpu_monitor.md b/system/autoware_system_monitor/docs/topics_cpu_monitor.md similarity index 100% rename from system/system_monitor/docs/topics_cpu_monitor.md rename to system/autoware_system_monitor/docs/topics_cpu_monitor.md diff --git a/system/system_monitor/docs/topics_gpu_monitor.md b/system/autoware_system_monitor/docs/topics_gpu_monitor.md similarity index 100% rename from system/system_monitor/docs/topics_gpu_monitor.md rename to system/autoware_system_monitor/docs/topics_gpu_monitor.md diff --git a/system/system_monitor/docs/topics_hdd_monitor.md b/system/autoware_system_monitor/docs/topics_hdd_monitor.md similarity index 100% rename from system/system_monitor/docs/topics_hdd_monitor.md rename to system/autoware_system_monitor/docs/topics_hdd_monitor.md diff --git a/system/system_monitor/docs/topics_mem_monitor.md b/system/autoware_system_monitor/docs/topics_mem_monitor.md similarity index 100% rename from system/system_monitor/docs/topics_mem_monitor.md rename to system/autoware_system_monitor/docs/topics_mem_monitor.md diff --git a/system/system_monitor/docs/topics_net_monitor.md b/system/autoware_system_monitor/docs/topics_net_monitor.md similarity index 85% rename from system/system_monitor/docs/topics_net_monitor.md rename to system/autoware_system_monitor/docs/topics_net_monitor.md index edb067c0a5be7..84afb250929b8 100644 --- a/system/system_monitor/docs/topics_net_monitor.md +++ b/system/autoware_system_monitor/docs/topics_net_monitor.md @@ -106,3 +106,23 @@ | --------------------------------------- | --------------- | | total packet reassembles failed | 0 | | packet reassembles failed per unit time | 0 | + +## UDP Buf Errors + +/diagnostics/net_monitor: UDP Buf Errors + +[summary] + +| level | message | +| ----- | -------------- | +| OK | OK | +| WARN | UDP buf errors | + +[values] + +| key | value (example) | +| -------------------------------- | --------------- | +| total UDP rcv buf errors | 0 | +| UDP rcv buf errors per unit time | 0 | +| total UDP snd buf errors | 0 | +| UDP snd buf errors per unit time | 0 | diff --git a/system/system_monitor/docs/topics_ntp_monitor.md b/system/autoware_system_monitor/docs/topics_ntp_monitor.md similarity index 100% rename from system/system_monitor/docs/topics_ntp_monitor.md rename to system/autoware_system_monitor/docs/topics_ntp_monitor.md diff --git a/system/system_monitor/docs/topics_process_monitor.md b/system/autoware_system_monitor/docs/topics_process_monitor.md similarity index 100% rename from system/system_monitor/docs/topics_process_monitor.md rename to system/autoware_system_monitor/docs/topics_process_monitor.md diff --git a/system/system_monitor/docs/topics_voltage_monitor.md b/system/autoware_system_monitor/docs/topics_voltage_monitor.md similarity index 100% rename from system/system_monitor/docs/topics_voltage_monitor.md rename to system/autoware_system_monitor/docs/topics_voltage_monitor.md diff --git a/system/system_monitor/docs/traffic_reader.md b/system/autoware_system_monitor/docs/traffic_reader.md similarity index 100% rename from system/system_monitor/docs/traffic_reader.md rename to system/autoware_system_monitor/docs/traffic_reader.md diff --git a/system/system_monitor/include/system_monitor/cpu_monitor/arm_cpu_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/cpu_monitor/arm_cpu_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/cpu_monitor/arm_cpu_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/cpu_monitor/arm_cpu_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/cpu_monitor/cpu_monitor_base.hpp b/system/autoware_system_monitor/include/system_monitor/cpu_monitor/cpu_monitor_base.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/cpu_monitor/cpu_monitor_base.hpp rename to system/autoware_system_monitor/include/system_monitor/cpu_monitor/cpu_monitor_base.hpp diff --git a/system/system_monitor/include/system_monitor/cpu_monitor/intel_cpu_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/cpu_monitor/intel_cpu_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/cpu_monitor/intel_cpu_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/cpu_monitor/intel_cpu_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/cpu_monitor/raspi_cpu_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/cpu_monitor/raspi_cpu_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/cpu_monitor/raspi_cpu_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/cpu_monitor/raspi_cpu_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/cpu_monitor/tegra_cpu_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/cpu_monitor/tegra_cpu_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/cpu_monitor/tegra_cpu_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/cpu_monitor/tegra_cpu_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/cpu_monitor/unknown_cpu_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/cpu_monitor/unknown_cpu_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/cpu_monitor/unknown_cpu_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/cpu_monitor/unknown_cpu_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/gpu_monitor/gpu_monitor_base.hpp b/system/autoware_system_monitor/include/system_monitor/gpu_monitor/gpu_monitor_base.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/gpu_monitor/gpu_monitor_base.hpp rename to system/autoware_system_monitor/include/system_monitor/gpu_monitor/gpu_monitor_base.hpp diff --git a/system/system_monitor/include/system_monitor/gpu_monitor/nvml_gpu_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/gpu_monitor/nvml_gpu_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/gpu_monitor/nvml_gpu_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/gpu_monitor/nvml_gpu_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/gpu_monitor/tegra_gpu_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/gpu_monitor/tegra_gpu_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/gpu_monitor/tegra_gpu_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/gpu_monitor/tegra_gpu_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/gpu_monitor/unknown_gpu_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/gpu_monitor/unknown_gpu_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/gpu_monitor/unknown_gpu_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/gpu_monitor/unknown_gpu_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/hdd_monitor/hdd_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/hdd_reader/hdd_reader.hpp b/system/autoware_system_monitor/include/system_monitor/hdd_reader/hdd_reader.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/hdd_reader/hdd_reader.hpp rename to system/autoware_system_monitor/include/system_monitor/hdd_reader/hdd_reader.hpp diff --git a/system/system_monitor/include/system_monitor/mem_monitor/mem_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/mem_monitor/mem_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/mem_monitor/mem_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/mem_monitor/mem_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/msr_reader/msr_reader.hpp b/system/autoware_system_monitor/include/system_monitor/msr_reader/msr_reader.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/msr_reader/msr_reader.hpp rename to system/autoware_system_monitor/include/system_monitor/msr_reader/msr_reader.hpp diff --git a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/net_monitor/net_monitor.hpp similarity index 77% rename from system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/net_monitor/net_monitor.hpp index 5aa2cc9790143..77536c4a60615 100644 --- a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp +++ b/system/autoware_system_monitor/include/system_monitor/net_monitor/net_monitor.hpp @@ -81,6 +81,93 @@ struct CrcErrors unsigned int last_rx_crc_errors{0}; //!< @brief rx_crc_error at the time of the last monitoring }; +/** + * @brief /proc/net/snmp information + */ +class NetSnmp +{ +public: + enum class Result { + OK, + CHECK_WARNING, + READ_ERROR, + }; + + /** + * @brief Constructor + * @param [in] node node using this class. + */ + explicit NetSnmp(rclcpp::Node * node); + + /** + * @brief Constructor + */ + NetSnmp() = delete; + + /** + * @brief Copy constructor + */ + NetSnmp(const NetSnmp &) = delete; + + /** + * @brief Copy assignment operator + */ + NetSnmp & operator=(const NetSnmp &) = delete; + + /** + * @brief Move constructor + */ + NetSnmp(const NetSnmp &&) = delete; + + /** + * @brief Move assignment operator + */ + NetSnmp & operator=(const NetSnmp &&) = delete; + + /** + * @brief Set parameters for check + * @param [in] check_duration the value for check_duration + * @param [in] check_count the value for check_count + */ + void set_check_parameters(unsigned int check_duration, unsigned int check_count); + + /** + * @brief Find index in `/proc/net/snmp` + * @param [in] protocol Protocol name (the first column string). e.g. "Ip:" or "Udp:" + * @param [in] metrics Metrics name. e.g. "ReasmFails" + */ + void find_index(const std::string & protocol, const std::string & metrics); + + /** + * @brief Check metrics + * @param [out] current_value the value read from snmp + * @param [out] value_per_unit_time the increase of the value during the duration + * @return the result of check + */ + Result check_metrics(uint64_t & current_value, uint64_t & value_per_unit_time); + +private: + /** + * @brief Read value from `/proc/net/snmp` + * @param [in] index_row row in `/proc/net/snmp` + * @param [in] index_col col in `/proc/net/snmp` + * @param [out] output_value retrieved value + * @return execution result + */ + bool read_value_from_proc( + unsigned int index_row, unsigned int index_col, uint64_t & output_value); + + rclcpp::Logger logger_; //!< @brief logger gotten from user node + unsigned int check_duration_; //!< @brief check duration + unsigned int check_count_; //!< @brief check count threshold + unsigned int index_row_; //!< @brief index for the target metrics in /proc/net/snmp + unsigned int index_col_; //!< @brief index for the target metrics in /proc/net/snmp + uint64_t current_value_; //!< @brief the value read from snmp + uint64_t last_value_; //!< @brief the value read from snmp at the last monitoring + uint64_t value_per_unit_time_; //!< @brief the increase of the value during the duration + std::deque queue_; //!< @brief queue that holds the delta of the value +}; + namespace local = boost::asio::local; class NetMonitor : public rclcpp::Node @@ -150,6 +237,12 @@ class NetMonitor : public rclcpp::Node */ void check_reassembles_failed(diagnostic_updater::DiagnosticStatusWrapper & status); + /** + * @brief Check UDP buf errors + * @param [out] status diagnostic message passed directly to diagnostic publish calls + */ + void check_udp_buf_errors(diagnostic_updater::DiagnosticStatusWrapper & status); + /** * @brief Timer callback */ @@ -273,18 +366,6 @@ class NetMonitor : public rclcpp::Node */ void close_connection(); - /** - * @brief Get column index of IP packet reassembles failed from `/proc/net/snmp` - */ - void get_reassembles_failed_column_index(); - - /** - * @brief get IP packet reassembles failed - * @param [out] reassembles_failed IP packet reassembles failed - * @return execution result - */ - bool get_reassembles_failed(uint64_t & reassembles_failed); - diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to get Network information @@ -307,16 +388,9 @@ class NetMonitor : public rclcpp::Node unsigned int crc_error_check_duration_; //!< @brief CRC error check duration unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold - std::deque - reassembles_failed_queue_; //!< @brief queue that holds count of IP packet reassembles failed - uint64_t last_reassembles_failed_; //!< @brief IP packet reassembles failed at the time of the - //!< last monitoring - unsigned int - reassembles_failed_check_duration_; //!< @brief IP packet reassembles failed check duration - unsigned int - reassembles_failed_check_count_; //!< @brief IP packet reassembles failed check count threshold - unsigned int reassembles_failed_column_index_; //!< @brief column index of IP Reassembles failed - //!< in /proc/net/snmp + NetSnmp reassembles_failed_info_; //!< @brief information of IP packet reassembles failed + NetSnmp udp_rcvbuf_errors_info_; //!< @brief information of UDP rcv buf errors + NetSnmp udp_sndbuf_errors_info_; //!< @brief information of UDP snd buf errors /** * @brief Network connection status messages diff --git a/system/system_monitor/include/system_monitor/net_monitor/nl80211.hpp b/system/autoware_system_monitor/include/system_monitor/net_monitor/nl80211.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/net_monitor/nl80211.hpp rename to system/autoware_system_monitor/include/system_monitor/net_monitor/nl80211.hpp diff --git a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp b/system/autoware_system_monitor/include/system_monitor/process_monitor/diag_task.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp rename to system/autoware_system_monitor/include/system_monitor/process_monitor/diag_task.hpp diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/process_monitor/process_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/process_monitor/process_monitor.hpp diff --git a/system/system_monitor/include/system_monitor/system_monitor_utility.hpp b/system/autoware_system_monitor/include/system_monitor/system_monitor_utility.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/system_monitor_utility.hpp rename to system/autoware_system_monitor/include/system_monitor/system_monitor_utility.hpp diff --git a/system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_common.hpp b/system/autoware_system_monitor/include/system_monitor/traffic_reader/traffic_reader_common.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_common.hpp rename to system/autoware_system_monitor/include/system_monitor/traffic_reader/traffic_reader_common.hpp diff --git a/system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_service.hpp b/system/autoware_system_monitor/include/system_monitor/traffic_reader/traffic_reader_service.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/traffic_reader/traffic_reader_service.hpp rename to system/autoware_system_monitor/include/system_monitor/traffic_reader/traffic_reader_service.hpp diff --git a/system/system_monitor/include/system_monitor/voltage_monitor/voltage_monitor.hpp b/system/autoware_system_monitor/include/system_monitor/voltage_monitor/voltage_monitor.hpp similarity index 100% rename from system/system_monitor/include/system_monitor/voltage_monitor/voltage_monitor.hpp rename to system/autoware_system_monitor/include/system_monitor/voltage_monitor/voltage_monitor.hpp diff --git a/system/system_monitor/launch/system_monitor.launch.xml b/system/autoware_system_monitor/launch/system_monitor.launch.xml similarity index 52% rename from system/system_monitor/launch/system_monitor.launch.xml rename to system/autoware_system_monitor/launch/system_monitor.launch.xml index c6d29dd6b772f..14d1e122801b9 100644 --- a/system/system_monitor/launch/system_monitor.launch.xml +++ b/system/autoware_system_monitor/launch/system_monitor.launch.xml @@ -1,37 +1,37 @@ - - - - - - - - + + + + + + + + - + - + - + - + - + - + - + - + diff --git a/system/system_monitor/package.xml b/system/autoware_system_monitor/package.xml similarity index 90% rename from system/system_monitor/package.xml rename to system/autoware_system_monitor/package.xml index df63dac90551c..0212b4ec61751 100644 --- a/system/system_monitor/package.xml +++ b/system/autoware_system_monitor/package.xml @@ -1,11 +1,12 @@ - system_monitor - 0.40.0 + autoware_system_monitor + 0.41.0 The system_monitor package Fumihito Ito TetsuKawa + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp b/system/autoware_system_monitor/reader/hdd_reader/hdd_reader.cpp similarity index 94% rename from system/system_monitor/reader/hdd_reader/hdd_reader.cpp rename to system/autoware_system_monitor/reader/hdd_reader/hdd_reader.cpp index 740f841382f47..098f4240782ae 100644 --- a/system/system_monitor/reader/hdd_reader/hdd_reader.cpp +++ b/system/autoware_system_monitor/reader/hdd_reader/hdd_reader.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include #include @@ -655,48 +656,56 @@ void run(int port) int main(int argc, char ** argv) { - static struct option long_options[] = { - {"help", no_argument, nullptr, 'h'}, - {"port", required_argument, nullptr, 'p'}, - {nullptr, 0, nullptr, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; - - default: - break; + try { + static struct option long_options[] = { + {"help", no_argument, nullptr, 'h'}, + {"port", required_argument, nullptr, 'p'}, + {nullptr, 0, nullptr, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } } - } - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); - run(port); + run(port); - // Close descriptor used to write to system logger - closelog(); + // Close descriptor used to write to system logger + closelog(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return EXIT_FAILURE; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return EXIT_FAILURE; + } return EXIT_SUCCESS; } diff --git a/system/system_monitor/reader/msr_reader/msr_reader.cpp b/system/autoware_system_monitor/reader/msr_reader/msr_reader.cpp similarity index 74% rename from system/system_monitor/reader/msr_reader/msr_reader.cpp rename to system/autoware_system_monitor/reader/msr_reader/msr_reader.cpp index fc7bcab795be0..89f95d05c282e 100644 --- a/system/system_monitor/reader/msr_reader/msr_reader.cpp +++ b/system/autoware_system_monitor/reader/msr_reader/msr_reader.cpp @@ -212,90 +212,98 @@ void run(int port, const std::vector & list) int main(int argc, char ** argv) { - static struct option long_options[] = { - {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; + try { + static struct option long_options[] = { + {"help", no_argument, 0, 'h'}, {"port", required_argument, 0, 'p'}, {0, 0, 0, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + int port = PORT; + while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 'p': + try { + port = boost::lexical_cast(optarg); + } catch (const boost::bad_lexical_cast & e) { + printf("Error: %s\n", e.what()); + return EXIT_FAILURE; + } + break; + + default: + break; + } + } - default: - break; + if (!fs::exists("/dev/cpu")) { + printf("Failed to access /dev/cpu.\n"); + return EXIT_FAILURE; } - } - if (!fs::exists("/dev/cpu")) { - printf("Failed to access /dev/cpu.\n"); - return EXIT_FAILURE; - } + std::vector list; + const fs::path root("/dev/cpu"); - std::vector list; - const fs::path root("/dev/cpu"); + for (const fs::path & path : boost::make_iterator_range( + fs::recursive_directory_iterator(root), fs::recursive_directory_iterator())) { + if (fs::is_directory(path)) { + continue; + } - for (const fs::path & path : boost::make_iterator_range( - fs::recursive_directory_iterator(root), fs::recursive_directory_iterator())) { - if (fs::is_directory(path)) { - continue; - } + std::cmatch match; + const char * msr = path.generic_string().c_str(); - std::cmatch match; - const char * msr = path.generic_string().c_str(); + // /dev/cpu/[0-9]/msr ? + if (!std::regex_match(msr, match, std::regex(".*msr"))) { + continue; + } - // /dev/cpu/[0-9]/msr ? - if (!std::regex_match(msr, match, std::regex(".*msr"))) { - continue; + list.push_back(path.generic_string()); } - list.push_back(path.generic_string()); - } + std::sort(list.begin(), list.end(), [](const std::string & c1, const std::string & c2) { + std::cmatch match; + const std::regex filter(".*/(\\d+)/msr"); + int n1 = 0; + int n2 = 0; + if (std::regex_match(c1.c_str(), match, filter)) { + n1 = std::stoi(match[1].str()); + } + if (std::regex_match(c2.c_str(), match, filter)) { + n2 = std::stoi(match[1].str()); + } + return n1 < n2; + }); // NOLINT - std::sort(list.begin(), list.end(), [](const std::string & c1, const std::string & c2) { - std::cmatch match; - const std::regex filter(".*/(\\d+)/msr"); - int n1 = 0; - int n2 = 0; - if (std::regex_match(c1.c_str(), match, filter)) { - n1 = std::stoi(match[1].str()); - } - if (std::regex_match(c2.c_str(), match, filter)) { - n2 = std::stoi(match[1].str()); + if (list.empty()) { + printf("No msr found in /dev/cpu.\n"); + return EXIT_FAILURE; } - return n1 < n2; - }); // NOLINT - - if (list.empty()) { - printf("No msr found in /dev/cpu.\n"); - return EXIT_FAILURE; - } - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); - run(port, list); + run(port, list); - // Close descriptor used to write to system logger - closelog(); + // Close descriptor used to write to system logger + closelog(); + } catch (const std::exception & e) { + std::cerr << "Exception in main(): " << e.what() << std::endl; + return EXIT_FAILURE; + } catch (...) { + std::cerr << "Unknown exception in main()" << std::endl; + return EXIT_FAILURE; + } return EXIT_SUCCESS; } diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp b/system/autoware_system_monitor/reader/traffic_reader/traffic_reader_main.cpp similarity index 100% rename from system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp rename to system/autoware_system_monitor/reader/traffic_reader/traffic_reader_main.cpp diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp b/system/autoware_system_monitor/reader/traffic_reader/traffic_reader_service.cpp similarity index 100% rename from system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp rename to system/autoware_system_monitor/reader/traffic_reader/traffic_reader_service.cpp diff --git a/system/system_monitor/src/cpu_monitor/arm_cpu_monitor.cpp b/system/autoware_system_monitor/src/cpu_monitor/arm_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/src/cpu_monitor/arm_cpu_monitor.cpp rename to system/autoware_system_monitor/src/cpu_monitor/arm_cpu_monitor.cpp diff --git a/system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp b/system/autoware_system_monitor/src/cpu_monitor/cpu_monitor_base.cpp similarity index 100% rename from system/system_monitor/src/cpu_monitor/cpu_monitor_base.cpp rename to system/autoware_system_monitor/src/cpu_monitor/cpu_monitor_base.cpp diff --git a/system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp b/system/autoware_system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp rename to system/autoware_system_monitor/src/cpu_monitor/intel_cpu_monitor.cpp diff --git a/system/system_monitor/src/cpu_monitor/raspi_cpu_monitor.cpp b/system/autoware_system_monitor/src/cpu_monitor/raspi_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/src/cpu_monitor/raspi_cpu_monitor.cpp rename to system/autoware_system_monitor/src/cpu_monitor/raspi_cpu_monitor.cpp diff --git a/system/system_monitor/src/cpu_monitor/tegra_cpu_monitor.cpp b/system/autoware_system_monitor/src/cpu_monitor/tegra_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/src/cpu_monitor/tegra_cpu_monitor.cpp rename to system/autoware_system_monitor/src/cpu_monitor/tegra_cpu_monitor.cpp diff --git a/system/system_monitor/src/cpu_monitor/unknown_cpu_monitor.cpp b/system/autoware_system_monitor/src/cpu_monitor/unknown_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/src/cpu_monitor/unknown_cpu_monitor.cpp rename to system/autoware_system_monitor/src/cpu_monitor/unknown_cpu_monitor.cpp diff --git a/system/system_monitor/src/gpu_monitor/gpu_monitor_base.cpp b/system/autoware_system_monitor/src/gpu_monitor/gpu_monitor_base.cpp similarity index 100% rename from system/system_monitor/src/gpu_monitor/gpu_monitor_base.cpp rename to system/autoware_system_monitor/src/gpu_monitor/gpu_monitor_base.cpp diff --git a/system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp b/system/autoware_system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp similarity index 100% rename from system/system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp rename to system/autoware_system_monitor/src/gpu_monitor/nvml_gpu_monitor.cpp diff --git a/system/system_monitor/src/gpu_monitor/tegra_gpu_monitor.cpp b/system/autoware_system_monitor/src/gpu_monitor/tegra_gpu_monitor.cpp similarity index 100% rename from system/system_monitor/src/gpu_monitor/tegra_gpu_monitor.cpp rename to system/autoware_system_monitor/src/gpu_monitor/tegra_gpu_monitor.cpp diff --git a/system/system_monitor/src/gpu_monitor/unknown_gpu_monitor.cpp b/system/autoware_system_monitor/src/gpu_monitor/unknown_gpu_monitor.cpp similarity index 100% rename from system/system_monitor/src/gpu_monitor/unknown_gpu_monitor.cpp rename to system/autoware_system_monitor/src/gpu_monitor/unknown_gpu_monitor.cpp diff --git a/system/system_monitor/src/hdd_monitor/hdd_monitor.cpp b/system/autoware_system_monitor/src/hdd_monitor/hdd_monitor.cpp similarity index 100% rename from system/system_monitor/src/hdd_monitor/hdd_monitor.cpp rename to system/autoware_system_monitor/src/hdd_monitor/hdd_monitor.cpp diff --git a/system/system_monitor/src/mem_monitor/mem_monitor.cpp b/system/autoware_system_monitor/src/mem_monitor/mem_monitor.cpp similarity index 100% rename from system/system_monitor/src/mem_monitor/mem_monitor.cpp rename to system/autoware_system_monitor/src/mem_monitor/mem_monitor.cpp diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/autoware_system_monitor/src/net_monitor/net_monitor.cpp similarity index 76% rename from system/system_monitor/src/net_monitor/net_monitor.cpp rename to system/autoware_system_monitor/src/net_monitor/net_monitor.cpp index d99150f0b3037..fd00d7f7b895b 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/autoware_system_monitor/src/net_monitor/net_monitor.cpp @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -51,11 +52,9 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) socket_path_(declare_parameter("socket_path", traffic_reader_service::socket_path)), crc_error_check_duration_(declare_parameter("crc_error_check_duration", 1)), crc_error_count_threshold_(declare_parameter("crc_error_count_threshold", 1)), - last_reassembles_failed_(0), - reassembles_failed_check_duration_( - declare_parameter("reassembles_failed_check_duration", 1)), - reassembles_failed_check_count_(declare_parameter("reassembles_failed_check_count", 1)), - reassembles_failed_column_index_(0) + reassembles_failed_info_(this), + udp_rcvbuf_errors_info_(this), + udp_sndbuf_errors_info_(this) { if (monitor_program_.empty()) { monitor_program_ = "*"; @@ -68,6 +67,7 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) updater_.add("Network Traffic", this, &NetMonitor::monitor_traffic); updater_.add("Network CRC Error", this, &NetMonitor::check_crc_error); updater_.add("IP Packet Reassembles Failed", this, &NetMonitor::check_reassembles_failed); + updater_.add("UDP Buf Errors", this, &NetMonitor::check_udp_buf_errors); nl80211_.init(); @@ -83,8 +83,21 @@ NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) using namespace std::literals::chrono_literals; timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&NetMonitor::on_timer, this)); - // Get column index of IP packet reassembles failed from `/proc/net/snmp` - get_reassembles_failed_column_index(); + // Initialize information for `/proc/net/snmp` + int reassembles_failed_check_duration = + declare_parameter("reassembles_failed_check_duration", 1); + int reassembles_failed_check_count = declare_parameter("reassembles_failed_check_count", 1); + int udp_buf_errors_check_duration = declare_parameter("udp_buf_errors_check_duration", 1); + int udp_buf_errors_check_count = declare_parameter("udp_buf_errors_check_count", 1); + reassembles_failed_info_.set_check_parameters( + reassembles_failed_check_duration, reassembles_failed_check_count); + udp_rcvbuf_errors_info_.set_check_parameters( + udp_buf_errors_check_duration, udp_buf_errors_check_count); + udp_sndbuf_errors_info_.set_check_parameters( + udp_buf_errors_check_duration, udp_buf_errors_check_count); + reassembles_failed_info_.find_index("Ip:", "ReasmFails"); + udp_rcvbuf_errors_info_.find_index("Udp:", "RcvbufErrors"); + udp_sndbuf_errors_info_.find_index("Udp:", "SndbufErrors"); // Send request to start nethogs if (enable_traffic_monitor_) { @@ -292,41 +305,66 @@ void NetMonitor::check_reassembles_failed(diagnostic_updater::DiagnosticStatusWr // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); - int whole_level = DiagStatus::OK; - std::string error_message; uint64_t total_reassembles_failed = 0; + uint64_t unit_reassembles_failed = 0; + NetSnmp::Result ret = + reassembles_failed_info_.check_metrics(total_reassembles_failed, unit_reassembles_failed); + status.add("total packet reassembles failed", total_reassembles_failed); + status.add("packet reassembles failed per unit time", unit_reassembles_failed); - if (get_reassembles_failed(total_reassembles_failed)) { - reassembles_failed_queue_.push_back(total_reassembles_failed - last_reassembles_failed_); - while (reassembles_failed_queue_.size() > reassembles_failed_check_duration_) { - reassembles_failed_queue_.pop_front(); - } + int whole_level = DiagStatus::OK; + std::string error_message = "OK"; + switch (ret) { + case NetSnmp::Result::OK: + default: + break; + case NetSnmp::Result::CHECK_WARNING: + whole_level = DiagStatus::WARN; + error_message = "reassembles failed"; + break; + case NetSnmp::Result::READ_ERROR: + whole_level = DiagStatus::ERROR; + error_message = "failed to read /proc/net/snmp"; + break; + } - uint64_t unit_reassembles_failed = 0; - for (auto reassembles_failed : reassembles_failed_queue_) { - unit_reassembles_failed += reassembles_failed; - } + status.summary(whole_level, error_message); - status.add(fmt::format("total packet reassembles failed"), total_reassembles_failed); - status.add(fmt::format("packet reassembles failed per unit time"), unit_reassembles_failed); + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, status); +} - if (unit_reassembles_failed >= reassembles_failed_check_count_) { - whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); - error_message = "reassembles failed"; - } +void NetMonitor::check_udp_buf_errors(diagnostic_updater::DiagnosticStatusWrapper & status) +{ + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); - last_reassembles_failed_ = total_reassembles_failed; - } else { - reassembles_failed_queue_.push_back(0); - whole_level = std::max(whole_level, static_cast(DiagStatus::ERROR)); + uint64_t total_udp_rcvbuf_errors = 0; + uint64_t unit_udp_rcvbuf_errors = 0; + NetSnmp::Result ret_rcv = + udp_rcvbuf_errors_info_.check_metrics(total_udp_rcvbuf_errors, unit_udp_rcvbuf_errors); + status.add("total UDP rcv buf errors", total_udp_rcvbuf_errors); + status.add("UDP rcv buf errors per unit time", unit_udp_rcvbuf_errors); + + uint64_t total_udp_sndbuf_errors = 0; + uint64_t unit_udp_sndbuf_errors = 0; + NetSnmp::Result ret_snd = + udp_sndbuf_errors_info_.check_metrics(total_udp_sndbuf_errors, unit_udp_sndbuf_errors); + status.add("total UDP snd buf errors", total_udp_sndbuf_errors); + status.add("UDP snd buf errors per unit time", unit_udp_sndbuf_errors); + + int whole_level = DiagStatus::OK; + std::string error_message = "OK"; + if (ret_rcv == NetSnmp::Result::READ_ERROR || ret_snd == NetSnmp::Result::READ_ERROR) { + whole_level = DiagStatus::ERROR; error_message = "failed to read /proc/net/snmp"; + } else if ( + ret_rcv == NetSnmp::Result::CHECK_WARNING || ret_snd == NetSnmp::Result::CHECK_WARNING) { + whole_level = DiagStatus::WARN; + error_message = "UDP buf errors"; } - if (!error_message.empty()) { - status.summary(whole_level, error_message); - } else { - status.summary(whole_level, "OK"); - } + status.summary(whole_level, error_message); // Measure elapsed time since start time and report SystemMonitorUtility::stopMeasurement(t_start, status); @@ -544,90 +582,6 @@ void NetMonitor::update_crc_error(NetworkInfomation & network, const struct rtnl crc_errors.last_rx_crc_errors = stats->rx_crc_errors; } -void NetMonitor::get_reassembles_failed_column_index() -{ - std::ifstream ifs("/proc/net/snmp"); - if (!ifs) { - RCLCPP_WARN(get_logger(), "Failed to open /proc/net/snmp."); - return; - } - - // Find column index of 'ReasmFails' - std::string line; - if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get header of /proc/net/snmp."); - return; - } - - // /proc/net/snmp - // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... - // Ip: 2 64 5636471397 ... 135 2303339 216166 270 .. - std::vector header_list; - boost::split(header_list, line, boost::is_space()); - - if (header_list.empty()) { - RCLCPP_WARN(get_logger(), "Failed to get header list of /proc/net/snmp."); - return; - } - if (header_list[0] != "Ip:") { - RCLCPP_WARN( - get_logger(), "Header column is invalid in /proc/net/snmp. %s", header_list[0].c_str()); - return; - } - - int index = 0; - for (const auto & header : header_list) { - if (header == "ReasmFails") { - reassembles_failed_column_index_ = index; - break; - } - ++index; - } -} - -bool NetMonitor::get_reassembles_failed(uint64_t & reassembles_failed) -{ - if (reassembles_failed_column_index_ == 0) { - RCLCPP_WARN(get_logger(), "Column index is invalid. : %d", reassembles_failed_column_index_); - return false; - } - - std::ifstream ifs("/proc/net/snmp"); - if (!ifs) { - RCLCPP_WARN(get_logger(), "Failed to open /proc/net/snmp."); - return false; - } - - std::string line; - - // Skip header row - if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get header of /proc/net/snmp."); - return false; - } - - // Find a value of 'ReasmFails' - if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get a line of /proc/net/snmp."); - return false; - } - - std::vector value_list; - boost::split(value_list, line, boost::is_space()); - - if (reassembles_failed_column_index_ >= value_list.size()) { - RCLCPP_WARN( - get_logger(), - "There are not enough columns for reassembles failed column index. : columns=%d index=%d", - static_cast(value_list.size()), reassembles_failed_column_index_); - return false; - } - - reassembles_failed = std::stoull(value_list[reassembles_failed_column_index_]); - - return true; -} - void NetMonitor::send_start_nethogs_request() { // Connect to boot/shutdown service @@ -701,7 +655,7 @@ bool NetMonitor::connect_service() socket_->connect(endpoint, error_code); if (error_code) { - RCLCPP_ERROR(get_logger(), "Failed to connect socket. %s", error_code.message().c_str()); + RCLCPP_ERROR_ONCE(get_logger(), "Failed to connect socket. %s", error_code.message().c_str()); return false; } @@ -782,5 +736,145 @@ void NetMonitor::close_connection() socket_->close(); } +NetSnmp::NetSnmp(rclcpp::Node * node) +: logger_(node->get_logger().get_child("net_snmp")), + check_duration_(1), + check_count_(1), + index_row_(0), + index_col_(0), + current_value_(0), + last_value_(0), + value_per_unit_time_(0), + queue_() +{ +} + +void NetSnmp::set_check_parameters(unsigned int check_duration, unsigned int check_count) +{ + check_duration_ = check_duration; + check_count_ = check_count; +} + +void NetSnmp::find_index(const std::string & protocol, const std::string & metrics) +{ + // /proc/net/snmp + // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... + // Ip: 2 64 5636471397 ... 135 2303339 216166 270 .. + std::ifstream ifs("/proc/net/snmp"); + if (!ifs) { + RCLCPP_WARN(logger_, "Failed to open /proc/net/snmp."); + index_row_ = index_col_ = 0; + return; + } + + std::vector target_header_list; + std::string line; + while (std::getline(ifs, line)) { + std::vector header_list; + boost::split(header_list, line, boost::is_space()); + if (header_list.empty()) continue; + if (header_list[0] == protocol) { + target_header_list = header_list; + break; + } + ++index_row_; + } + + ++index_row_; // The values are placed in the row following the header + + for (const auto & header : target_header_list) { + if (header == metrics) { + return; + } + ++index_col_; + } + + RCLCPP_WARN(logger_, "Failed to get header of /proc/net/snmp."); + index_row_ = index_col_ = 0; + return; +} + +NetSnmp::Result NetSnmp::check_metrics(uint64_t & current_value, uint64_t & value_per_unit_time) +{ + if (!read_value_from_proc(index_row_, index_col_, current_value_)) { + queue_.push_back(0); + current_value = value_per_unit_time = 0; + return Result::READ_ERROR; + } + + if (queue_.empty()) { + last_value_ = current_value_; + } + queue_.push_back(current_value_ - last_value_); + last_value_ = current_value_; + while (queue_.size() > check_duration_) { + queue_.pop_front(); + } + + value_per_unit_time_ = std::accumulate(queue_.begin(), queue_.end(), static_cast(0)); + + current_value = current_value_; + value_per_unit_time = value_per_unit_time_; + + if (value_per_unit_time_ >= check_count_) { + return Result::CHECK_WARNING; + } else { + return Result::OK; + } +} + +bool NetSnmp::read_value_from_proc( + unsigned int index_row, unsigned int index_col, uint64_t & output_value) +{ + if (index_row == 0 && index_col == 0) { + RCLCPP_WARN_ONCE(logger_, "index is invalid. : %u, %u", index_row, index_col); + return false; + } + + std::ifstream ifs("/proc/net/snmp"); + if (!ifs) { + RCLCPP_WARN_ONCE(logger_, "Failed to open /proc/net/snmp."); + return false; + } + + std::string target_line; + std::string line; + for (unsigned int row_index = 0; std::getline(ifs, line); ++row_index) { + if (row_index == index_row) { + target_line = line; + break; + } + } + + if (target_line.empty()) { + RCLCPP_WARN_ONCE(logger_, "Failed to get a line of /proc/net/snmp."); + return false; + } + + std::vector value_list; + boost::split(value_list, target_line, boost::is_space()); + if (index_col >= value_list.size()) { + RCLCPP_WARN_ONCE( + logger_, "There are not enough columns for the column index. : column size=%lu index=%u, %u", + value_list.size(), index_row, index_col); + return false; + } + + std::string value_str = value_list[index_col]; + if (value_str.empty()) { + RCLCPP_WARN_ONCE(logger_, "The value is empty. : index=%u, %u", index_row, index_col); + return false; + } + + if (value_str[0] == '-') { + RCLCPP_WARN_ONCE(logger_, "The value is minus. : %s", value_str.c_str()); + output_value = 0; + return false; + } else { + output_value = std::stoull(value_str); + return true; + } +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(NetMonitor) diff --git a/system/system_monitor/src/net_monitor/nl80211.cpp b/system/autoware_system_monitor/src/net_monitor/nl80211.cpp similarity index 100% rename from system/system_monitor/src/net_monitor/nl80211.cpp rename to system/autoware_system_monitor/src/net_monitor/nl80211.cpp diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/autoware_system_monitor/src/ntp_monitor/ntp_monitor.cpp similarity index 100% rename from system/system_monitor/src/ntp_monitor/ntp_monitor.cpp rename to system/autoware_system_monitor/src/ntp_monitor/ntp_monitor.cpp diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/autoware_system_monitor/src/process_monitor/process_monitor.cpp similarity index 100% rename from system/system_monitor/src/process_monitor/process_monitor.cpp rename to system/autoware_system_monitor/src/process_monitor/process_monitor.cpp diff --git a/system/system_monitor/src/voltage_monitor/voltage_monitor.cpp b/system/autoware_system_monitor/src/voltage_monitor/voltage_monitor.cpp similarity index 100% rename from system/system_monitor/src/voltage_monitor/voltage_monitor.cpp rename to system/autoware_system_monitor/src/voltage_monitor/voltage_monitor.cpp diff --git a/system/system_monitor/test/config/test_hdd_monitor.param.yaml b/system/autoware_system_monitor/test/config/test_hdd_monitor.param.yaml similarity index 100% rename from system/system_monitor/test/config/test_hdd_monitor.param.yaml rename to system/autoware_system_monitor/test/config/test_hdd_monitor.param.yaml diff --git a/system/system_monitor/test/config/test_net_monitor.param.yaml b/system/autoware_system_monitor/test/config/test_net_monitor.param.yaml similarity index 100% rename from system/system_monitor/test/config/test_net_monitor.param.yaml rename to system/autoware_system_monitor/test/config/test_net_monitor.param.yaml diff --git a/system/system_monitor/test/config/test_ntp_monitor.param.yaml b/system/autoware_system_monitor/test/config/test_ntp_monitor.param.yaml similarity index 100% rename from system/system_monitor/test/config/test_ntp_monitor.param.yaml rename to system/autoware_system_monitor/test/config/test_ntp_monitor.param.yaml diff --git a/system/system_monitor/test/src/cpu_monitor/mpstat1.cpp b/system/autoware_system_monitor/test/src/cpu_monitor/mpstat1.cpp similarity index 100% rename from system/system_monitor/test/src/cpu_monitor/mpstat1.cpp rename to system/autoware_system_monitor/test/src/cpu_monitor/mpstat1.cpp diff --git a/system/system_monitor/test/src/cpu_monitor/mpstat2.cpp b/system/autoware_system_monitor/test/src/cpu_monitor/mpstat2.cpp similarity index 100% rename from system/system_monitor/test/src/cpu_monitor/mpstat2.cpp rename to system/autoware_system_monitor/test/src/cpu_monitor/mpstat2.cpp diff --git a/system/system_monitor/test/src/cpu_monitor/test_arm_cpu_monitor.cpp b/system/autoware_system_monitor/test/src/cpu_monitor/test_arm_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/cpu_monitor/test_arm_cpu_monitor.cpp rename to system/autoware_system_monitor/test/src/cpu_monitor/test_arm_cpu_monitor.cpp diff --git a/system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp b/system/autoware_system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp rename to system/autoware_system_monitor/test/src/cpu_monitor/test_intel_cpu_monitor.cpp diff --git a/system/system_monitor/test/src/cpu_monitor/test_raspi_cpu_monitor.cpp b/system/autoware_system_monitor/test/src/cpu_monitor/test_raspi_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/cpu_monitor/test_raspi_cpu_monitor.cpp rename to system/autoware_system_monitor/test/src/cpu_monitor/test_raspi_cpu_monitor.cpp diff --git a/system/system_monitor/test/src/cpu_monitor/test_tegra_cpu_monitor.cpp b/system/autoware_system_monitor/test/src/cpu_monitor/test_tegra_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/cpu_monitor/test_tegra_cpu_monitor.cpp rename to system/autoware_system_monitor/test/src/cpu_monitor/test_tegra_cpu_monitor.cpp diff --git a/system/system_monitor/test/src/cpu_monitor/test_unknown_cpu_monitor.cpp b/system/autoware_system_monitor/test/src/cpu_monitor/test_unknown_cpu_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/cpu_monitor/test_unknown_cpu_monitor.cpp rename to system/autoware_system_monitor/test/src/cpu_monitor/test_unknown_cpu_monitor.cpp diff --git a/system/system_monitor/test/src/gpu_monitor/test_nvml_gpu_monitor.cpp b/system/autoware_system_monitor/test/src/gpu_monitor/test_nvml_gpu_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/gpu_monitor/test_nvml_gpu_monitor.cpp rename to system/autoware_system_monitor/test/src/gpu_monitor/test_nvml_gpu_monitor.cpp diff --git a/system/system_monitor/test/src/gpu_monitor/test_tegra_gpu_monitor.cpp b/system/autoware_system_monitor/test/src/gpu_monitor/test_tegra_gpu_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/gpu_monitor/test_tegra_gpu_monitor.cpp rename to system/autoware_system_monitor/test/src/gpu_monitor/test_tegra_gpu_monitor.cpp diff --git a/system/system_monitor/test/src/gpu_monitor/test_unknown_gpu_monitor.cpp b/system/autoware_system_monitor/test/src/gpu_monitor/test_unknown_gpu_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/gpu_monitor/test_unknown_gpu_monitor.cpp rename to system/autoware_system_monitor/test/src/gpu_monitor/test_unknown_gpu_monitor.cpp diff --git a/system/system_monitor/test/src/hdd_monitor/df1.cpp b/system/autoware_system_monitor/test/src/hdd_monitor/df1.cpp similarity index 100% rename from system/system_monitor/test/src/hdd_monitor/df1.cpp rename to system/autoware_system_monitor/test/src/hdd_monitor/df1.cpp diff --git a/system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp b/system/autoware_system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp rename to system/autoware_system_monitor/test/src/hdd_monitor/test_hdd_monitor.cpp diff --git a/system/system_monitor/test/src/mem_monitor/free1.cpp b/system/autoware_system_monitor/test/src/mem_monitor/free1.cpp similarity index 100% rename from system/system_monitor/test/src/mem_monitor/free1.cpp rename to system/autoware_system_monitor/test/src/mem_monitor/free1.cpp diff --git a/system/system_monitor/test/src/mem_monitor/test_mem_monitor.cpp b/system/autoware_system_monitor/test/src/mem_monitor/test_mem_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/mem_monitor/test_mem_monitor.cpp rename to system/autoware_system_monitor/test/src/mem_monitor/test_mem_monitor.cpp diff --git a/system/system_monitor/test/src/net_monitor/test_net_monitor.cpp b/system/autoware_system_monitor/test/src/net_monitor/test_net_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/net_monitor/test_net_monitor.cpp rename to system/autoware_system_monitor/test/src/net_monitor/test_net_monitor.cpp diff --git a/system/system_monitor/test/src/ntp_monitor/ntpdate1.cpp b/system/autoware_system_monitor/test/src/ntp_monitor/ntpdate1.cpp similarity index 100% rename from system/system_monitor/test/src/ntp_monitor/ntpdate1.cpp rename to system/autoware_system_monitor/test/src/ntp_monitor/ntpdate1.cpp diff --git a/system/system_monitor/test/src/ntp_monitor/test_ntp_monitor.cpp b/system/autoware_system_monitor/test/src/ntp_monitor/test_ntp_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/ntp_monitor/test_ntp_monitor.cpp rename to system/autoware_system_monitor/test/src/ntp_monitor/test_ntp_monitor.cpp diff --git a/system/system_monitor/test/src/process_monitor/echo1.cpp b/system/autoware_system_monitor/test/src/process_monitor/echo1.cpp similarity index 100% rename from system/system_monitor/test/src/process_monitor/echo1.cpp rename to system/autoware_system_monitor/test/src/process_monitor/echo1.cpp diff --git a/system/system_monitor/test/src/process_monitor/sed1.cpp b/system/autoware_system_monitor/test/src/process_monitor/sed1.cpp similarity index 100% rename from system/system_monitor/test/src/process_monitor/sed1.cpp rename to system/autoware_system_monitor/test/src/process_monitor/sed1.cpp diff --git a/system/system_monitor/test/src/process_monitor/sort1.cpp b/system/autoware_system_monitor/test/src/process_monitor/sort1.cpp similarity index 100% rename from system/system_monitor/test/src/process_monitor/sort1.cpp rename to system/autoware_system_monitor/test/src/process_monitor/sort1.cpp diff --git a/system/system_monitor/test/src/process_monitor/test_process_monitor.cpp b/system/autoware_system_monitor/test/src/process_monitor/test_process_monitor.cpp similarity index 100% rename from system/system_monitor/test/src/process_monitor/test_process_monitor.cpp rename to system/autoware_system_monitor/test/src/process_monitor/test_process_monitor.cpp diff --git a/system/system_monitor/test/src/process_monitor/top1.cpp b/system/autoware_system_monitor/test/src/process_monitor/top1.cpp similarity index 100% rename from system/system_monitor/test/src/process_monitor/top1.cpp rename to system/autoware_system_monitor/test/src/process_monitor/top1.cpp diff --git a/system/system_monitor/test/src/process_monitor/top2.cpp b/system/autoware_system_monitor/test/src/process_monitor/top2.cpp similarity index 100% rename from system/system_monitor/test/src/process_monitor/top2.cpp rename to system/autoware_system_monitor/test/src/process_monitor/top2.cpp diff --git a/system/system_monitor/test/src/process_monitor/top3.cpp b/system/autoware_system_monitor/test/src/process_monitor/top3.cpp similarity index 100% rename from system/system_monitor/test/src/process_monitor/top3.cpp rename to system/autoware_system_monitor/test/src/process_monitor/top3.cpp diff --git a/system/autoware_topic_relay_controller/CHANGELOG.rst b/system/autoware_topic_relay_controller/CHANGELOG.rst new file mode 100644 index 0000000000000..e08d796d12b5d --- /dev/null +++ b/system/autoware_topic_relay_controller/CHANGELOG.rst @@ -0,0 +1,78 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_topic_relay_controller +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_topic_relay_controller): add topic relay controller node (`#9964 `_) + * feat: add node base + * modify: include guard + * feat: delete schema + * feat: delete config + * feat: add subscriber + * modify: add include + * feat: add publisher + * feat: add service + * modify: typo + * style(pre-commit): autofix + * modify: add include memory + * modify: add qos setting + * style(pre-commit): autofix + * feat: add enable_keep_publishing + * style(pre-commit): autofix + * feat: add readme + * style(pre-commit): autofix + * feat: change qos name and add parameter type + * feat: add config and delete arg from launch + * modify: typo + * style(pre-commit): autofix + * modify: add comment + * modify: modify comment + * feat: add maintainer + * modift: change readme param format + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, TetsuKawa + +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_topic_relay_controller): add topic relay controller node (`#9964 `_) + * feat: add node base + * modify: include guard + * feat: delete schema + * feat: delete config + * feat: add subscriber + * modify: add include + * feat: add publisher + * feat: add service + * modify: typo + * style(pre-commit): autofix + * modify: add include memory + * modify: add qos setting + * style(pre-commit): autofix + * feat: add enable_keep_publishing + * style(pre-commit): autofix + * feat: add readme + * style(pre-commit): autofix + * feat: change qos name and add parameter type + * feat: add config and delete arg from launch + * modify: typo + * style(pre-commit): autofix + * modify: add comment + * modify: modify comment + * feat: add maintainer + * modift: change readme param format + * style(pre-commit): autofix + --------- + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> +* Contributors: Fumiya Watanabe, TetsuKawa + +0.40.0 (2025-01-06) +------------------- + +0.39.0 (2024-11-25) +------------------- + +0.38.0 (2024-11-11) +------------------- diff --git a/system/autoware_topic_relay_controller/CMakeLists.txt b/system/autoware_topic_relay_controller/CMakeLists.txt new file mode 100644 index 0000000000000..768affd7fd121 --- /dev/null +++ b/system/autoware_topic_relay_controller/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_topic_relay_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/topic_relay_controller_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::topic_relay_controller::TopicRelayController" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + +ament_auto_package(INSTALL_TO_SHARE + config + launch +) diff --git a/system/autoware_topic_relay_controller/README.md b/system/autoware_topic_relay_controller/README.md new file mode 100644 index 0000000000000..a5651ff32fbd2 --- /dev/null +++ b/system/autoware_topic_relay_controller/README.md @@ -0,0 +1,45 @@ +# topic_relay_controller + +## Purpose + +The node subscribes to a specified topic, remaps it, and republishes it. Additionally, it has the capability to continue publishing the last received value if the subscription stops. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------ | -------------------------- | -------------------------------------------------------------------- | +| `/` | `` | Topic to be subscribed, as defined by the `topic` parameter. | +| `/tf` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf`, used for transform message relay. | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | (Optional) If the topic is `/tf_static`, used for static transforms. | + +### Output + +| Name | Type | Description | +| ---------------- | -------------------------- | ----------------------------------------------------------------------------- | +| `/` | `` | Republished topic after remapping, as defined by the `remap_topic` parameter. | + +## Parameters + +| Variable | Type | Description | +| ---------------------- | ------- | ---------------------------------------------------------------------------------------------------- | +| topic | string | The name of the input topic to subscribe to | +| remap_topic | string | The name of the output topic to publish to | +| topic_type | string | The type of messages being relayed | +| qos | integer | QoS profile to use for subscriptions and publications (default: `1`) | +| transient_local | boolean | Enables transient local QoS for subscribers (default: `false`) | +| best_effort | boolean | Enables best-effort QoS for subscribers (default: `false`) | +| enable_relay_control | boolean | Allows dynamic relay control via a service (default: `true`) | +| srv_name | string | The service name for relay control when `enable_relay_control` is `true` | +| enable_keep_publishing | boolean | Keeps publishing the last received topic value when not subscribed (default: `false`) | +| update_rate | integer | The rate (Hz) for publishing the last topic value when `enable_keep_publishing` is `true` (optional) | +| frame_id | string | Frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional) | +| child_frame_id | string | Child frame ID for transform messages when subscribing to `/tf` or `/tf_static` (optional) | + +## Assumptions / Known limits + +- The node assumes that the specified `topic` and `remap_topic` are valid and accessible within the ROS 2 environment. +- If `enable_keep_publishing` is `true`, the node continuously republishes the last received value even if no new messages are being received. +- For `/tf` and `/tf_static`, additional parameters like `frame_id` and `child_frame_id` are required for selective transformation relays. +- QoS settings must be carefully chosen to match the requirements of the subscribed and published topics. diff --git a/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml new file mode 100644 index 0000000000000..ad2665497ca9e --- /dev/null +++ b/system/autoware_topic_relay_controller/config/topic_relay_controller.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + topic: "/default" + topic_type: "std_msgs/msg/String" # unnecessary for /tf or /tf_static + remap_topic: "/remap_default" + #frame_id: necessary for /tf or /tf_static + #child_frame_id: necessary for /tf or /tf_static + qos_depth: 1 + transient_local: false # Change qos to transient_local. Default is volatile. + best_effort: false # Change qos to best_effort. Default is reliable. + enable_relay_control: true + srv_name: "/system/topic_relay_controller_default/operate" # necessary for enable_relay_control is true + enable_keep_publishing: false + #update_rate: #necessary for enable_keep_publishing is true diff --git a/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml new file mode 100644 index 0000000000000..f4c6fa43a8d61 --- /dev/null +++ b/system/autoware_topic_relay_controller/launch/topic_relay_controller.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/system/autoware_topic_relay_controller/package.xml b/system/autoware_topic_relay_controller/package.xml new file mode 100644 index 0000000000000..09cb55d3e250d --- /dev/null +++ b/system/autoware_topic_relay_controller/package.xml @@ -0,0 +1,25 @@ + + + + autoware_topic_relay_controller + 0.41.0 + The topic_relay_controller ROS 2 package + Makoto Kurihara + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + rclcpp + rclcpp_components + tf2_msgs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp new file mode 100644 index 0000000000000..88056c271cb40 --- /dev/null +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.cpp @@ -0,0 +1,129 @@ +// Copyright 2025 TIER 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 "topic_relay_controller_node.hpp" + +#include +#include + +namespace autoware::topic_relay_controller +{ +TopicRelayController::TopicRelayController(const rclcpp::NodeOptions & options) +: Node("topic_relay_controller", options), is_relaying_(true) +{ + // Parameter + node_param_.topic = declare_parameter("topic"); + node_param_.remap_topic = declare_parameter("remap_topic"); + node_param_.qos_depth = declare_parameter("qos_depth", 1); + node_param_.transient_local = declare_parameter("transient_local", false); + node_param_.best_effort = declare_parameter("best_effort", false); + node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static"); + node_param_.enable_relay_control = declare_parameter("enable_relay_control"); + if (node_param_.enable_relay_control) + node_param_.srv_name = declare_parameter("srv_name"); + node_param_.enable_keep_publishing = declare_parameter("enable_keep_publishing"); + if (node_param_.enable_keep_publishing) + node_param_.update_rate = declare_parameter("update_rate"); + + if (node_param_.is_transform) { + node_param_.frame_id = declare_parameter("frame_id"); + node_param_.child_frame_id = declare_parameter("child_frame_id"); + } else { + node_param_.topic_type = declare_parameter("topic_type"); + } + + // Service + if (node_param_.enable_relay_control) { + srv_change_relay_control_ = create_service( + node_param_.srv_name, + [this]( + const tier4_system_msgs::srv::ChangeTopicRelayControl::Request::SharedPtr request, + tier4_system_msgs::srv::ChangeTopicRelayControl::Response::SharedPtr response) { + is_relaying_ = request->relay_on; + RCLCPP_INFO(get_logger(), "relay control: %s", is_relaying_ ? "ON" : "OFF"); + response->status.success = true; + }); + } + + // Subscriber + rclcpp::QoS qos = rclcpp::SystemDefaultsQoS(); + if (node_param_.qos_depth > 0) { + size_t qos_depth = static_cast(node_param_.qos_depth); + qos.keep_last(qos_depth); + } else { + RCLCPP_ERROR(get_logger(), "qos_depth must be greater than 0"); + return; + } + + if (node_param_.transient_local) { + qos.transient_local(); + } + if (node_param_.best_effort) { + qos.best_effort(); + } + + if (node_param_.is_transform) { + // Publisher + pub_transform_ = this->create_publisher(node_param_.remap_topic, qos); + + sub_transform_ = this->create_subscription( + node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::SharedPtr msg) { + for (const auto & transform : msg->transforms) { + if ( + transform.header.frame_id != node_param_.frame_id || + transform.child_frame_id != node_param_.child_frame_id || !is_relaying_) + return; + + if (node_param_.enable_keep_publishing) { + last_tf_topic_ = msg; + } else { + pub_transform_->publish(*msg); + } + } + }); + } else { + // Publisher + pub_topic_ = + this->create_generic_publisher(node_param_.remap_topic, node_param_.topic_type, qos); + + sub_topic_ = this->create_generic_subscription( + node_param_.topic, node_param_.topic_type, qos, + [this]([[maybe_unused]] std::shared_ptr msg) { + if (!is_relaying_) return; + + if (node_param_.enable_keep_publishing) { + last_topic_ = msg; + } else { + pub_topic_->publish(*msg); + } + }); + } + + // Timer + if (node_param_.enable_keep_publishing) { + const auto update_period_ns = rclcpp::Rate(node_param_.update_rate).period(); + timer_ = rclcpp::create_timer(this, get_clock(), update_period_ns, [this]() { + if (!is_relaying_) return; + + if (node_param_.is_transform) { + if (last_tf_topic_) pub_transform_->publish(*last_tf_topic_); + } else { + if (last_topic_) pub_topic_->publish(*last_topic_); + } + }); + } +} +} // namespace autoware::topic_relay_controller + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_relay_controller::TopicRelayController) diff --git a/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp new file mode 100644 index 0000000000000..446b9f156456a --- /dev/null +++ b/system/autoware_topic_relay_controller/src/topic_relay_controller_node.hpp @@ -0,0 +1,77 @@ +// Copyright 2025 TIER 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 TOPIC_RELAY_CONTROLLER_NODE_HPP_ +#define TOPIC_RELAY_CONTROLLER_NODE_HPP_ + +// ROS 2 core +#include + +#include +#include + +#include +#include + +namespace autoware::topic_relay_controller +{ +struct NodeParam +{ + std::string topic; + std::string remap_topic; + std::string topic_type; + int qos_depth; + std::string frame_id; + std::string child_frame_id; + bool transient_local; + bool best_effort; + bool is_transform; + bool enable_relay_control; + std::string srv_name; + bool enable_keep_publishing; + int update_rate; +}; + +class TopicRelayController : public rclcpp::Node +{ +public: + explicit TopicRelayController(const rclcpp::NodeOptions & options); + +private: + // Parameter + NodeParam node_param_; + + // Subscriber + rclcpp::GenericSubscription::SharedPtr sub_topic_; + rclcpp::Subscription::SharedPtr sub_transform_; + + // Publisher + rclcpp::GenericPublisher::SharedPtr pub_topic_; + rclcpp::Publisher::SharedPtr pub_transform_; + + // Service + rclcpp::Service::SharedPtr + srv_change_relay_control_; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // State + bool is_relaying_; + tf2_msgs::msg::TFMessage::SharedPtr last_tf_topic_; + std::shared_ptr last_topic_; +}; +} // namespace autoware::topic_relay_controller + +#endif // TOPIC_RELAY_CONTROLLER_NODE_HPP_ diff --git a/system/topic_state_monitor/CHANGELOG.rst b/system/autoware_topic_state_monitor/CHANGELOG.rst similarity index 96% rename from system/topic_state_monitor/CHANGELOG.rst rename to system/autoware_topic_state_monitor/CHANGELOG.rst index 1eebcd1bcb1b1..2eb63f5be43e0 100644 --- a/system/topic_state_monitor/CHANGELOG.rst +++ b/system/autoware_topic_state_monitor/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package topic_state_monitor -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_topic_state_monitor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware` prefix for `component_state_monitor` and its dependencies (`#9961 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/system/autoware_topic_state_monitor/CMakeLists.txt b/system/autoware_topic_state_monitor/CMakeLists.txt new file mode 100644 index 0000000000000..822560a12e3d5 --- /dev/null +++ b/system/autoware_topic_state_monitor/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_topic_state_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/topic_state_monitor/topic_state_monitor.cpp + src/topic_state_monitor_core.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::topic_state_monitor::TopicStateMonitorNode" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/system/topic_state_monitor/README.md b/system/autoware_topic_state_monitor/README.md similarity index 99% rename from system/topic_state_monitor/README.md rename to system/autoware_topic_state_monitor/README.md index 28333305ef5a9..bebf096db08ac 100644 --- a/system/topic_state_monitor/README.md +++ b/system/autoware_topic_state_monitor/README.md @@ -1,4 +1,4 @@ -# topic_state_monitor +# autoware_topic_state_monitor ## Purpose diff --git a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp b/system/autoware_topic_state_monitor/include/autoware/topic_state_monitor/topic_state_monitor.hpp similarity index 84% rename from system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp rename to system/autoware_topic_state_monitor/include/autoware/topic_state_monitor/topic_state_monitor.hpp index 2057ca4c5c86c..e9a651fef3879 100644 --- a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp +++ b/system/autoware_topic_state_monitor/include/autoware/topic_state_monitor/topic_state_monitor.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ -#define TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ +#ifndef AUTOWARE__TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ +#define AUTOWARE__TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ #include #include #include -namespace topic_state_monitor +namespace autoware::topic_state_monitor { struct Param { @@ -68,6 +68,6 @@ class TopicStateMonitor bool isErrorRate() const; bool isTimeout() const; }; -} // namespace topic_state_monitor +} // namespace autoware::topic_state_monitor -#endif // TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ +#endif // AUTOWARE__TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_HPP_ diff --git a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp b/system/autoware_topic_state_monitor/include/autoware/topic_state_monitor/topic_state_monitor_core.hpp similarity index 83% rename from system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp rename to system/autoware_topic_state_monitor/include/autoware/topic_state_monitor/topic_state_monitor_core.hpp index 4696823ffbd4b..5d72d437f7b12 100644 --- a/system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp +++ b/system/autoware_topic_state_monitor/include/autoware/topic_state_monitor/topic_state_monitor_core.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ -#define TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ +#ifndef AUTOWARE__TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ +#define AUTOWARE__TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ -#include "topic_state_monitor/topic_state_monitor.hpp" +#include "autoware/topic_state_monitor/topic_state_monitor.hpp" #include #include @@ -28,7 +28,7 @@ #include #include -namespace topic_state_monitor +namespace autoware::topic_state_monitor { struct NodeParam { @@ -74,6 +74,6 @@ class TopicStateMonitorNode : public rclcpp::Node void checkTopicStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); }; -} // namespace topic_state_monitor +} // namespace autoware::topic_state_monitor -#endif // TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ +#endif // AUTOWARE__TOPIC_STATE_MONITOR__TOPIC_STATE_MONITOR_CORE_HPP_ diff --git a/system/topic_state_monitor/launch/topic_state_monitor.launch.xml b/system/autoware_topic_state_monitor/launch/topic_state_monitor.launch.xml similarity index 88% rename from system/topic_state_monitor/launch/topic_state_monitor.launch.xml rename to system/autoware_topic_state_monitor/launch/topic_state_monitor.launch.xml index 7304dc8c5a273..7df4b2b035944 100644 --- a/system/topic_state_monitor/launch/topic_state_monitor.launch.xml +++ b/system/autoware_topic_state_monitor/launch/topic_state_monitor.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml b/system/autoware_topic_state_monitor/launch/topic_state_monitor_tf.launch.xml similarity index 89% rename from system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml rename to system/autoware_topic_state_monitor/launch/topic_state_monitor_tf.launch.xml index 1a255a8a5859a..8a981d30969e4 100644 --- a/system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml +++ b/system/autoware_topic_state_monitor/launch/topic_state_monitor_tf.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/system/topic_state_monitor/package.xml b/system/autoware_topic_state_monitor/package.xml similarity index 77% rename from system/topic_state_monitor/package.xml rename to system/autoware_topic_state_monitor/package.xml index 0468db6124666..a932d01656ba1 100644 --- a/system/topic_state_monitor/package.xml +++ b/system/autoware_topic_state_monitor/package.xml @@ -1,10 +1,11 @@ - topic_state_monitor - 0.40.0 - The topic_state_monitor package + autoware_topic_state_monitor + 0.41.0 + The autoware_topic_state_monitor package Ryohsuke Mitsudome + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp b/system/autoware_topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp similarity index 94% rename from system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp rename to system/autoware_topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp index ff6806e96fac9..a688bfe501960 100644 --- a/system/topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp +++ b/system/autoware_topic_state_monitor/src/topic_state_monitor/topic_state_monitor.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "topic_state_monitor/topic_state_monitor.hpp" +#include "autoware/topic_state_monitor/topic_state_monitor.hpp" -namespace topic_state_monitor +namespace autoware::topic_state_monitor { TopicStateMonitor::TopicStateMonitor(rclcpp::Node & node) : clock_(node.get_clock()) { @@ -99,4 +99,4 @@ bool TopicStateMonitor::isTimeout() const return time_diff > param_.timeout; } -} // namespace topic_state_monitor +} // namespace autoware::topic_state_monitor diff --git a/system/topic_state_monitor/src/topic_state_monitor_core.cpp b/system/autoware_topic_state_monitor/src/topic_state_monitor_core.cpp similarity index 96% rename from system/topic_state_monitor/src/topic_state_monitor_core.cpp rename to system/autoware_topic_state_monitor/src/topic_state_monitor_core.cpp index ff58a0c6fd584..5947136b1cd57 100644 --- a/system/topic_state_monitor/src/topic_state_monitor_core.cpp +++ b/system/autoware_topic_state_monitor/src/topic_state_monitor_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "topic_state_monitor/topic_state_monitor_core.hpp" +#include "autoware/topic_state_monitor/topic_state_monitor_core.hpp" #include #include @@ -34,7 +34,7 @@ void update_param( } } // namespace -namespace topic_state_monitor +namespace autoware::topic_state_monitor { TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_options) : Node("topic_state_monitor", node_options), updater_(this) @@ -205,7 +205,7 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu stat.summary(level, msg); } -} // namespace topic_state_monitor +} // namespace autoware::topic_state_monitor #include -RCLCPP_COMPONENTS_REGISTER_NODE(topic_state_monitor::TopicStateMonitorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::topic_state_monitor::TopicStateMonitorNode) diff --git a/system/velodyne_monitor/CHANGELOG.rst b/system/autoware_velodyne_monitor/CHANGELOG.rst similarity index 95% rename from system/velodyne_monitor/CHANGELOG.rst rename to system/autoware_velodyne_monitor/CHANGELOG.rst index 4cc3be4c7a3f0..87a23f9e4b660 100644 --- a/system/velodyne_monitor/CHANGELOG.rst +++ b/system/autoware_velodyne_monitor/CHANGELOG.rst @@ -1,6 +1,12 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package velodyne_monitor -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_velodyne_monitor +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `velodyne_monitor` (`#9976 `_) +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/system/velodyne_monitor/CMakeLists.txt b/system/autoware_velodyne_monitor/CMakeLists.txt similarity index 83% rename from system/velodyne_monitor/CMakeLists.txt rename to system/autoware_velodyne_monitor/CMakeLists.txt index a09e1fe1bf012..ab2fe0aa5f56d 100644 --- a/system/velodyne_monitor/CMakeLists.txt +++ b/system/autoware_velodyne_monitor/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(velodyne_monitor) +project(autoware_velodyne_monitor) find_package(autoware_cmake REQUIRED) find_package(fmt) @@ -13,7 +13,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED target_link_libraries(${PROJECT_NAME} cpprest crypto fmt) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "VelodyneMonitor" + PLUGIN "autoware::velodyne_monitor::VelodyneMonitor" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/system/velodyne_monitor/README.md b/system/autoware_velodyne_monitor/README.md similarity index 99% rename from system/velodyne_monitor/README.md rename to system/autoware_velodyne_monitor/README.md index f8381dbd41159..9d45f3d6a501f 100644 --- a/system/velodyne_monitor/README.md +++ b/system/autoware_velodyne_monitor/README.md @@ -1,4 +1,4 @@ -# velodyne_monitor +# autoware_velodyne_monitor ## Purpose diff --git a/system/velodyne_monitor/config/HDL-32E.param.yaml b/system/autoware_velodyne_monitor/config/HDL-32E.param.yaml similarity index 100% rename from system/velodyne_monitor/config/HDL-32E.param.yaml rename to system/autoware_velodyne_monitor/config/HDL-32E.param.yaml diff --git a/system/velodyne_monitor/config/VLP-16.param.yaml b/system/autoware_velodyne_monitor/config/VLP-16.param.yaml similarity index 100% rename from system/velodyne_monitor/config/VLP-16.param.yaml rename to system/autoware_velodyne_monitor/config/VLP-16.param.yaml diff --git a/system/velodyne_monitor/config/VLP-32C.param.yaml b/system/autoware_velodyne_monitor/config/VLP-32C.param.yaml similarity index 100% rename from system/velodyne_monitor/config/VLP-32C.param.yaml rename to system/autoware_velodyne_monitor/config/VLP-32C.param.yaml diff --git a/system/velodyne_monitor/config/VLS-128.param.yaml b/system/autoware_velodyne_monitor/config/VLS-128.param.yaml similarity index 100% rename from system/velodyne_monitor/config/VLS-128.param.yaml rename to system/autoware_velodyne_monitor/config/VLS-128.param.yaml diff --git a/system/velodyne_monitor/config/Velarray_M1600.param.yaml b/system/autoware_velodyne_monitor/config/Velarray_M1600.param.yaml similarity index 100% rename from system/velodyne_monitor/config/Velarray_M1600.param.yaml rename to system/autoware_velodyne_monitor/config/Velarray_M1600.param.yaml diff --git a/system/velodyne_monitor/include/velodyne_monitor/velodyne_monitor.hpp b/system/autoware_velodyne_monitor/include/autoware/velodyne_monitor/velodyne_monitor.hpp similarity index 94% rename from system/velodyne_monitor/include/velodyne_monitor/velodyne_monitor.hpp rename to system/autoware_velodyne_monitor/include/autoware/velodyne_monitor/velodyne_monitor.hpp index 841199a845bbf..092dcb24d7ae9 100644 --- a/system/velodyne_monitor/include/velodyne_monitor/velodyne_monitor.hpp +++ b/system/autoware_velodyne_monitor/include/autoware/velodyne_monitor/velodyne_monitor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ -#define VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ +#ifndef AUTOWARE__VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ +#define AUTOWARE__VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ /** * @file velodyne_monitor.hpp @@ -31,6 +31,9 @@ // Include after diagnostic_updater because it causes errors #include +namespace autoware::velodyne_monitor +{ + namespace http = web::http; namespace client = web::http::client; namespace json = web::json; @@ -115,4 +118,6 @@ class VelodyneMonitor : public rclcpp::Node {DiagStatus::OK, "OK"}, {DiagStatus::WARN, "RPM low"}, {DiagStatus::ERROR, "RPM too low"}}; }; -#endif // VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ +} // namespace autoware::velodyne_monitor + +#endif // AUTOWARE__VELODYNE_MONITOR__VELODYNE_MONITOR_HPP_ diff --git a/system/velodyne_monitor/launch/velodyne_monitor.launch.xml b/system/autoware_velodyne_monitor/launch/velodyne_monitor.launch.xml similarity index 57% rename from system/velodyne_monitor/launch/velodyne_monitor.launch.xml rename to system/autoware_velodyne_monitor/launch/velodyne_monitor.launch.xml index 0dbbc39a9f9e4..3d72d4f1f077e 100644 --- a/system/velodyne_monitor/launch/velodyne_monitor.launch.xml +++ b/system/autoware_velodyne_monitor/launch/velodyne_monitor.launch.xml @@ -1,8 +1,8 @@ - + - + diff --git a/system/velodyne_monitor/package.xml b/system/autoware_velodyne_monitor/package.xml similarity index 79% rename from system/velodyne_monitor/package.xml rename to system/autoware_velodyne_monitor/package.xml index 0dc8ae5f00a5d..daf843c09ef14 100644 --- a/system/velodyne_monitor/package.xml +++ b/system/autoware_velodyne_monitor/package.xml @@ -1,10 +1,11 @@ - velodyne_monitor - 0.40.0 - The velodyne_monitor package + autoware_velodyne_monitor + 0.41.0 + The autoware_velodyne_monitor package Fumihito Ito + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/system/velodyne_monitor/schema/velodyne_monitor.json b/system/autoware_velodyne_monitor/schema/velodyne_monitor.json similarity index 100% rename from system/velodyne_monitor/schema/velodyne_monitor.json rename to system/autoware_velodyne_monitor/schema/velodyne_monitor.json diff --git a/system/velodyne_monitor/src/velodyne_monitor.cpp b/system/autoware_velodyne_monitor/src/velodyne_monitor.cpp similarity index 97% rename from system/velodyne_monitor/src/velodyne_monitor.cpp rename to system/autoware_velodyne_monitor/src/velodyne_monitor.cpp index f9ab3ea14b4c0..83429229b7c4c 100644 --- a/system/velodyne_monitor/src/velodyne_monitor.cpp +++ b/system/autoware_velodyne_monitor/src/velodyne_monitor.cpp @@ -17,7 +17,7 @@ * @brief Velodyne monitor class */ -#include "velodyne_monitor/velodyne_monitor.hpp" +#include "autoware/velodyne_monitor/velodyne_monitor.hpp" #include @@ -30,6 +30,9 @@ #define FMT_HEADER_ONLY #include +namespace autoware::velodyne_monitor +{ + VelodyneMonitor::VelodyneMonitor(const rclcpp::NodeOptions & options) : Node("velodyne_monitor", options), updater_(this) { @@ -231,5 +234,7 @@ float VelodyneMonitor::convertTemperature(int raw) return std::sqrt(2.1962e6 + (1.8639 - static_cast(raw) * 5.0 / 4096) / 3.88e-6) - 1481.96; } +} // namespace autoware::velodyne_monitor + #include -RCLCPP_COMPONENTS_REGISTER_NODE(VelodyneMonitor) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::velodyne_monitor::VelodyneMonitor) diff --git a/system/bluetooth_monitor/launch/bluetooth_monitor.launch.xml b/system/bluetooth_monitor/launch/bluetooth_monitor.launch.xml deleted file mode 100644 index 7dd98572e544b..0000000000000 --- a/system/bluetooth_monitor/launch/bluetooth_monitor.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml b/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml deleted file mode 100644 index 26fb4720ca435..0000000000000 --- a/system/default_ad_api_helpers/ad_api_visualizers/launch/ad_api_visualizers.launch.xml +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg b/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg deleted file mode 100644 index b0af17360079d..0000000000000 --- a/system/default_ad_api_helpers/ad_api_visualizers/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/ad_api_visualizers -[install] -install_scripts=$base/lib/ad_api_visualizers diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml b/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml deleted file mode 100644 index e9a94efd6be7b..0000000000000 --- a/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/system/diagnostic_graph_aggregator/example/example-edit.launch.xml b/system/diagnostic_graph_aggregator/example/example-edit.launch.xml deleted file mode 100644 index d56f76b1726cf..0000000000000 --- a/system/diagnostic_graph_aggregator/example/example-edit.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/system/diagnostic_graph_aggregator/example/example-main.launch.xml b/system/diagnostic_graph_aggregator/example/example-main.launch.xml deleted file mode 100644 index b7019640e993c..0000000000000 --- a/system/diagnostic_graph_aggregator/example/example-main.launch.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/system/diagnostic_graph_aggregator/launch/aggregator.launch.xml b/system/diagnostic_graph_aggregator/launch/aggregator.launch.xml deleted file mode 100644 index c06c3d1d96cfa..0000000000000 --- a/system/diagnostic_graph_aggregator/launch/aggregator.launch.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - diff --git a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml b/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml deleted file mode 100644 index 9d9193fb5f7a7..0000000000000 --- a/system/dummy_diag_publisher/launch/dummy_diag_publisher_node.launch.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/system/dummy_infrastructure/CMakeLists.txt b/system/dummy_infrastructure/CMakeLists.txt deleted file mode 100644 index 5d3d39d02365d..0000000000000 --- a/system/dummy_infrastructure/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(dummy_infrastructure) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(dummy_infrastructure_node_component SHARED - src/dummy_infrastructure_node/dummy_infrastructure_node.cpp -) - -rclcpp_components_register_node(dummy_infrastructure_node_component - PLUGIN "dummy_infrastructure::DummyInfrastructureNode" - EXECUTABLE dummy_infrastructure_node -) - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml b/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml deleted file mode 100644 index 87f41f045545d..0000000000000 --- a/system/duplicated_node_checker/launch/duplicated_node_checker.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/system/mrm_comfortable_stop_operator/CMakeLists.txt b/system/mrm_comfortable_stop_operator/CMakeLists.txt deleted file mode 100644 index 19bc4f6d66d5f..0000000000000 --- a/system/mrm_comfortable_stop_operator/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(mrm_comfortable_stop_operator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(mrm_comfortable_stop_operator_component SHARED - src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.cpp -) - -rclcpp_components_register_node(mrm_comfortable_stop_operator_component - PLUGIN "mrm_comfortable_stop_operator::MrmComfortableStopOperator" - EXECUTABLE mrm_comfortable_stop_operator) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/system/mrm_emergency_stop_operator/CMakeLists.txt b/system/mrm_emergency_stop_operator/CMakeLists.txt deleted file mode 100644 index 77d300f5c370b..0000000000000 --- a/system/mrm_emergency_stop_operator/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(mrm_emergency_stop_operator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(mrm_emergency_stop_operator_component SHARED - src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp -) - -rclcpp_components_register_node(mrm_emergency_stop_operator_component - PLUGIN "mrm_emergency_stop_operator::MrmEmergencyStopOperator" - EXECUTABLE mrm_emergency_stop_operator) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml deleted file mode 100644 index 8e2566a747abf..0000000000000 --- a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/system/topic_state_monitor/CMakeLists.txt b/system/topic_state_monitor/CMakeLists.txt deleted file mode 100644 index a4a040c449cdb..0000000000000 --- a/system/topic_state_monitor/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(topic_state_monitor) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(topic_state_monitor SHARED - src/topic_state_monitor/topic_state_monitor.cpp - src/topic_state_monitor_core.cpp -) - -rclcpp_components_register_node(topic_state_monitor - PLUGIN "topic_state_monitor::TopicStateMonitorNode" - EXECUTABLE topic_state_monitor_node -) - -ament_auto_package(INSTALL_TO_SHARE - launch -) diff --git a/tools/reaction_analyzer/CHANGELOG.rst b/tools/reaction_analyzer/CHANGELOG.rst index f2f1ac255fc06..b1bd59f3f33de 100644 --- a/tools/reaction_analyzer/CHANGELOG.rst +++ b/tools/reaction_analyzer/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package reaction_analyzer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml index 540376e032743..9d1928813f07e 100644 --- a/tools/reaction_analyzer/package.xml +++ b/tools/reaction_analyzer/package.xml @@ -2,7 +2,7 @@ reaction_analyzer - 0.40.0 + 0.41.0 Analyzer that measures reaction times of the nodes Berkay Karaman diff --git a/vehicle/autoware_accel_brake_map_calibrator/CHANGELOG.rst b/vehicle/autoware_accel_brake_map_calibrator/CHANGELOG.rst index cbd3907bd854a..3936c60ca160b 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/CHANGELOG.rst +++ b/vehicle/autoware_accel_brake_map_calibrator/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package autoware_accel_brake_map_calibrator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_accel_brake_map_calibrator)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_accel_brake_map_calibrator (`#9923 `_) + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 2a0ad4f987dad..cd05bed5ce476 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/autoware_accel_brake_map_calibrator/include/autoware_accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -28,6 +28,8 @@ #include +#include "autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -37,8 +39,6 @@ #include "std_msgs/msg/multi_array_dimension.hpp" #include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" #include "tier4_external_api_msgs/msg/calibration_status.hpp" #include "tier4_external_api_msgs/msg/calibration_status_array.hpp" #include "tier4_external_api_msgs/srv/get_accel_brake_map_calibration_data.hpp" @@ -57,6 +57,8 @@ namespace autoware::accel_brake_map_calibrator { +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32Stamped; using autoware_vehicle_msgs::msg::SteeringReport; using autoware_vehicle_msgs::msg::VelocityReport; using geometry_msgs::msg::TwistStamped; @@ -64,8 +66,6 @@ using nav_msgs::msg::OccupancyGrid; using raw_vehicle_cmd_converter::AccelMap; using raw_vehicle_cmd_converter::BrakeMap; using std_msgs::msg::Float32MultiArray; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_debug_msgs::msg::Float32Stamped; using tier4_external_api_msgs::msg::CalibrationStatus; using tier4_vehicle_msgs::msg::ActuationCommandStamped; using tier4_vehicle_msgs::msg::ActuationStatusStamped; diff --git a/vehicle/autoware_accel_brake_map_calibrator/package.xml b/vehicle/autoware_accel_brake_map_calibrator/package.xml index de2dcf2d8e440..78c3b518057de 100644 --- a/vehicle/autoware_accel_brake_map_calibrator/package.xml +++ b/vehicle/autoware_accel_brake_map_calibrator/package.xml @@ -2,7 +2,7 @@ autoware_accel_brake_map_calibrator - 0.40.0 + 0.41.0 The accel_brake_map_calibrator Tomoya Kimura Taiki Tanaka @@ -14,6 +14,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_motion_utils autoware_raw_vehicle_cmd_converter autoware_universe_utils @@ -25,7 +26,6 @@ std_srvs tf2_geometry_msgs tf2_ros - tier4_debug_msgs tier4_external_api_msgs tier4_vehicle_msgs visualization_msgs diff --git a/vehicle/autoware_accel_brake_map_calibrator/scripts/accel_tester.py b/vehicle/autoware_accel_brake_map_calibrator/scripts/accel_tester.py index f73718f7ea7bc..8aef30f9ee251 100755 --- a/vehicle/autoware_accel_brake_map_calibrator/scripts/accel_tester.py +++ b/vehicle/autoware_accel_brake_map_calibrator/scripts/accel_tester.py @@ -15,9 +15,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_internal_debug_msgs.msg import Float32Stamped import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped MAX_ACCEL = 1.0 # [-] MIN_ACCEL = 0.0 # [-] diff --git a/vehicle/autoware_accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py b/vehicle/autoware_accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py index 3a029e193f41f..334b5652dc8b6 100755 --- a/vehicle/autoware_accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py +++ b/vehicle/autoware_accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py @@ -15,10 +15,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_internal_debug_msgs.msg import Float32Stamped from autoware_vehicle_msgs.msg import GearCommand import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped from tier4_vehicle_msgs.msg import ActuationCommandStamped diff --git a/vehicle/autoware_accel_brake_map_calibrator/scripts/brake_tester.py b/vehicle/autoware_accel_brake_map_calibrator/scripts/brake_tester.py index 050f232334afd..fe067494a0c39 100755 --- a/vehicle/autoware_accel_brake_map_calibrator/scripts/brake_tester.py +++ b/vehicle/autoware_accel_brake_map_calibrator/scripts/brake_tester.py @@ -16,9 +16,9 @@ # limitations under the License. +from autoware_internal_debug_msgs.msg import Float32Stamped import rclpy from rclpy.node import Node -from tier4_debug_msgs.msg import Float32Stamped MAX_BRAKE = 1.0 # [-] MIN_BRAKE = 0.0 # [-] diff --git a/vehicle/autoware_external_cmd_converter/CHANGELOG.rst b/vehicle/autoware_external_cmd_converter/CHANGELOG.rst index ddbd655a9c34f..dbb5e1d429613 100644 --- a/vehicle/autoware_external_cmd_converter/CHANGELOG.rst +++ b/vehicle/autoware_external_cmd_converter/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_external_cmd_converter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/vehicle/autoware_external_cmd_converter/package.xml b/vehicle/autoware_external_cmd_converter/package.xml index eb44058775a3f..b28b9b6f3dd24 100644 --- a/vehicle/autoware_external_cmd_converter/package.xml +++ b/vehicle/autoware_external_cmd_converter/package.xml @@ -2,7 +2,7 @@ autoware_external_cmd_converter - 0.40.0 + 0.41.0 The autoware_external_cmd_converter package Takamasa Horibe Eiki Nagata diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/CHANGELOG.rst b/vehicle/autoware_raw_vehicle_cmd_converter/CHANGELOG.rst index de088bb8d5a76..630698c89bbbe 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/CHANGELOG.rst +++ b/vehicle/autoware_raw_vehicle_cmd_converter/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package autoware_raw_vehicle_cmd_converter ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_raw_vehicle_cmd_converter)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_raw_vehicle_cmd_converter (`#9924 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files vehicle/autoware_raw_vehicle_cmd_converter + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* fix(raw_veihicle_converter): fix too long line (`#9716 `_) +* feat(raw_vehicle_cmd_converter): add vehicle adaptor (`#8782 `_) + * feat(raw_vehicle_cmd_converter): add vehicle adaptor + sub operation status + * feat(raw_vehicle_cmd_converter): publish vehicle adaptor output + * use control horizon + * revert carla + * update docs + --------- +* Contributors: Fumiya Watanabe, Kosuke Takeuchi, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt index 9e5b7439e1de2..0d5b6734418c4 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt +++ b/vehicle/autoware_raw_vehicle_cmd_converter/CMakeLists.txt @@ -13,11 +13,18 @@ ament_auto_add_library(actuation_map_converter SHARED src/vgr.cpp ) +ament_auto_add_library(vehicle_adaptor SHARED + src/vehicle_adaptor/vehicle_adaptor.cpp +) + ament_auto_add_library(autoware_raw_vehicle_cmd_converter_node_component SHARED src/node.cpp ) -target_link_libraries(autoware_raw_vehicle_cmd_converter_node_component actuation_map_converter) +target_link_libraries(autoware_raw_vehicle_cmd_converter_node_component + actuation_map_converter + vehicle_adaptor +) rclcpp_components_register_node(autoware_raw_vehicle_cmd_converter_node_component PLUGIN "autoware::raw_vehicle_cmd_converter::RawVehicleCommandConverterNode" @@ -30,7 +37,11 @@ if(BUILD_TESTING) ) set(TEST_RAW_VEHICLE_CMD_CONVERTER_EXE test_autoware_raw_vehicle_cmd_converter) ament_add_ros_isolated_gtest(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} ${TEST_SOURCES}) - target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} actuation_map_converter autoware_raw_vehicle_cmd_converter_node_component) + target_link_libraries(${TEST_RAW_VEHICLE_CMD_CONVERTER_EXE} + actuation_map_converter + vehicle_adaptor + autoware_raw_vehicle_cmd_converter_node_component + ) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/README.md b/vehicle/autoware_raw_vehicle_cmd_converter/README.md index 1df083f5c5370..a4d72a684e7d3 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/README.md +++ b/vehicle/autoware_raw_vehicle_cmd_converter/README.md @@ -45,6 +45,12 @@ vgr_coef_c: 0.042 When `convert_steer_cmd_method: "vgr"` is selected, the node receives the control command from the controller as the desired tire angle and calculates the desired steering angle to output. Also, when `convert_actuation_to_steering_status: true`, this node receives the `actuation_status` topic and calculates the steer tire angle from the `steer_wheel_angle` and publishes it. +### Vehicle Adaptor + +**Under development** +A feature that compensates for control commands according to the dynamic characteristics of the vehicle. +This feature works when `use_vehicle_adaptor: true` is set and requires `control_horizon` to be enabled, so you need to set `enable_control_cmd_horizon_pub: true` in the trajectory_follower node. + ## Input topics | Name | Type | Description | @@ -54,6 +60,14 @@ Also, when `convert_actuation_to_steering_status: true`, this node receives the | `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. | | `~/input/actuation_status` | tier4_vehicle_msgs::msg::ActuationStatus | actuation status is assumed to receive the same type of status as sent to the vehicle side. For example, if throttle/brake pedal/steer_wheel_angle is sent, the same type of status is received. In the case of steer_wheel_angle, it is used to calculate steer_tire_angle and VGR in this node. | +Input topics when vehicle_adaptor is enabled + +| Name | Type | Description | +| ------------------------------ | ----------------------------------------------- | ----------------------- | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped; | acceleration status | +| `~/input/operation_mode_state` | autoware_adapi_v1_msgs::msg::OperationModeState | operation mode status | +| `~/input/control_horizon` | autoware_control_msgs::msg::ControlHorizon | control horizon command | + ## Output topics | Name | Type | Description | diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp index b5e13985c036e..eaf39cf729971 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/node.hpp @@ -21,15 +21,17 @@ #include "autoware_raw_vehicle_cmd_converter/brake_map.hpp" #include "autoware_raw_vehicle_cmd_converter/pid.hpp" #include "autoware_raw_vehicle_cmd_converter/steer_map.hpp" +#include "autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp" #include "autoware_raw_vehicle_cmd_converter/vgr.hpp" #include +#include #include +#include #include #include #include -#include #include #include @@ -40,13 +42,14 @@ namespace autoware::raw_vehicle_cmd_converter { using Control = autoware_control_msgs::msg::Control; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using autoware_internal_debug_msgs::msg::Float32MultiArrayStamped; using tier4_vehicle_msgs::msg::ActuationCommandStamped; using tier4_vehicle_msgs::msg::ActuationStatusStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; using Odometry = nav_msgs::msg::Odometry; using Steering = autoware_vehicle_msgs::msg::SteeringReport; - +using autoware_adapi_v1_msgs::msg::OperationModeState; +using geometry_msgs::msg::AccelWithCovarianceStamped; class DebugValues { public: @@ -79,6 +82,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief topic publisher for low level vehicle command rclcpp::Publisher::SharedPtr pub_actuation_cmd_; rclcpp::Publisher::SharedPtr pub_steering_status_; + rclcpp::Publisher::SharedPtr pub_compensated_control_cmd_; //!< @brief subscriber for vehicle command rclcpp::Subscription::SharedPtr sub_control_cmd_; rclcpp::Subscription::SharedPtr sub_actuation_status_; @@ -86,17 +90,27 @@ class RawVehicleCommandConverterNode : public rclcpp::Node // polling subscribers autoware::universe_utils::InterProcessPollingSubscriber sub_odometry_{ this, "~/input/odometry"}; + // polling subscribers for vehicle_adaptor + autoware::universe_utils::InterProcessPollingSubscriber sub_accel_{ + this, "~/input/accel"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/operation_mode_state"}; + // control_horizon is an experimental topic, but vehicle_adaptor uses it to improve performance, + autoware::universe_utils::InterProcessPollingSubscriber sub_control_horizon_{ + this, "~/input/control_horizon"}; rclcpp::TimerBase::SharedPtr timer_; std::unique_ptr current_twist_ptr_; // [m/s] std::unique_ptr current_steer_ptr_; ActuationStatusStamped::ConstSharedPtr actuation_status_ptr_; + Odometry::ConstSharedPtr current_odometry_; Control::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; SteerMap steer_map_; VGR vgr_; + VehicleAdaptor vehicle_adaptor_; // TODO(tanaka): consider accel/brake pid too PIDController steer_pid_; bool ff_map_initialized_; @@ -112,6 +126,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node bool convert_brake_cmd_; //!< @brief use brake or not std::optional convert_steer_cmd_method_{std::nullopt}; //!< @brief method to convert bool need_to_subscribe_actuation_status_{false}; + bool use_vehicle_adaptor_{false}; rclcpp::Time prev_time_steer_calculation_{0, 0, RCL_ROS_TIME}; // Whether to subscribe to actuation_status and calculate and publish steering_status diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp new file mode 100644 index 0000000000000..595cddd29817f --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/include/autoware_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 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_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_ +#define AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace autoware::raw_vehicle_cmd_converter +{ + +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_control_msgs::msg::Control; +using autoware_control_msgs::msg::ControlHorizon; +using autoware_vehicle_msgs::msg::SteeringReport; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using nav_msgs::msg::Odometry; + +class VehicleAdaptor +{ +public: + VehicleAdaptor() = default; + Control compensate( + const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry, + [[maybe_unused]] const AccelWithCovarianceStamped & accel, + [[maybe_unused]] const double steering, + [[maybe_unused]] const OperationModeState & operation_mode, + [[maybe_unused]] const ControlHorizon & control_horizon); + +private: +}; +} // namespace autoware::raw_vehicle_cmd_converter + +#endif // AUTOWARE_RAW_VEHICLE_CMD_CONVERTER__VEHICLE_ADAPTOR__VEHICLE_ADAPTOR_HPP_ diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml b/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml index 441d924a0e6c3..dc96533c505cd 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/launch/raw_vehicle_converter.launch.xml @@ -2,8 +2,11 @@ + + + @@ -13,8 +16,11 @@ + + + diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml index 468e039312520..2a200ea92967d 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/package.xml +++ b/vehicle/autoware_raw_vehicle_cmd_converter/package.xml @@ -2,7 +2,7 @@ autoware_raw_vehicle_cmd_converter - 0.40.0 + 0.41.0 The autoware_raw_vehicle_cmd_converter package Takamasa Horibe Tanaka Taiki @@ -21,14 +21,15 @@ ament_cmake_auto autoware_cmake + autoware_adapi_v1_msgs autoware_control_msgs + autoware_internal_debug_msgs autoware_interpolation autoware_vehicle_msgs geometry_msgs nav_msgs rclcpp rclcpp_components - tier4_debug_msgs tier4_vehicle_msgs rclpy diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json b/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json index 30642663a39f5..84bc78e1e7ace 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json +++ b/vehicle/autoware_raw_vehicle_cmd_converter/schema/raw_vehicle_cmd_converter.schema.json @@ -180,6 +180,11 @@ "type": "boolean", "default": "true", "description": "convert actuation to steering status or not. Whether to subscribe to actuation_status and calculate and publish steering_status For example, receive the steering wheel angle and calculate the steering wheel angle based on the gear ratio. If false, the vehicle interface must publish steering_status." + }, + "use_vehicle_adaptor": { + "type": "boolean", + "default": "false", + "description": "flag to enable feature that compensates control commands according to vehicle dynamics." } }, "required": [ diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp index e4cdbe60fd4cd..8d05e4faa69ad 100644 --- a/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/node.cpp @@ -40,6 +40,8 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( // for steering steer controller use_steer_ff_ = declare_parameter("use_steer_ff"); use_steer_fb_ = declare_parameter("use_steer_fb"); + use_vehicle_adaptor_ = declare_parameter("use_vehicle_adaptor", false); + if (convert_accel_cmd_) { if (!accel_map_.readAccelMapFromCSV(csv_path_accel_map, true)) { throw std::invalid_argument("Accel map is invalid."); @@ -116,15 +118,21 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( debug_pub_steer_pid_ = create_publisher( "/vehicle/raw_vehicle_cmd_converter/debug/steer_pid", 1); + if (use_vehicle_adaptor_) { + pub_compensated_control_cmd_ = create_publisher( + "/vehicle/raw_vehicle_cmd_converter/debug/compensated_control_cmd", 1); + } + logger_configure_ = std::make_unique(this); } void RawVehicleCommandConverterNode::publishActuationCmd() { - if (!current_twist_ptr_ || !control_cmd_ptr_ || !current_steer_ptr_) { + /* check if all necessary data is available */ + if (!current_odometry_ || !control_cmd_ptr_ || !current_steer_ptr_) { RCLCPP_WARN_EXPRESSION( get_logger(), is_debugging_, "some pointers are null: %s, %s, %s", - !current_twist_ptr_ ? "twist" : "", !control_cmd_ptr_ ? "cmd" : "", + !current_odometry_ ? "odometry" : "", !control_cmd_ptr_ ? "cmd" : "", !current_steer_ptr_ ? "steer" : ""); return; } @@ -136,14 +144,34 @@ void RawVehicleCommandConverterNode::publishActuationCmd() } } + /* compensate control command if vehicle adaptor is enabled */ + Control control_cmd = *control_cmd_ptr_; + if (use_vehicle_adaptor_) { + const auto current_accel = sub_accel_.takeData(); + const auto current_operation_mode = sub_operation_mode_.takeData(); + const auto control_horizon = sub_control_horizon_.takeData(); + if (!current_accel || !current_operation_mode || !control_horizon) { + RCLCPP_WARN_EXPRESSION( + get_logger(), is_debugging_, "some pointers are null: %s, %s %s", + !current_accel ? "accel" : "", !current_operation_mode ? "operation_mode" : "", + !control_horizon ? "control_horizon" : ""); + return; + } + control_cmd = vehicle_adaptor_.compensate( + *control_cmd_ptr_, *current_odometry_, *current_accel, *current_steer_ptr_, + *current_operation_mode, *control_horizon); + pub_compensated_control_cmd_->publish(control_cmd); + } + + /* calculate actuation command */ double desired_accel_cmd = 0.0; double desired_brake_cmd = 0.0; double desired_steer_cmd = 0.0; ActuationCommandStamped actuation_cmd; - const double acc = control_cmd_ptr_->longitudinal.acceleration; - const double vel = current_twist_ptr_->twist.linear.x; - const double steer = control_cmd_ptr_->lateral.steering_tire_angle; - const double steer_rate = control_cmd_ptr_->lateral.steering_tire_rotation_rate; + const double acc = control_cmd.longitudinal.acceleration; + const double vel = current_odometry_->twist.twist.linear.x; + const double steer = control_cmd.lateral.steering_tire_angle; + const double steer_rate = control_cmd.lateral.steering_tire_rotation_rate; bool accel_cmd_is_zero = true; if (convert_accel_cmd_) { desired_accel_cmd = calculateAccelMap(vel, acc, accel_cmd_is_zero); @@ -173,7 +201,7 @@ void RawVehicleCommandConverterNode::publishActuationCmd() desired_steer_cmd = calculateSteerFromMap(vel, steer, steer_rate); } actuation_cmd.header.frame_id = "base_link"; - actuation_cmd.header.stamp = control_cmd_ptr_->stamp; + actuation_cmd.header.stamp = control_cmd.stamp; actuation_cmd.actuation.accel_cmd = desired_accel_cmd; actuation_cmd.actuation.brake_cmd = desired_brake_cmd; actuation_cmd.actuation.steer_cmd = desired_steer_cmd; @@ -217,7 +245,7 @@ double RawVehicleCommandConverterNode::calculateSteerFromMap( debug_steer_.setValues(DebugValues::TYPE::ERROR_P, pid_errors.at(0)); debug_steer_.setValues(DebugValues::TYPE::ERROR_I, pid_errors.at(1)); debug_steer_.setValues(DebugValues::TYPE::ERROR_D, pid_errors.at(2)); - tier4_debug_msgs::msg::Float32MultiArrayStamped msg{}; + autoware_internal_debug_msgs::msg::Float32MultiArrayStamped msg{}; for (const auto & v : debug_steer_.getValues()) { msg.data.push_back(v); } @@ -252,12 +280,7 @@ double RawVehicleCommandConverterNode::calculateBrakeMap( void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { - const auto odometry_msg = sub_odometry_.takeData(); - if (odometry_msg) { - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = odometry_msg->header; - current_twist_ptr_->twist = odometry_msg->twist.twist; - } + current_odometry_ = sub_odometry_.takeData(); control_cmd_ptr_ = msg; publishActuationCmd(); } @@ -277,14 +300,11 @@ void RawVehicleCommandConverterNode::onActuationStatus( } // calculate steering status from actuation status - const auto odometry_msg = sub_odometry_.takeData(); - if (odometry_msg) { + current_odometry_ = sub_odometry_.takeData(); + if (current_odometry_) { if (convert_steer_cmd_method_.value() == "vgr") { - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = odometry_msg->header; - current_twist_ptr_->twist = odometry_msg->twist.twist; current_steer_ptr_ = std::make_unique(vgr_.calculateSteeringTireState( - current_twist_ptr_->twist.linear.x, actuation_status_ptr_->status.steer_status)); + current_odometry_->twist.twist.linear.x, actuation_status_ptr_->status.steer_status)); Steering steering_msg{}; steering_msg.steering_tire_angle = *current_steer_ptr_; pub_steering_status_->publish(steering_msg); diff --git a/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp b/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp new file mode 100644 index 0000000000000..7bc9076ae944b --- /dev/null +++ b/vehicle/autoware_raw_vehicle_cmd_converter/src/vehicle_adaptor/vehicle_adaptor.cpp @@ -0,0 +1,32 @@ +// Copyright 2024 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_raw_vehicle_cmd_converter/vehicle_adaptor/vehicle_adaptor.hpp" + +#include + +namespace autoware::raw_vehicle_cmd_converter +{ +Control VehicleAdaptor::compensate( + const Control & input_control_cmd, [[maybe_unused]] const Odometry & odometry, + [[maybe_unused]] const AccelWithCovarianceStamped & accel, [[maybe_unused]] const double steering, + [[maybe_unused]] const OperationModeState & operation_mode, + [[maybe_unused]] const ControlHorizon & control_horizon) +{ + // TODO(someone): implement the control command compensation + Control output_control_cmd = input_control_cmd; + std::cerr << "vehicle adaptor: compensate control command" << std::endl; + return output_control_cmd; +} +} // namespace autoware::raw_vehicle_cmd_converter diff --git a/vehicle/autoware_steer_offset_estimator/CHANGELOG.rst b/vehicle/autoware_steer_offset_estimator/CHANGELOG.rst index 4e4f9c1a39a66..b2e8e642f936a 100644 --- a/vehicle/autoware_steer_offset_estimator/CHANGELOG.rst +++ b/vehicle/autoware_steer_offset_estimator/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package autoware_steer_offset_estimator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(autoware_steer_offset_estimator)!: tier4_debug_msgs changed to autoware_internal_debug_msgs in autoware_steer_offset_estimator (`#9926 `_) + feat: tier4_debug_msgs changed to autoware_internal_debug_msgs in files vehicle/autoware_steer_offset_estimator + Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> +* Contributors: Fumiya Watanabe, Vishal Chauhan + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/vehicle/autoware_steer_offset_estimator/Readme.md b/vehicle/autoware_steer_offset_estimator/Readme.md index 2cd6bd2aabe2a..a7e9b6722a648 100644 --- a/vehicle/autoware_steer_offset_estimator/Readme.md +++ b/vehicle/autoware_steer_offset_estimator/Readme.md @@ -24,10 +24,10 @@ Calculate yaw rate error and then calculate steering error recursively by least ### Output -| Name | Type | Description | -| ------------------------------------- | --------------------------------------- | ----------------------------- | -| `~/output/steering_offset` | `tier4_debug_msgs::msg::Float32Stamped` | steering offset | -| `~/output/steering_offset_covariance` | `tier4_debug_msgs::msg::Float32Stamped` | covariance of steering offset | +| Name | Type | Description | +| ------------------------------------- | --------------------------------------------------- | ----------------------------- | +| `~/output/steering_offset` | `autoware_internal_debug_msgs::msg::Float32Stamped` | steering offset | +| `~/output/steering_offset_covariance` | `autoware_internal_debug_msgs::msg::Float32Stamped` | covariance of steering offset | ## Launch Calibrator diff --git a/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp b/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp index 4e03cbe0162fe..0d7896d53a705 100644 --- a/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp +++ b/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp @@ -18,16 +18,16 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" +#include "autoware_internal_debug_msgs/msg/float32_stamped.hpp" #include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" #include namespace autoware::steer_offset_estimator { +using autoware_internal_debug_msgs::msg::Float32Stamped; using geometry_msgs::msg::TwistStamped; -using tier4_debug_msgs::msg::Float32Stamped; using Steering = autoware_vehicle_msgs::msg::SteeringReport; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; diff --git a/vehicle/autoware_steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml index 715d80504dafe..5673025dbb54a 100644 --- a/vehicle/autoware_steer_offset_estimator/package.xml +++ b/vehicle/autoware_steer_offset_estimator/package.xml @@ -1,7 +1,7 @@ autoware_steer_offset_estimator - 0.40.0 + 0.41.0 The steer_offset_estimator Taiki Tanaka Apache License 2.0 @@ -9,6 +9,7 @@ ament_cmake_auto autoware_cmake + autoware_internal_debug_msgs autoware_universe_utils autoware_vehicle_info_utils autoware_vehicle_msgs @@ -17,7 +18,6 @@ rclcpp rclcpp_components std_msgs - tier4_debug_msgs autoware_global_parameter_loader autoware_pose2twist diff --git a/visualization/bag_time_manager_rviz_plugin/CHANGELOG.rst b/visualization/autoware_bag_time_manager_rviz_plugin/CHANGELOG.rst similarity index 78% rename from visualization/bag_time_manager_rviz_plugin/CHANGELOG.rst rename to visualization/autoware_bag_time_manager_rviz_plugin/CHANGELOG.rst index df222e8eb584e..226a8fa910ba8 100644 --- a/visualization/bag_time_manager_rviz_plugin/CHANGELOG.rst +++ b/visualization/autoware_bag_time_manager_rviz_plugin/CHANGELOG.rst @@ -1,6 +1,15 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package bag_time_manager_rviz_plugin -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_bag_time_manager_rviz_plugin +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: apply `autoware\_` prefix for `bag_time_manager_rviz_plugin` (`#9986 `_) + Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> + Co-authored-by: SakodaShintaro + Co-authored-by: Ryohsuke Mitsudome +* Contributors: Fumiya Watanabe, Junya Sasaki 0.40.0 (2024-12-12) ------------------- diff --git a/visualization/bag_time_manager_rviz_plugin/CMakeLists.txt b/visualization/autoware_bag_time_manager_rviz_plugin/CMakeLists.txt similarity index 92% rename from visualization/bag_time_manager_rviz_plugin/CMakeLists.txt rename to visualization/autoware_bag_time_manager_rviz_plugin/CMakeLists.txt index ccf9961fc55cc..5ee02349f7d96 100644 --- a/visualization/bag_time_manager_rviz_plugin/CMakeLists.txt +++ b/visualization/autoware_bag_time_manager_rviz_plugin/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(bag_time_manager_rviz_plugin) +project(autoware_bag_time_manager_rviz_plugin) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/visualization/bag_time_manager_rviz_plugin/README.md b/visualization/autoware_bag_time_manager_rviz_plugin/README.md similarity index 92% rename from visualization/bag_time_manager_rviz_plugin/README.md rename to visualization/autoware_bag_time_manager_rviz_plugin/README.md index 2fad6b2204bb9..58d81d06dffa4 100644 --- a/visualization/bag_time_manager_rviz_plugin/README.md +++ b/visualization/autoware_bag_time_manager_rviz_plugin/README.md @@ -1,4 +1,4 @@ -# bag_time_manager_rviz_plugin +# autoware_bag_time_manager_rviz_plugin ## Purpose diff --git a/visualization/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png b/visualization/autoware_bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png similarity index 100% rename from visualization/bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png rename to visualization/autoware_bag_time_manager_rviz_plugin/icons/classes/BagTimeManagerPanel.png diff --git a/visualization/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png b/visualization/autoware_bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png similarity index 100% rename from visualization/bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png rename to visualization/autoware_bag_time_manager_rviz_plugin/images/add_bag_time_manager_panel.png diff --git a/visualization/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png b/visualization/autoware_bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png similarity index 100% rename from visualization/bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png rename to visualization/autoware_bag_time_manager_rviz_plugin/images/bag_time_manager_panel.png diff --git a/visualization/bag_time_manager_rviz_plugin/images/select_panels.png b/visualization/autoware_bag_time_manager_rviz_plugin/images/select_panels.png similarity index 100% rename from visualization/bag_time_manager_rviz_plugin/images/select_panels.png rename to visualization/autoware_bag_time_manager_rviz_plugin/images/select_panels.png diff --git a/visualization/bag_time_manager_rviz_plugin/package.xml b/visualization/autoware_bag_time_manager_rviz_plugin/package.xml similarity index 85% rename from visualization/bag_time_manager_rviz_plugin/package.xml rename to visualization/autoware_bag_time_manager_rviz_plugin/package.xml index 365dad286d22c..481eac1ac8f21 100644 --- a/visualization/bag_time_manager_rviz_plugin/package.xml +++ b/visualization/autoware_bag_time_manager_rviz_plugin/package.xml @@ -1,10 +1,11 @@ - bag_time_manager_rviz_plugin - 0.40.0 + autoware_bag_time_manager_rviz_plugin + 0.41.0 Rviz plugin to publish and control the ros bag Taiki Tanaka + Junya Sasaki Apache License 2.0 ament_cmake_auto diff --git a/visualization/bag_time_manager_rviz_plugin/plugins/plugin_description.xml b/visualization/autoware_bag_time_manager_rviz_plugin/plugins/plugin_description.xml similarity index 52% rename from visualization/bag_time_manager_rviz_plugin/plugins/plugin_description.xml rename to visualization/autoware_bag_time_manager_rviz_plugin/plugins/plugin_description.xml index 2a3bbf667b715..f8c678a75b4ee 100644 --- a/visualization/bag_time_manager_rviz_plugin/plugins/plugin_description.xml +++ b/visualization/autoware_bag_time_manager_rviz_plugin/plugins/plugin_description.xml @@ -1,7 +1,7 @@ - + Panel that publishes a service to modify its speed. diff --git a/visualization/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp b/visualization/autoware_bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp similarity index 92% rename from visualization/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp rename to visualization/autoware_bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp index cae9660b51266..7ab003e2880de 100644 --- a/visualization/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp +++ b/visualization/autoware_bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp @@ -1,5 +1,5 @@ // -// Copyright 2022 Tier IV, Inc. All rights reserved. +// Copyright 2025 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. @@ -23,7 +23,7 @@ #include -namespace rviz_plugins +namespace autoware::visualization::bag_time_manager_rviz_plugin { BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) { @@ -111,7 +111,8 @@ void BagTimeManagerPanel::onApplyRateClicked() } }); } -} // namespace rviz_plugins +} // namespace autoware::visualization::bag_time_manager_rviz_plugin #include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::BagTimeManagerPanel, rviz_common::Panel) +PLUGINLIB_EXPORT_CLASS( + autoware::visualization::bag_time_manager_rviz_plugin::BagTimeManagerPanel, rviz_common::Panel) diff --git a/visualization/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp b/visualization/autoware_bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp similarity index 91% rename from visualization/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp rename to visualization/autoware_bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp index 2cc4b98a6dabf..710d20046a548 100644 --- a/visualization/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp +++ b/visualization/autoware_bag_time_manager_rviz_plugin/src/bag_time_manager_panel.hpp @@ -1,5 +1,5 @@ // -// Copyright 2022 Tier IV, Inc. All rights reserved. +// Copyright 2025 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. @@ -29,7 +29,7 @@ #include #include -namespace rviz_plugins +namespace autoware::visualization::bag_time_manager_rviz_plugin { using rosbag2_interfaces::srv::Pause; using rosbag2_interfaces::srv::Resume; @@ -67,6 +67,6 @@ protected Q_SLOTS: STATE current_state_{RESUME}; }; -} // namespace rviz_plugins +} // namespace autoware::visualization::bag_time_manager_rviz_plugin #endif // BAG_TIME_MANAGER_PANEL_HPP_ diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CHANGELOG.rst b/visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CHANGELOG.rst index 91b2ce31af3bf..01fe650191a47 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CHANGELOG.rst +++ b/visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_mission_details_overlay_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml b/visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml index 82b4c10194131..7323b3b6594c5 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml +++ b/visualization/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/package.xml @@ -2,7 +2,7 @@ autoware_mission_details_overlay_rviz_plugin - 0.40.0 + 0.41.0 RViz2 plugin for 2D overlays for mission details in the 3D view. Mainly a port of the JSK overlay plugin (https://github.com/jsk-ros-pkg/jsk_visualization). diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CHANGELOG.rst b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CHANGELOG.rst index 44c9901bda3ec..0f3b61630d790 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CHANGELOG.rst +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package autoware_overlay_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(autoware_overlay_rviz_plugin): fix clang-tidy errors (`#9627 `_) + * fix: clang-tidy errors + * fix: clang-fmt + --------- +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml index 86bab15660d4d..fed43c91ddcdb 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -2,7 +2,7 @@ autoware_overlay_rviz_plugin - 0.40.0 + 0.41.0 RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin (https://github.com/jsk-ros-pkg/jsk_visualization). diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index faf9c39af672d..83d7f2f76bde3 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -84,7 +84,8 @@ void SpeedDisplay::drawSpeedDisplay( painter.setFont(referenceFont); QRect referenceRect = painter.fontMetrics().boundingRect("88"); QPointF referencePos( - backgroundRect.width() / 2 - referenceRect.width() / 2 - 5, backgroundRect.height() / 2); + backgroundRect.width() / 2.0 - referenceRect.width() / 2.0 - 5.0, + backgroundRect.height() / 2.0); QString speedNumber = QString::number(current_speed_, 'f', 0); int fontSize = 40; @@ -96,7 +97,7 @@ void SpeedDisplay::drawSpeedDisplay( // Center the speed number in the backgroundRect QPointF speedPos( - backgroundRect.center().x() - speedNumberRect.width() / 2, + backgroundRect.center().x() - speedNumberRect.width() / 2.0, backgroundRect.center().y() + speedNumberRect.bottom()); painter.setPen(color); painter.drawText(speedPos, speedNumber); @@ -106,7 +107,8 @@ void SpeedDisplay::drawSpeedDisplay( QString speedUnit = "km/h"; QRect unitRect = painter.fontMetrics().boundingRect(speedUnit); QPointF unitPos( - (backgroundRect.width() / 2 - unitRect.width() / 2), referencePos.y() + unitRect.height() + 15); + (backgroundRect.width() / 2.0 - unitRect.width() / 2.0), + referencePos.y() + unitRect.height() + 15.0); painter.drawText(unitPos, speedUnit); } diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp index d475a8231e595..fec3ecfb90a43 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -93,7 +93,7 @@ void SteeringWheelDisplay::drawSteeringWheel( QImage rotatedWheel = wheel.transformed(rotationTransform, Qt::SmoothTransformation); QPointF drawPoint( - wheelCenterX - rotatedWheel.width() / 2, wheelCenterY - rotatedWheel.height() / 2); + wheelCenterX - rotatedWheel.width() / 2.0, wheelCenterY - rotatedWheel.height() / 2.0); // Draw the rotated image painter.drawImage(drawPoint.x(), drawPoint.y(), rotatedWheel); diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp index dddd9b52f3d0b..817d49889162a 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -83,9 +83,6 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF painter.drawEllipse(circleRect); break; case 4: - painter.setBrush(tl_gray_); - painter.drawEllipse(circleRect); - break; default: painter.setBrush(tl_gray_); painter.drawEllipse(circleRect); diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp index 6748f01fe139b..ef0721d821e63 100644 --- a/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp +++ b/visualization/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp @@ -76,7 +76,7 @@ void TurnSignalsDisplay::drawArrows( QImage scaledLeftArrow = arrowImage.scaled(50, 32, Qt::KeepAspectRatio, Qt::SmoothTransformation); scaledLeftArrow = coloredImage(scaledLeftArrow, gray); QImage scaledRightArrow = scaledLeftArrow.mirrored(true, false); - int arrowYPos = (backgroundRect.height() / 2 - scaledLeftArrow.height() / 2 - 4); + int arrowYPos = (backgroundRect.height() / 2.0 - scaledLeftArrow.height() / 2.0 - 4.0); int leftArrowXPos = backgroundRect.left() + scaledLeftArrow.width() * 2 + 180; int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175; diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CHANGELOG.rst b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CHANGELOG.rst new file mode 100644 index 0000000000000..bbcb0616d9ca2 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CHANGELOG.rst @@ -0,0 +1,40 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_string_stamped_rviz_plugin +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(autoware_string_stamped_overlay_rviz_plugin): fix version in package.xml (`#9874 `_) +* feat(autoware_overlay_rviz_plugin): add plugin to show string stamped (`#9683 `_) + * feat(autoware_overlay_rviz_plugin): add plugin to show string stamped + * fix + * better visualization + * update + * fix cpp check and typo + * fix + * fix + --------- +* Contributors: Fumiya Watanabe, Takayuki Murooka + +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* chore(autoware_string_stamped_overlay_rviz_plugin): fix version in package.xml (`#9874 `_) +* feat(autoware_overlay_rviz_plugin): add plugin to show string stamped (`#9683 `_) + * feat(autoware_overlay_rviz_plugin): add plugin to show string stamped + * fix + * better visualization + * update + * fix cpp check and typo + * fix + * fix + --------- +* Contributors: Fumiya Watanabe, Takayuki Murooka + +0.40.0 (2025-01-06) +------------------- + +0.39.0 (2024-11-25) +------------------- + +0.38.0 (2024-11-11) +------------------- diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..7ff4e19c69419 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.8) +project(autoware_string_stamped_rviz_plugin) + +# find dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + DIRECTORY src +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugins_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md new file mode 100644 index 0000000000000..e5add261325f4 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/README.md @@ -0,0 +1,7 @@ +# autoware_string_stamped_rviz_plugin + +Plugin for displaying 2D overlays over the RViz2 3D scene. + +## Purpose + +This plugin displays the ROS message whose topic type is `autoware_internal_debug_msgs::msg::StringStamped` in rviz. diff --git a/common/autoware_osqp_interface/package.xml b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml similarity index 53% rename from common/autoware_osqp_interface/package.xml rename to visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml index 92a2afeccc4e0..a7267c3dc2896 100644 --- a/common/autoware_osqp_interface/package.xml +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/package.xml @@ -1,27 +1,28 @@ - autoware_osqp_interface - 0.40.0 - Interface for the OSQP solver - Maxime CLEMENT + autoware_string_stamped_rviz_plugin + 0.41.0 + + RViz2 plugin for 2D overlays in the 3D view. Mainly a port of the JSK overlay plugin + (https://github.com/jsk-ros-pkg/jsk_visualization). + Takayuki Murooka - Fumiya Watanabe Satoshi Ota - Apache 2.0 + + Apache License 2.0 ament_cmake_auto autoware_cmake - eigen - osqp_vendor - rclcpp - rclcpp_components + ament_index_cpp + autoware_internal_debug_msgs + rviz_common + rviz_ogre_vendor + rviz_rendering - ament_cmake_ros ament_lint_auto autoware_lint_common - eigen ament_cmake diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml new file mode 100644 index 0000000000000..302bcc629b892 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/plugins/plugins_description.xml @@ -0,0 +1,5 @@ + + + String stamped overlay plugin for the 3D view. + + diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 0000000000000..03fd8bca5aee8 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,225 @@ +// 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) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject( + Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name) +: name_(name), logger_(logger) +{ + rviz_rendering::RenderSystem::get()->prepareOverlays(manager); + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(logger_, "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(logger_, "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp new file mode 100644 index 0000000000000..e69abed49f371 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/jsk_overlay_utils.hpp @@ -0,0 +1,143 @@ +// 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) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + OverlayObject(Ogre::SceneManager * manager, rclcpp::Logger logger, const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + /*virtual*/ void hide(); // remove "virtual" for cppcheck + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + rclcpp::Logger logger_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp new file mode 100644 index 0000000000000..d5746e99ff084 --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.cpp @@ -0,0 +1,237 @@ +// 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) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "string_stamped_overlay_display.hpp" + +#include "jsk_overlay_utils.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include + +namespace autoware::string_stamped_rviz_plugin +{ +StringStampedOverlayDisplay::StringStampedOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 15, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); + + property_last_diag_keep_time_ = new rviz_common::properties::FloatProperty( + "Time To Keep Last Diag", 1.0, "Time To Keep Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_keep_time_->setMin(0); + + property_last_diag_erase_time_ = new rviz_common::properties::FloatProperty( + "Time To Erase Last Diag", 2.0, "Time To Erase Last Diag", this, SLOT(updateVisualization()), + this); + property_last_diag_erase_time_->setMin(0.001); +} + +StringStampedOverlayDisplay::~StringStampedOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void StringStampedOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "StringOverlayDisplayObject" << count++; + auto logger = context_->getRosNodeAbstraction().lock()->get_raw_node()->get_logger(); + overlay_.reset(new jsk_rviz_plugins::OverlayObject(scene_manager_, logger, ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void StringStampedOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void StringStampedOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void StringStampedOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + { + std::lock_guard message_lock(mutex_); + if (!last_non_empty_msg_ptr_) { + return; + } + } + + // calculate text and alpha + const auto text_with_alpha = [&]() { + std::lock_guard message_lock(mutex_); + if (last_msg_text_.empty()) { + const auto current_time = context_->getRosNodeAbstraction().lock()->get_raw_node()->now(); + const auto duration = (current_time - last_non_empty_msg_ptr_->stamp).seconds(); + if ( + duration < + property_last_diag_keep_time_->getFloat() + property_last_diag_erase_time_->getFloat()) { + const int dynamic_alpha = static_cast(std::max( + (1.0 - std::max(duration - property_last_diag_keep_time_->getFloat(), 0.0) / + property_last_diag_erase_time_->getFloat()) * + 255, + 0.0)); + return std::make_pair(last_non_empty_msg_ptr_->data, dynamic_alpha); + } + } + return std::make_pair(last_msg_text_, 255); + }(); + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(text_with_alpha.second); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + text_with_alpha.first.c_str()); + painter.end(); + updateVisualization(); +} + +void StringStampedOverlayDisplay::processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_text_ = msg_ptr->data; + + // keep the non empty last message for visualization + if (!msg_ptr->data.empty()) { + last_non_empty_msg_ptr_ = msg_ptr; + } + } + + queueRender(); +} + +void StringStampedOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace autoware::string_stamped_rviz_plugin + +#include +PLUGINLIB_EXPORT_CLASS( + autoware::string_stamped_rviz_plugin::StringStampedOverlayDisplay, rviz_common::Display) diff --git a/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp new file mode 100644 index 0000000000000..6f7cd84b91aaa --- /dev/null +++ b/visualization/autoware_overlay_rviz_plugin/autoware_string_stamped_overlay_rviz_plugin/src/string_stamped_overlay_display.hpp @@ -0,0 +1,110 @@ +// 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) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef STRING_STAMPED_OVERLAY_DISPLAY_HPP_ +#define STRING_STAMPED_OVERLAY_DISPLAY_HPP_ + +#include +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#endif + +#include + +namespace autoware::string_stamped_rviz_plugin +{ +class StringStampedOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + StringStampedOverlayDisplay(); + ~StringStampedOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + rviz_common::properties::FloatProperty * property_last_diag_keep_time_; + rviz_common::properties::FloatProperty * property_last_diag_erase_time_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + std::string last_msg_text_; + autoware_internal_debug_msgs::msg::StringStamped::ConstSharedPtr last_non_empty_msg_ptr_; +}; +} // namespace autoware::string_stamped_rviz_plugin + +#endif // STRING_STAMPED_OVERLAY_DISPLAY_HPP_ diff --git a/visualization/autoware_perception_rviz_plugin/CHANGELOG.rst b/visualization/autoware_perception_rviz_plugin/CHANGELOG.rst index a1b325e441fc0..6f585a963c1c8 100644 --- a/visualization/autoware_perception_rviz_plugin/CHANGELOG.rst +++ b/visualization/autoware_perception_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package autoware_perception_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/autoware_perception_rviz_plugin/package.xml b/visualization/autoware_perception_rviz_plugin/package.xml index b3d2d69a34f35..000a50c8c9428 100644 --- a/visualization/autoware_perception_rviz_plugin/package.xml +++ b/visualization/autoware_perception_rviz_plugin/package.xml @@ -2,7 +2,7 @@ autoware_perception_rviz_plugin - 0.40.0 + 0.41.0 Contains plugins to visualize object detection outputs Apex.AI, Inc. Taiki Tanaka diff --git a/visualization/tier4_adapi_rviz_plugin/CHANGELOG.rst b/visualization/tier4_adapi_rviz_plugin/CHANGELOG.rst index 32c8d0a621304..41232c228872d 100644 --- a/visualization/tier4_adapi_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_adapi_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package tier4_adapi_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat: operation mode debug panel (`#8933 `_) +* Contributors: Fumiya Watanabe, Takagi, Isamu + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_adapi_rviz_plugin/CMakeLists.txt b/visualization/tier4_adapi_rviz_plugin/CMakeLists.txt index f06ab33823a3f..c65f1de69e414 100644 --- a/visualization/tier4_adapi_rviz_plugin/CMakeLists.txt +++ b/visualization/tier4_adapi_rviz_plugin/CMakeLists.txt @@ -12,7 +12,7 @@ set(CMAKE_INCLUDE_CURRENT_DIR ON) ament_auto_add_library(${PROJECT_NAME} SHARED src/route_tool.cpp src/route_panel.cpp - src/state_panel.cpp + src/operation_mode_debug_panel.cpp src/door_panel.cpp ) diff --git a/visualization/tier4_adapi_rviz_plugin/README.md b/visualization/tier4_adapi_rviz_plugin/README.md index 4056130776178..e067e6d09b11d 100644 --- a/visualization/tier4_adapi_rviz_plugin/README.md +++ b/visualization/tier4_adapi_rviz_plugin/README.md @@ -1,5 +1,7 @@ # tier4_adapi_rviz_plugin +This package contains tools for testing AD API. For general AD API usage, we recommend using [tier4_state_rviz_plugin](../tier4_state_rviz_plugin/README.md). + ## RoutePanel To use the panel, set the topic name from 2D Goal Pose Tool to `/rviz/routing/pose`. diff --git a/visualization/tier4_adapi_rviz_plugin/package.xml b/visualization/tier4_adapi_rviz_plugin/package.xml index da1af7b150c70..eaf02a0653d49 100644 --- a/visualization/tier4_adapi_rviz_plugin/package.xml +++ b/visualization/tier4_adapi_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_adapi_rviz_plugin - 0.40.0 + 0.41.0 The autoware adapi rviz plugin package Takagi, Isamu Hiroki OTA diff --git a/visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml b/visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml index 0b4f00bd56278..5eaa5ec658be1 100644 --- a/visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml +++ b/visualization/tier4_adapi_rviz_plugin/plugins/plugin_description.xml @@ -8,8 +8,8 @@ RoutePanel - - StatePanel + + OperationModeDebugPanel diff --git a/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.cpp b/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.cpp new file mode 100644 index 0000000000000..8476039b3d2a4 --- /dev/null +++ b/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.cpp @@ -0,0 +1,221 @@ +// +// Copyright 2020 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 "operation_mode_debug_panel.hpp" + +#include +#include +#include + +#include +#include + +namespace tier4_adapi_rviz_plugins +{ + +OperationModeDebugPanel::OperationModeDebugPanel(QWidget * parent) : rviz_common::Panel(parent) +{ + auto * layout = new QGridLayout; + setLayout(layout); + + operation_mode_label_ptr_ = new QLabel("INIT"); + operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); + operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + layout->addWidget(operation_mode_label_ptr_, 0, 0, 2, 1); + + auto_button_ptr_ = new QPushButton("AUTO"); + connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); + layout->addWidget(auto_button_ptr_, 0, 1); + + stop_button_ptr_ = new QPushButton("STOP"); + connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); + layout->addWidget(stop_button_ptr_, 0, 2); + + local_button_ptr_ = new QPushButton("LOCAL"); + connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); + layout->addWidget(local_button_ptr_, 1, 1); + + remote_button_ptr_ = new QPushButton("REMOTE"); + connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); + layout->addWidget(remote_button_ptr_, 1, 2); + + control_mode_label_ptr_ = new QLabel("INIT"); + control_mode_label_ptr_->setAlignment(Qt::AlignCenter); + control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); + layout->addWidget(control_mode_label_ptr_, 2, 0); + + enable_button_ptr_ = new QPushButton("Enable"); + connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); + layout->addWidget(enable_button_ptr_, 2, 1); + + disable_button_ptr_ = new QPushButton("Disable"); + connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); + layout->addWidget(disable_button_ptr_, 2, 2); +} + +void OperationModeDebugPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + + sub_operation_mode_ = raw_node_->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&OperationModeDebugPanel::onOperationMode, this, std::placeholders::_1)); + + client_change_to_autonomous_ = + raw_node_->create_client("/api/operation_mode/change_to_autonomous"); + + client_change_to_stop_ = + raw_node_->create_client("/api/operation_mode/change_to_stop"); + + client_change_to_local_ = + raw_node_->create_client("/api/operation_mode/change_to_local"); + + client_change_to_remote_ = + raw_node_->create_client("/api/operation_mode/change_to_remote"); + + client_enable_autoware_control_ = + raw_node_->create_client("/api/operation_mode/enable_autoware_control"); + + client_enable_direct_control_ = + raw_node_->create_client("/api/operation_mode/disable_autoware_control"); +} + +template +void callService(const rclcpp::Logger & logger, const typename rclcpp::Client::SharedPtr client) +{ + auto req = std::make_shared(); + + RCLCPP_DEBUG(logger, "client request"); + + if (!client->service_is_ready()) { + RCLCPP_DEBUG(logger, "client is unavailable"); + return; + } + + client->async_send_request(req, [logger](typename rclcpp::Client::SharedFuture result) { + RCLCPP_DEBUG( + logger, "Status: %d, %s", result.get()->status.code, result.get()->status.message.c_str()); + }); +} + +void OperationModeDebugPanel::onClickAutonomous() +{ + callService(raw_node_->get_logger(), client_change_to_autonomous_); +} + +void OperationModeDebugPanel::onClickStop() +{ + callService(raw_node_->get_logger(), client_change_to_stop_); +} + +void OperationModeDebugPanel::onClickLocal() +{ + callService(raw_node_->get_logger(), client_change_to_local_); +} + +void OperationModeDebugPanel::onClickRemote() +{ + callService(raw_node_->get_logger(), client_change_to_remote_); +} + +void OperationModeDebugPanel::onClickAutowareControl() +{ + callService(raw_node_->get_logger(), client_enable_autoware_control_); +} + +void OperationModeDebugPanel::onClickDirectControl() +{ + callService(raw_node_->get_logger(), client_enable_direct_control_); +} + +void OperationModeDebugPanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + const auto updateLabel = [](QLabel * label, QString text, QString style) { + label->setText(text); + label->setStyleSheet(style); + }; + + const auto updateButton = [](QPushButton * button, bool available) { + if (available) { + button->setStyleSheet("color: black;"); + } else { + button->setStyleSheet("color: white;"); + } + }; + + // Update current operation mode. + + QString state = ""; + QString style = ""; + + switch (msg->mode) { + case OperationModeState::AUTONOMOUS: + state = "AUTONOMOUS"; + style = "background-color: #00FF00;"; // green + break; + + case OperationModeState::LOCAL: + state = "LOCAL"; + style = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::REMOTE: + state = "REMOTE"; + style = "background-color: #FFFF00;"; // yellow + break; + + case OperationModeState::STOP: + state = "STOP"; + style = "background-color: #FFA500;"; // orange + break; + + default: + state = "UNKNOWN (" + QString::number(msg->mode) + ")"; + style = "background-color: #FF0000;"; // red + break; + } + + if (msg->is_in_transition) { + state += "\n(TRANSITION)"; + } + + updateLabel(operation_mode_label_ptr_, state, style); + + // Update current control mode. + + if (msg->is_autoware_control_enabled) { + updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green + } else { + updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow + } + + // Update operation mode available. + + updateButton(auto_button_ptr_, msg->is_autonomous_mode_available); + updateButton(stop_button_ptr_, msg->is_stop_mode_available); + updateButton(local_button_ptr_, msg->is_local_mode_available); + updateButton(remote_button_ptr_, msg->is_remote_mode_available); + + // Update control mode available. + + updateButton(enable_button_ptr_, true); + updateButton(disable_button_ptr_, true); +} + +} // namespace tier4_adapi_rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::OperationModeDebugPanel, rviz_common::Panel) diff --git a/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.hpp b/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.hpp new file mode 100644 index 0000000000000..52eeb8da74493 --- /dev/null +++ b/visualization/tier4_adapi_rviz_plugin/src/operation_mode_debug_panel.hpp @@ -0,0 +1,76 @@ +// +// Copyright 2020 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 OPERATION_MODE_DEBUG_PANEL_HPP_ +#define OPERATION_MODE_DEBUG_PANEL_HPP_ + +#include +#include +#include +#include + +#include +#include + +namespace tier4_adapi_rviz_plugins +{ + +class OperationModeDebugPanel : public rviz_common::Panel +{ + Q_OBJECT + +public: + explicit OperationModeDebugPanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onClickAutonomous(); + void onClickStop(); + void onClickLocal(); + void onClickRemote(); + void onClickAutowareControl(); + void onClickDirectControl(); + +protected: + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + + QLabel * operation_mode_label_ptr_{nullptr}; + QPushButton * stop_button_ptr_{nullptr}; + QPushButton * auto_button_ptr_{nullptr}; + QPushButton * local_button_ptr_{nullptr}; + QPushButton * remote_button_ptr_{nullptr}; + + QLabel * control_mode_label_ptr_{nullptr}; + QPushButton * enable_button_ptr_{nullptr}; + QPushButton * disable_button_ptr_{nullptr}; + + rclcpp::Node::SharedPtr raw_node_; + rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Client::SharedPtr client_change_to_autonomous_; + rclcpp::Client::SharedPtr client_change_to_stop_; + rclcpp::Client::SharedPtr client_change_to_local_; + rclcpp::Client::SharedPtr client_change_to_remote_; + rclcpp::Client::SharedPtr client_enable_autoware_control_; + rclcpp::Client::SharedPtr client_enable_direct_control_; + + void onOperationMode(const OperationModeState::ConstSharedPtr msg); + void changeOperationMode(const rclcpp::Client::SharedPtr client); +}; + +} // namespace tier4_adapi_rviz_plugins + +#endif // OPERATION_MODE_DEBUG_PANEL_HPP_ diff --git a/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp b/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp deleted file mode 100644 index 0525f5fdc1a09..0000000000000 --- a/visualization/tier4_adapi_rviz_plugin/src/state_panel.cpp +++ /dev/null @@ -1,633 +0,0 @@ -// -// Copyright 2020 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 "state_panel.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace tier4_adapi_rviz_plugins -{ - -StatePanel::StatePanel(QWidget * parent) : rviz_common::Panel(parent) -{ - // Gear - auto * gear_prefix_label_ptr = new QLabel("GEAR: "); - gear_prefix_label_ptr->setAlignment(Qt::AlignRight | Qt::AlignVCenter); - gear_label_ptr_ = new QLabel("INIT"); - gear_label_ptr_->setAlignment(Qt::AlignCenter); - auto * gear_layout = new QHBoxLayout; - gear_layout->addWidget(gear_prefix_label_ptr); - gear_layout->addWidget(gear_label_ptr_); - - // Velocity Limit - velocity_limit_button_ptr_ = new QPushButton("Send Velocity Limit"); - pub_velocity_limit_input_ = new QSpinBox(); - pub_velocity_limit_input_->setRange(-100.0, 100.0); - pub_velocity_limit_input_->setValue(0.0); - pub_velocity_limit_input_->setSingleStep(5.0); - connect(velocity_limit_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickVelocityLimit())); - - // Emergency Button - emergency_button_ptr_ = new QPushButton("Set Emergency"); - connect(emergency_button_ptr_, SIGNAL(clicked()), this, SLOT(onClickEmergencyButton())); - - // Layout - auto * v_layout = new QVBoxLayout; - auto * velocity_limit_layout = new QHBoxLayout(); - v_layout->addWidget(makeOperationModeGroup()); - v_layout->addWidget(makeControlModeGroup()); - { - auto * h_layout = new QHBoxLayout(); - h_layout->addWidget(makeRoutingGroup()); - h_layout->addWidget(makeLocalizationGroup()); - h_layout->addWidget(makeMotionGroup()); - h_layout->addWidget(makeFailSafeGroup()); - v_layout->addLayout(h_layout); - } - - v_layout->addLayout(gear_layout); - velocity_limit_layout->addWidget(velocity_limit_button_ptr_); - velocity_limit_layout->addWidget(pub_velocity_limit_input_); - velocity_limit_layout->addWidget(new QLabel(" [km/h]")); - velocity_limit_layout->addWidget(emergency_button_ptr_); - v_layout->addLayout(velocity_limit_layout); - setLayout(v_layout); -} - -QGroupBox * StatePanel::makeOperationModeGroup() -{ - auto * group = new QGroupBox("OperationMode"); - auto * grid = new QGridLayout; - - operation_mode_label_ptr_ = new QLabel("INIT"); - operation_mode_label_ptr_->setAlignment(Qt::AlignCenter); - operation_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(operation_mode_label_ptr_, 0, 0, 0, 1); - - auto_button_ptr_ = new QPushButton("AUTO"); - auto_button_ptr_->setCheckable(true); - connect(auto_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutonomous())); - grid->addWidget(auto_button_ptr_, 0, 1); - - stop_button_ptr_ = new QPushButton("STOP"); - stop_button_ptr_->setCheckable(true); - connect(stop_button_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); - grid->addWidget(stop_button_ptr_, 0, 2); - - local_button_ptr_ = new QPushButton("LOCAL"); - local_button_ptr_->setCheckable(true); - connect(local_button_ptr_, SIGNAL(clicked()), SLOT(onClickLocal())); - grid->addWidget(local_button_ptr_, 1, 1); - - remote_button_ptr_ = new QPushButton("REMOTE"); - remote_button_ptr_->setCheckable(true); - connect(remote_button_ptr_, SIGNAL(clicked()), SLOT(onClickRemote())); - grid->addWidget(remote_button_ptr_, 1, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeControlModeGroup() -{ - auto * group = new QGroupBox("AutowareControl"); - auto * grid = new QGridLayout; - - control_mode_label_ptr_ = new QLabel("INIT"); - control_mode_label_ptr_->setAlignment(Qt::AlignCenter); - control_mode_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(control_mode_label_ptr_, 0, 0); - - enable_button_ptr_ = new QPushButton("Enable"); - enable_button_ptr_->setCheckable(true); - connect(enable_button_ptr_, SIGNAL(clicked()), SLOT(onClickAutowareControl())); - grid->addWidget(enable_button_ptr_, 0, 1); - - disable_button_ptr_ = new QPushButton("Disable"); - disable_button_ptr_->setCheckable(true); - connect(disable_button_ptr_, SIGNAL(clicked()), SLOT(onClickDirectControl())); - grid->addWidget(disable_button_ptr_, 0, 2); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeRoutingGroup() -{ - auto * group = new QGroupBox("Routing"); - auto * grid = new QGridLayout; - - routing_label_ptr_ = new QLabel("INIT"); - routing_label_ptr_->setAlignment(Qt::AlignCenter); - routing_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(routing_label_ptr_, 0, 0); - - clear_route_button_ptr_ = new QPushButton("Clear Route"); - clear_route_button_ptr_->setCheckable(true); - connect(clear_route_button_ptr_, SIGNAL(clicked()), SLOT(onClickClearRoute())); - grid->addWidget(clear_route_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeLocalizationGroup() -{ - auto * group = new QGroupBox("Localization"); - auto * grid = new QGridLayout; - - localization_label_ptr_ = new QLabel("INIT"); - localization_label_ptr_->setAlignment(Qt::AlignCenter); - localization_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(localization_label_ptr_, 0, 0); - - init_by_gnss_button_ptr_ = new QPushButton("Init by GNSS"); - connect(init_by_gnss_button_ptr_, SIGNAL(clicked()), SLOT(onClickInitByGnss())); - grid->addWidget(init_by_gnss_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeMotionGroup() -{ - auto * group = new QGroupBox("Motion"); - auto * grid = new QGridLayout; - - motion_label_ptr_ = new QLabel("INIT"); - motion_label_ptr_->setAlignment(Qt::AlignCenter); - motion_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(motion_label_ptr_, 0, 0); - - accept_start_button_ptr_ = new QPushButton("Accept Start"); - accept_start_button_ptr_->setCheckable(true); - connect(accept_start_button_ptr_, SIGNAL(clicked()), SLOT(onClickAcceptStart())); - grid->addWidget(accept_start_button_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -QGroupBox * StatePanel::makeFailSafeGroup() -{ - auto * group = new QGroupBox("FailSafe"); - auto * grid = new QGridLayout; - - mrm_state_label_ptr_ = new QLabel("INIT"); - mrm_state_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_state_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_state_label_ptr_, 0, 0); - - mrm_behavior_label_ptr_ = new QLabel("INIT"); - mrm_behavior_label_ptr_->setAlignment(Qt::AlignCenter); - mrm_behavior_label_ptr_->setStyleSheet("border:1px solid black;"); - grid->addWidget(mrm_behavior_label_ptr_, 1, 0); - - group->setLayout(grid); - return group; -} - -void StatePanel::onInitialize() -{ - using std::placeholders::_1; - - raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - - // Operation Mode - sub_operation_mode_ = raw_node_->create_subscription( - "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onOperationMode, this, _1)); - - client_change_to_autonomous_ = - raw_node_->create_client("/api/operation_mode/change_to_autonomous"); - - client_change_to_stop_ = - raw_node_->create_client("/api/operation_mode/change_to_stop"); - - client_change_to_local_ = - raw_node_->create_client("/api/operation_mode/change_to_local"); - - client_change_to_remote_ = - raw_node_->create_client("/api/operation_mode/change_to_remote"); - - client_enable_autoware_control_ = - raw_node_->create_client("/api/operation_mode/enable_autoware_control"); - - client_enable_direct_control_ = - raw_node_->create_client("/api/operation_mode/disable_autoware_control"); - - // Routing - sub_route_ = raw_node_->create_subscription( - "/api/routing/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onRoute, this, _1)); - - client_clear_route_ = raw_node_->create_client("/api/routing/clear_route"); - - // Localization - sub_localization_ = raw_node_->create_subscription( - "/api/localization/initialization_state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onLocalization, this, _1)); - - client_init_by_gnss_ = - raw_node_->create_client("/api/localization/initialize"); - - // Motion - sub_motion_ = raw_node_->create_subscription( - "/api/motion/state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onMotion, this, _1)); - - client_accept_start_ = raw_node_->create_client("/api/motion/accept_start"); - - // FailSafe - sub_mrm_ = raw_node_->create_subscription( - "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), - std::bind(&StatePanel::onMRMState, this, _1)); - - // Others - sub_gear_ = raw_node_->create_subscription( - "/vehicle/status/gear_status", 10, std::bind(&StatePanel::onShift, this, _1)); - - sub_emergency_ = raw_node_->create_subscription( - "/api/autoware/get/emergency", 10, std::bind(&StatePanel::onEmergencyStatus, this, _1)); - - client_emergency_stop_ = raw_node_->create_client( - "/api/autoware/set/emergency"); - - pub_velocity_limit_ = raw_node_->create_publisher( - "/planning/scenario_planning/max_velocity_default", rclcpp::QoS{1}.transient_local()); -} - -void StatePanel::onOperationMode(const OperationModeState::ConstSharedPtr msg) -{ - auto changeButtonState = []( - QPushButton * button, const bool is_desired_mode_available, - const uint8_t current_mode = OperationModeState::UNKNOWN, - const uint8_t desired_mode = OperationModeState::STOP) { - if (is_desired_mode_available && current_mode != desired_mode) { - activateButton(button); - } else { - deactivateButton(button); - } - }; - - QString text = ""; - QString style_sheet = ""; - // Operation Mode - switch (msg->mode) { - case OperationModeState::AUTONOMOUS: - text = "AUTONOMOUS"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case OperationModeState::LOCAL: - text = "LOCAL"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case OperationModeState::REMOTE: - text = "REMOTE"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case OperationModeState::STOP: - text = "STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - if (msg->is_in_transition) { - text += "\n(TRANSITION)"; - } - - updateLabel(operation_mode_label_ptr_, text, style_sheet); - - // Control Mode - if (msg->is_autoware_control_enabled) { - updateLabel(control_mode_label_ptr_, "Enable", "background-color: #00FF00;"); // green - } else { - updateLabel(control_mode_label_ptr_, "Disable", "background-color: #FFFF00;"); // yellow - } - - // Button - changeButtonState( - auto_button_ptr_, msg->is_autonomous_mode_available, msg->mode, OperationModeState::AUTONOMOUS); - changeButtonState( - stop_button_ptr_, msg->is_stop_mode_available, msg->mode, OperationModeState::STOP); - changeButtonState( - local_button_ptr_, msg->is_local_mode_available, msg->mode, OperationModeState::LOCAL); - changeButtonState( - remote_button_ptr_, msg->is_remote_mode_available, msg->mode, OperationModeState::REMOTE); - - changeButtonState(enable_button_ptr_, !msg->is_autoware_control_enabled); - changeButtonState(disable_button_ptr_, msg->is_autoware_control_enabled); -} - -void StatePanel::onRoute(const RouteState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case RouteState::UNSET: - text = "UNSET"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case RouteState::SET: - text = "SET"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case RouteState::ARRIVED: - text = "ARRIVED"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case RouteState::CHANGING: - text = "CHANGING"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(routing_label_ptr_, text, style_sheet); - - if (msg->state == RouteState::SET) { - activateButton(clear_route_button_ptr_); - } else { - deactivateButton(clear_route_button_ptr_); - } -} - -void StatePanel::onLocalization(const LocalizationInitializationState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case LocalizationInitializationState::UNINITIALIZED: - text = "UNINITIALIZED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case LocalizationInitializationState::INITIALIZING: - text = "INITIALIZING"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case LocalizationInitializationState::INITIALIZED: - text = "INITIALIZED"; - style_sheet = "background-color: #00FF00;"; // green - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(localization_label_ptr_, text, style_sheet); -} - -void StatePanel::onMotion(const MotionState::ConstSharedPtr msg) -{ - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MotionState::STARTING: - text = "STARTING"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MotionState::STOPPED: - text = "STOPPED"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case MotionState::MOVING: - text = "MOVING"; - style_sheet = "background-color: #00FF00;"; // green - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(motion_label_ptr_, text, style_sheet); - - if (msg->state == MotionState::STARTING) { - activateButton(accept_start_button_ptr_); - } else { - deactivateButton(accept_start_button_ptr_); - } -} - -void StatePanel::onMRMState(const MRMState::ConstSharedPtr msg) -{ - // state - { - QString text = ""; - QString style_sheet = ""; - switch (msg->state) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case MRMState::MRM_OPERATING: - text = "MRM_OPERATING"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - case MRMState::MRM_SUCCEEDED: - text = "MRM_SUCCEEDED"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::MRM_FAILED: - text = "MRM_FAILED"; - style_sheet = "background-color: #FF0000;"; // red - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(mrm_state_label_ptr_, text, style_sheet); - } - - // behavior - { - QString text = ""; - QString style_sheet = ""; - switch (msg->behavior) { - case MRMState::NONE: - text = "NONE"; - style_sheet = "background-color: #00FF00;"; // green - break; - - case MRMState::PULL_OVER: - text = "PULL_OVER"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::COMFORTABLE_STOP: - text = "COMFORTABLE_STOP"; - style_sheet = "background-color: #FFFF00;"; // yellow - break; - - case MRMState::EMERGENCY_STOP: - text = "EMERGENCY_STOP"; - style_sheet = "background-color: #FFA500;"; // orange - break; - - default: - text = "UNKNOWN"; - style_sheet = "background-color: #FF0000;"; // red - break; - } - - updateLabel(mrm_behavior_label_ptr_, text, style_sheet); - } -} - -void StatePanel::onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) -{ - switch (msg->report) { - case autoware_vehicle_msgs::msg::GearReport::PARK: - gear_label_ptr_->setText("PARKING"); - break; - case autoware_vehicle_msgs::msg::GearReport::REVERSE: - gear_label_ptr_->setText("REVERSE"); - break; - case autoware_vehicle_msgs::msg::GearReport::DRIVE: - gear_label_ptr_->setText("DRIVE"); - break; - case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: - gear_label_ptr_->setText("NEUTRAL"); - break; - case autoware_vehicle_msgs::msg::GearReport::LOW: - gear_label_ptr_->setText("LOW"); - break; - } -} - -void StatePanel::onEmergencyStatus( - const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg) -{ - current_emergency_ = msg->emergency; - if (msg->emergency) { - emergency_button_ptr_->setText(QString::fromStdString("Clear Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #FF0000;"); - } else { - emergency_button_ptr_->setText(QString::fromStdString("Set Emergency")); - emergency_button_ptr_->setStyleSheet("background-color: #00FF00;"); - } -} - -void StatePanel::onClickVelocityLimit() -{ - auto velocity_limit = std::make_shared(); - velocity_limit->stamp = raw_node_->now(); - velocity_limit->max_velocity = pub_velocity_limit_input_->value() / 3.6; - pub_velocity_limit_->publish(*velocity_limit); -} - -void StatePanel::onClickAutonomous() -{ - callServiceWithoutResponse(client_change_to_autonomous_); -} -void StatePanel::onClickStop() -{ - callServiceWithoutResponse(client_change_to_stop_); -} -void StatePanel::onClickLocal() -{ - callServiceWithoutResponse(client_change_to_local_); -} -void StatePanel::onClickRemote() -{ - callServiceWithoutResponse(client_change_to_remote_); -} -void StatePanel::onClickAutowareControl() -{ - callServiceWithoutResponse(client_enable_autoware_control_); -} -void StatePanel::onClickDirectControl() -{ - callServiceWithoutResponse(client_enable_direct_control_); -} - -void StatePanel::onClickClearRoute() -{ - callServiceWithoutResponse(client_clear_route_); -} - -void StatePanel::onClickInitByGnss() -{ - callServiceWithoutResponse(client_init_by_gnss_); -} - -void StatePanel::onClickAcceptStart() -{ - callServiceWithoutResponse(client_accept_start_); -} - -void StatePanel::onClickEmergencyButton() -{ - using tier4_external_api_msgs::msg::ResponseStatus; - using tier4_external_api_msgs::srv::SetEmergency; - - auto request = std::make_shared(); - request->emergency = !current_emergency_; - - RCLCPP_INFO(raw_node_->get_logger(), request->emergency ? "Set Emergency" : "Clear Emergency"); - - client_emergency_stop_->async_send_request( - request, [this](rclcpp::Client::SharedFuture result) { - const auto & response = result.get(); - if (response->status.code == ResponseStatus::SUCCESS) { - RCLCPP_INFO(raw_node_->get_logger(), "service succeeded"); - } else { - RCLCPP_WARN( - raw_node_->get_logger(), "service failed: %s", response->status.message.c_str()); - } - }); -} - -} // namespace tier4_adapi_rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(tier4_adapi_rviz_plugins::StatePanel, rviz_common::Panel) diff --git a/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp b/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp deleted file mode 100644 index b30494772061e..0000000000000 --- a/visualization/tier4_adapi_rviz_plugin/src/state_panel.hpp +++ /dev/null @@ -1,206 +0,0 @@ -// -// Copyright 2020 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 STATE_PANEL_HPP_ -#define STATE_PANEL_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace tier4_adapi_rviz_plugins -{ - -class StatePanel : public rviz_common::Panel -{ - using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; - using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; - using RouteState = autoware_adapi_v1_msgs::msg::RouteState; - using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; - using LocalizationInitializationState = - autoware_adapi_v1_msgs::msg::LocalizationInitializationState; - using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; - using MotionState = autoware_adapi_v1_msgs::msg::MotionState; - using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; - using MRMState = autoware_adapi_v1_msgs::msg::MrmState; - - Q_OBJECT - -public: - explicit StatePanel(QWidget * parent = nullptr); - void onInitialize() override; - -public Q_SLOTS: // NOLINT for Qt - void onClickAutonomous(); - void onClickStop(); - void onClickLocal(); - void onClickRemote(); - void onClickAutowareControl(); - void onClickDirectControl(); - void onClickClearRoute(); - void onClickInitByGnss(); - void onClickAcceptStart(); - void onClickVelocityLimit(); - void onClickEmergencyButton(); - -protected: - // Layout - QGroupBox * makeOperationModeGroup(); - QGroupBox * makeControlModeGroup(); - QGroupBox * makeRoutingGroup(); - QGroupBox * makeLocalizationGroup(); - QGroupBox * makeMotionGroup(); - QGroupBox * makeFailSafeGroup(); - - void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); - void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); - - rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_gear_; - rclcpp::Client::SharedPtr client_emergency_stop_; - rclcpp::Subscription::SharedPtr sub_emergency_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_; - - // Operation Mode - QLabel * operation_mode_label_ptr_{nullptr}; - QPushButton * stop_button_ptr_{nullptr}; - QPushButton * auto_button_ptr_{nullptr}; - QPushButton * local_button_ptr_{nullptr}; - QPushButton * remote_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_operation_mode_; - rclcpp::Client::SharedPtr client_change_to_autonomous_; - rclcpp::Client::SharedPtr client_change_to_stop_; - rclcpp::Client::SharedPtr client_change_to_local_; - rclcpp::Client::SharedPtr client_change_to_remote_; - - // Control Mode - QLabel * control_mode_label_ptr_{nullptr}; - QPushButton * enable_button_ptr_{nullptr}; - QPushButton * disable_button_ptr_{nullptr}; - rclcpp::Client::SharedPtr client_enable_autoware_control_; - rclcpp::Client::SharedPtr client_enable_direct_control_; - - // Functions - void onOperationMode(const OperationModeState::ConstSharedPtr msg); - void changeOperationMode(const rclcpp::Client::SharedPtr client); - - // Routing - QLabel * routing_label_ptr_{nullptr}; - QPushButton * clear_route_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Client::SharedPtr client_clear_route_; - - void onRoute(const RouteState::ConstSharedPtr msg); - - // Localization - QLabel * localization_label_ptr_{nullptr}; - QPushButton * init_by_gnss_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_localization_; - rclcpp::Client::SharedPtr client_init_by_gnss_; - - void onLocalization(const LocalizationInitializationState::ConstSharedPtr msg); - - // Motion - QLabel * motion_label_ptr_{nullptr}; - QPushButton * accept_start_button_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_motion_; - rclcpp::Client::SharedPtr client_accept_start_; - - void onMotion(const MotionState::ConstSharedPtr msg); - - // FailSafe - QLabel * mrm_state_label_ptr_{nullptr}; - QLabel * mrm_behavior_label_ptr_{nullptr}; - - rclcpp::Subscription::SharedPtr sub_mrm_; - - void onMRMState(const MRMState::ConstSharedPtr msg); - - // Others - QPushButton * velocity_limit_button_ptr_; - QLabel * gear_label_ptr_; - - QSpinBox * pub_velocity_limit_input_; - QPushButton * emergency_button_ptr_; - - bool current_emergency_{false}; - - template - void callServiceWithoutResponse(const typename rclcpp::Client::SharedPtr client) - { - auto req = std::make_shared(); - - RCLCPP_DEBUG(raw_node_->get_logger(), "client request"); - - if (!client->service_is_ready()) { - RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable"); - return; - } - - client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { - RCLCPP_DEBUG( - raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code, - result.get()->status.message.c_str()); - }); - } - - static void updateLabel(QLabel * label, QString text, QString style_sheet) - { - label->setText(text); - label->setStyleSheet(style_sheet); - } - - static void activateButton(QAbstractButton * button) - { - button->setChecked(false); - button->setEnabled(true); - } - - static void deactivateButton(QAbstractButton * button) - { - button->setChecked(true); - button->setEnabled(false); - } -}; - -} // namespace tier4_adapi_rviz_plugins - -#endif // STATE_PANEL_HPP_ diff --git a/visualization/tier4_camera_view_rviz_plugin/CHANGELOG.rst b/visualization/tier4_camera_view_rviz_plugin/CHANGELOG.rst index 99d51e3064524..3dc5b3cf668f3 100644 --- a/visualization/tier4_camera_view_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_camera_view_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,15 @@ Changelog for package tier4_camera_view_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(tier4_camera_view_rviz_plugin): fix bugprone-parent-virtual-call (`#9815 `_) + * fix:bugprone-error + * fix:fmt + --------- +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_camera_view_rviz_plugin/package.xml b/visualization/tier4_camera_view_rviz_plugin/package.xml index b25aac8a74570..0a1d30c509aab 100644 --- a/visualization/tier4_camera_view_rviz_plugin/package.xml +++ b/visualization/tier4_camera_view_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_camera_view_rviz_plugin - 0.40.0 + 0.41.0 The autoware camera view rviz plugin package Yuxuan Liu Makoto Yabuta diff --git a/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp b/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp index b7d754b02d4bd..386c8cdfb0b13 100644 --- a/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp +++ b/visualization/tier4_camera_view_rviz_plugin/src/third_person_view_controller.cpp @@ -184,7 +184,7 @@ void ThirdPersonViewController::handleMouseEvent(rviz_common::ViewportMouseEvent void ThirdPersonViewController::mimic(rviz_common::ViewController * source_view) { - FramePositionTrackingViewController::mimic(source_view); + FramePositionTrackingViewController::mimic(source_view); // NOLINT target_frame_property_->setValue(TARGET_FRAME_START); getNewTransform(); diff --git a/visualization/tier4_datetime_rviz_plugin/CHANGELOG.rst b/visualization/tier4_datetime_rviz_plugin/CHANGELOG.rst index 155d36c369d77..76211d6be48f9 100644 --- a/visualization/tier4_datetime_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_datetime_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_datetime_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_datetime_rviz_plugin/package.xml b/visualization/tier4_datetime_rviz_plugin/package.xml index 80c19ec0957fd..5ade60b0d3f3c 100644 --- a/visualization/tier4_datetime_rviz_plugin/package.xml +++ b/visualization/tier4_datetime_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_datetime_rviz_plugin - 0.40.0 + 0.41.0 The tier4_datetime_rviz_plugin package Takagi, Isamu Apache License 2.0 diff --git a/visualization/tier4_localization_rviz_plugin/CHANGELOG.rst b/visualization/tier4_localization_rviz_plugin/CHANGELOG.rst index 4e9985286862b..7d7ab591b1544 100644 --- a/visualization/tier4_localization_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_localization_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_localization_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_localization_rviz_plugin/package.xml b/visualization/tier4_localization_rviz_plugin/package.xml index e3f200aeb7111..2c3fef66f43f3 100644 --- a/visualization/tier4_localization_rviz_plugin/package.xml +++ b/visualization/tier4_localization_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_localization_rviz_plugin - 0.40.0 + 0.41.0 The tier4_localization_rviz_plugin package Takagi, Isamu Takamasa Horibe diff --git a/visualization/tier4_planning_factor_rviz_plugin/CHANGELOG.rst b/visualization/tier4_planning_factor_rviz_plugin/CHANGELOG.rst new file mode 100644 index 0000000000000..e62ea3267b362 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/CHANGELOG.rst @@ -0,0 +1,32 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package tier4_planning_factor_rviz_plugin +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(rviz): add new plugin to visualize planning factor (`#9999 `_) + * feat(rviz): add new plugin to visualize planning factor + * feat(rviz): show safety factors + * chore: add maintainer + * feat: show detail + --------- +* Contributors: Fumiya Watanabe, Satoshi OTA + +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* feat(rviz): add new plugin to visualize planning factor (`#9999 `_) + * feat(rviz): add new plugin to visualize planning factor + * feat(rviz): show safety factors + * chore: add maintainer + * feat: show detail + --------- +* Contributors: Fumiya Watanabe, Satoshi OTA + +0.40.0 (2025-01-06) +------------------- + +0.39.0 (2024-11-25) +------------------- + +0.38.0 (2024-11-11) +------------------- diff --git a/visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt b/visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..db1fa63c128fb --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_planning_factor_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(tier4_planning_factor_rviz_plugin SHARED + src/planning_factor_rviz_plugin.cpp +) + +target_link_libraries(tier4_planning_factor_rviz_plugin + ${QT_LIBRARIES} +) + +pluginlib_export_plugin_description_file(rviz_common plugins.xml) + +ament_auto_package( + INSTALL_TO_SHARE + icons +) diff --git a/visualization/tier4_planning_factor_rviz_plugin/README.md b/visualization/tier4_planning_factor_rviz_plugin/README.md new file mode 100644 index 0000000000000..7b65c89d5c22d --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/README.md @@ -0,0 +1 @@ +# tier4_planning_factor_rviz_plugin diff --git a/visualization/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png b/visualization/tier4_planning_factor_rviz_plugin/icons/classes/PlanningFactorRvizPlugin.png similarity index 100% rename from visualization/tier4_adapi_rviz_plugin/icons/classes/StatePanel.png rename to visualization/tier4_planning_factor_rviz_plugin/icons/classes/PlanningFactorRvizPlugin.png diff --git a/visualization/tier4_planning_factor_rviz_plugin/package.xml b/visualization/tier4_planning_factor_rviz_plugin/package.xml new file mode 100644 index 0000000000000..6e703458782c4 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/package.xml @@ -0,0 +1,33 @@ + + + + tier4_planning_factor_rviz_plugin + 0.41.0 + The tier4_planning_factor_rviz_plugin package + Satoshi Ota + Mamoru Sobue + Shumpei Wakabayashi + Apache 2.0 + + ament_cmake + autoware_cmake + + qtbase5-dev + + autoware_motion_utils + autoware_universe_utils + autoware_vehicle_info_utils + rviz_common + rviz_default_plugins + tier4_planning_msgs + + libqt5-widgets + rviz2 + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/visualization/tier4_planning_factor_rviz_plugin/plugins.xml b/visualization/tier4_planning_factor_rviz_plugin/plugins.xml new file mode 100644 index 0000000000000..7e5b53d2b1d61 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/plugins.xml @@ -0,0 +1,8 @@ + + + + tier4_planning_factor_rviz_plugin + tier4_planning_msgs/msg/PlanningFactorArray + + + diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp new file mode 100644 index 0000000000000..9530368421596 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.cpp @@ -0,0 +1,189 @@ +// 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 "planning_factor_rviz_plugin.hpp" + +#include +#include +#include +#include + +#include + +namespace autoware::rviz_plugins +{ + +using autoware::motion_utils::createDeadLineVirtualWallMarker; +using autoware::motion_utils::createSlowDownVirtualWallMarker; +using autoware::motion_utils::createStopVirtualWallMarker; +using autoware::universe_utils::createDefaultMarker; +using autoware::universe_utils::createMarkerScale; + +namespace +{ + +std_msgs::msg::ColorRGBA convertFromColorCode(const uint64_t code, const float alpha) +{ + const float r = static_cast(code >> 16) / 255.0; + const float g = static_cast((code << 48) >> 56) / 255.0; + const float b = static_cast((code << 56) >> 56) / 255.0; + + return autoware::universe_utils::createMarkerColor(r, g, b, alpha); +} + +std_msgs::msg::ColorRGBA getGreen(const float alpha) +{ + constexpr uint64_t code = 0x00e676; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getRed(const float alpha) +{ + constexpr uint64_t code = 0xff3d00; + return convertFromColorCode(code, alpha); +} + +std_msgs::msg::ColorRGBA getGray(const float alpha) +{ + constexpr uint64_t code = 0xbdbdbd; + return convertFromColorCode(code, alpha); +} + +visualization_msgs::msg::Marker createArrowMarker( + const size_t id, const geometry_msgs::msg::Point & position, + const std_msgs::msg::ColorRGBA & color, const std::string & name, const double height_offset, + const double arrow_length = 1.0) +{ + const double line_width = 0.25 * arrow_length; + const double head_width = 0.5 * arrow_length; + const double head_height = 0.5 * arrow_length; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_arrow", id, + visualization_msgs::msg::Marker::ARROW, createMarkerScale(line_width, head_width, head_height), + color); + + geometry_msgs::msg::Point src, dst; + src = position; + src.z += height_offset + arrow_length; + dst = position; + dst.z += height_offset; + + marker.points.push_back(src); + marker.points.push_back(dst); + + return marker; +} + +visualization_msgs::msg::Marker createCircleMarker( + const size_t id, const geometry_msgs::msg::Point & position, + const std_msgs::msg::ColorRGBA & color, const std::string & name, const double radius, + const double height_offset, const double line_width = 0.1) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name, id, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(line_width, 0.0, 0.0), color); + + constexpr size_t num_points = 20; + for (size_t i = 0; i < num_points; ++i) { + geometry_msgs::msg::Point point; + const double ratio = static_cast(i) / static_cast(num_points); + const double theta = 2 * autoware::universe_utils::pi * ratio; + point.x = position.x + radius * autoware::universe_utils::cos(theta); + point.y = position.y + radius * autoware::universe_utils::sin(theta); + point.z = position.z + height_offset; + marker.points.push_back(point); + } + marker.points.push_back(marker.points.front()); + + return marker; +} + +visualization_msgs::msg::Marker createNameTextMarker( + const size_t id, const geometry_msgs::msg::Point & position, const std::string & name, + const double height_offset, const double text_size = 0.5) +{ + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), name + "_name_text", id, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, text_size), + getGray(0.999)); + + marker.text = name; + + marker.pose.position = position; + marker.pose.position.z += height_offset; + + return marker; +} + +visualization_msgs::msg::MarkerArray createTargetMarker( + const size_t id, const geometry_msgs::msg::Point & position, + const std_msgs::msg::ColorRGBA & color, const std::string & name, + const double height_offset = 2.0, const double arrow_length = 1.0, const double line_width = 0.1) +{ + visualization_msgs::msg::MarkerArray marker_array; + marker_array.markers.push_back( + createArrowMarker(id, position, color, name, height_offset, arrow_length)); + marker_array.markers.push_back(createCircleMarker( + id, position, color, name + "_circle1", 0.5 * arrow_length, height_offset + 0.75 * arrow_length, + line_width)); + marker_array.markers.push_back(createCircleMarker( + id, position, color, name + "_circle2", 0.75 * arrow_length, + height_offset + 0.75 * arrow_length, line_width)); + marker_array.markers.push_back(createNameTextMarker( + id, position, name, height_offset + 1.5 * arrow_length, 0.5 * arrow_length)); + + return marker_array; +} +} // namespace + +void PlanningFactorRvizPlugin::processMessage( + const tier4_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) +{ + size_t i = 0L; + for (const auto & factor : msg->factors) { + const auto text = factor.module + (factor.detail.empty() ? "" : " (" + factor.detail + ")"); + + switch (factor.behavior) { + case tier4_planning_msgs::msg::PlanningFactor::STOP: + for (const auto & control_point : factor.control_points) { + const auto virtual_wall = createStopVirtualWallMarker( + control_point.pose, text, msg->header.stamp, i++, baselink2front_.getFloat()); + add_marker(std::make_shared(virtual_wall)); + } + break; + + case tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN: + for (const auto & control_point : factor.control_points) { + const auto virtual_wall = createSlowDownVirtualWallMarker( + control_point.pose, text, msg->header.stamp, i++, baselink2front_.getFloat()); + add_marker(std::make_shared(virtual_wall)); + } + break; + } + + for (const auto & safety_factor : factor.safety_factors.factors) { + const auto color = safety_factor.is_safe ? getGreen(0.999) : getRed(0.999); + for (const auto & point : safety_factor.points) { + const auto safety_factor_marker = createTargetMarker(i++, point, color, factor.module); + add_marker(std::make_shared(safety_factor_marker)); + } + } + } +} +} // namespace autoware::rviz_plugins + +// Export the plugin +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(autoware::rviz_plugins::PlanningFactorRvizPlugin, rviz_common::Display) diff --git a/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.hpp new file mode 100644 index 0000000000000..f2af16e3e06b1 --- /dev/null +++ b/visualization/tier4_planning_factor_rviz_plugin/src/planning_factor_rviz_plugin.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 PLANNING_FACTOR_RVIZ_PLUGIN_HPP_ +#define PLANNING_FACTOR_RVIZ_PLUGIN_HPP_ + +#include +#include +#include +#include +#include + +#include + +#include + +namespace autoware::rviz_plugins +{ + +using RosTopicDisplay = rviz_common::RosTopicDisplay; + +class PlanningFactorRvizPlugin +: public rviz_common::RosTopicDisplay +{ +public: + PlanningFactorRvizPlugin() + : marker_common_{this}, + baselink2front_{"Baselink To Front", 0.0, "Length between base link to front.", this}, + topic_name_{"planning_factors"} + { + } + + void onInitialize() override + { + RosTopicDisplay::RTDClass::onInitialize(); + marker_common_.initialize(this->context_, this->scene_node_); + QString message_type = QString::fromStdString( + rosidl_generator_traits::name()); + this->topic_property_->setMessageType(message_type); + this->topic_property_->setValue(topic_name_.c_str()); + this->topic_property_->setDescription("Topic to subscribe to."); + + const auto vehicle_info = + autoware::vehicle_info_utils::VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()) + .getVehicleInfo(); + baselink2front_.setValue(vehicle_info.max_longitudinal_offset_m); + } + + void load(const rviz_common::Config & config) override + { + RosTopicDisplay::Display::load(config); + marker_common_.load(config); + } + + void update(float wall_dt, float ros_dt) override { marker_common_.update(wall_dt, ros_dt); } + + void reset() override + { + RosTopicDisplay::reset(); + marker_common_.clearMarkers(); + } + + void clear_markers() { marker_common_.clearMarkers(); } + + void add_marker(visualization_msgs::msg::Marker::ConstSharedPtr marker_ptr) + { + marker_common_.addMessage(marker_ptr); + } + + void add_marker(visualization_msgs::msg::MarkerArray::ConstSharedPtr markers_ptr) + { + marker_common_.addMessage(markers_ptr); + } + + void delete_marker(rviz_default_plugins::displays::MarkerID marker_id) + { + marker_common_.deleteMarker(marker_id); + } + +private: + void processMessage( + const tier4_planning_msgs::msg::PlanningFactorArray::ConstSharedPtr msg) override; + + rviz_default_plugins::displays::MarkerCommon marker_common_; + + rviz_common::properties::FloatProperty baselink2front_; + + std::string topic_name_; +}; +} // namespace autoware::rviz_plugins + +#endif // PLANNING_FACTOR_RVIZ_PLUGIN_HPP_ diff --git a/visualization/tier4_planning_rviz_plugin/CHANGELOG.rst b/visualization/tier4_planning_rviz_plugin/CHANGELOG.rst index 8faac0b6ac8e7..5d6e8fa8a819b 100644 --- a/visualization/tier4_planning_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_planning_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_planning_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_planning_rviz_plugin/package.xml b/visualization/tier4_planning_rviz_plugin/package.xml index d615878f91354..352e3bfbf5840 100644 --- a/visualization/tier4_planning_rviz_plugin/package.xml +++ b/visualization/tier4_planning_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_planning_rviz_plugin - 0.40.0 + 0.41.0 The tier4_planning_rviz_plugin package Yukihiro Saito Takayuki Murooka diff --git a/visualization/tier4_state_rviz_plugin/CHANGELOG.rst b/visualization/tier4_state_rviz_plugin/CHANGELOG.rst index 243d3c28733d7..bc1cc3050c97b 100644 --- a/visualization/tier4_state_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_state_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package tier4_state_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- +* Merge remote-tracking branch 'origin/main' into tmp/bot/bump_version_base +* fix(tier4_state_rviz_plugin): fix bugprone-integer-division (`#9628 `_) + fix: bugprone-integer-division +* Contributors: Fumiya Watanabe, kobayu858 + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_state_rviz_plugin/package.xml b/visualization/tier4_state_rviz_plugin/package.xml index f5f5fa8b9c0c1..7b1f08ceef6eb 100644 --- a/visualization/tier4_state_rviz_plugin/package.xml +++ b/visualization/tier4_state_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_state_rviz_plugin - 0.40.0 + 0.41.0 The autoware state rviz plugin package Hiroki OTA Takagi, Isamu diff --git a/visualization/tier4_state_rviz_plugin/src/custom_container.cpp b/visualization/tier4_state_rviz_plugin/src/custom_container.cpp index 681cebbefd492..5f0895ddae9e7 100644 --- a/visualization/tier4_state_rviz_plugin/src/custom_container.cpp +++ b/visualization/tier4_state_rviz_plugin/src/custom_container.cpp @@ -51,7 +51,7 @@ void CustomContainer::paintEvent(QPaintEvent *) // Draw background QPainterPath path; - path.addRoundedRect(rect(), height() / 2, height() / 2); // Use height for rounded corners + path.addRoundedRect(rect(), height() / 2.0, height() / 2.0); // Use height for rounded corners painter.setPen(Qt::NoPen); painter.setBrush(QColor( autoware::state_rviz_plugin::colors::default_colors.surface.c_str())); // Background color diff --git a/visualization/tier4_state_rviz_plugin/src/custom_icon_label.cpp b/visualization/tier4_state_rviz_plugin/src/custom_icon_label.cpp index 6e4d32d40f7fb..e01d2904ffbd2 100644 --- a/visualization/tier4_state_rviz_plugin/src/custom_icon_label.cpp +++ b/visualization/tier4_state_rviz_plugin/src/custom_icon_label.cpp @@ -65,7 +65,7 @@ void CustomIconLabel::paintEvent(QPaintEvent *) // Draw background circle QPainterPath path; - path.addEllipse(width() / 2 - radius, height() / 2 - radius, diameter, diameter); + path.addEllipse(width() / 2.0 - radius, height() / 2.0 - radius, diameter, diameter); painter.setPen(Qt::NoPen); painter.setBrush(backgroundColor); painter.drawPath(path); diff --git a/visualization/tier4_state_rviz_plugin/src/custom_segmented_button.cpp b/visualization/tier4_state_rviz_plugin/src/custom_segmented_button.cpp index 8063bdc0fbc28..87b4f7dba4653 100644 --- a/visualization/tier4_state_rviz_plugin/src/custom_segmented_button.cpp +++ b/visualization/tier4_state_rviz_plugin/src/custom_segmented_button.cpp @@ -66,7 +66,7 @@ void CustomSegmentedButton::paintEvent(QPaintEvent *) // Draw background QPainterPath path; - path.addRoundedRect(rect(), height() / 2, height() / 2); + path.addRoundedRect(rect(), height() / 2.0, height() / 2.0); painter.setPen(Qt::NoPen); painter.setBrush( diff --git a/visualization/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp b/visualization/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp index cdd3aa0d25263..192b1cbfa86f5 100644 --- a/visualization/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp +++ b/visualization/tier4_state_rviz_plugin/src/custom_segmented_button_item.cpp @@ -125,7 +125,7 @@ void CustomSegmentedButtonItem::paintEvent(QPaintEvent *) } QPainterPath path; - double radius = (height() - 2) / 2; + double radius = (height() - 2.0) / 2.0; path.setFillRule(Qt::WindingFill); if (isFirstButton) { diff --git a/visualization/tier4_system_rviz_plugin/CHANGELOG.rst b/visualization/tier4_system_rviz_plugin/CHANGELOG.rst index a895b7135e0c8..91792789b5d42 100644 --- a/visualization/tier4_system_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_system_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_system_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_system_rviz_plugin/package.xml b/visualization/tier4_system_rviz_plugin/package.xml index 5b768aa077120..9493e5b1c45a3 100644 --- a/visualization/tier4_system_rviz_plugin/package.xml +++ b/visualization/tier4_system_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_system_rviz_plugin - 0.40.0 + 0.41.0 The tier4_vehicle_rviz_plugin package Koji Minoda Apache License 2.0 diff --git a/visualization/tier4_traffic_light_rviz_plugin/CHANGELOG.rst b/visualization/tier4_traffic_light_rviz_plugin/CHANGELOG.rst index 72e4ec068910b..cb9a5e8225c04 100644 --- a/visualization/tier4_traffic_light_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_traffic_light_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_traffic_light_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_traffic_light_rviz_plugin/package.xml b/visualization/tier4_traffic_light_rviz_plugin/package.xml index b6645782290c1..3741d1ea0c46b 100644 --- a/visualization/tier4_traffic_light_rviz_plugin/package.xml +++ b/visualization/tier4_traffic_light_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_traffic_light_rviz_plugin - 0.40.0 + 0.41.0 The autoware state rviz plugin package Satoshi OTA Apache License 2.0 diff --git a/visualization/tier4_vehicle_rviz_plugin/CHANGELOG.rst b/visualization/tier4_vehicle_rviz_plugin/CHANGELOG.rst index 45d81a4646466..9deceeff4d0b6 100644 --- a/visualization/tier4_vehicle_rviz_plugin/CHANGELOG.rst +++ b/visualization/tier4_vehicle_rviz_plugin/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package tier4_vehicle_rviz_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.41.0 (2025-01-29) +------------------- + 0.40.0 (2024-12-12) ------------------- * Merge branch 'main' into release-0.40.0 diff --git a/visualization/tier4_vehicle_rviz_plugin/package.xml b/visualization/tier4_vehicle_rviz_plugin/package.xml index 4124ed2d514a3..91c34ec8ca5b4 100644 --- a/visualization/tier4_vehicle_rviz_plugin/package.xml +++ b/visualization/tier4_vehicle_rviz_plugin/package.xml @@ -2,7 +2,7 @@ tier4_vehicle_rviz_plugin - 0.40.0 + 0.41.0 The tier4_vehicle_rviz_plugin package Yukihiro Saito Apache License 2.0