diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 1ee1d82297baf..b241c8c45bb3e 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,9 +1,9 @@ ### Automatically generated from package.xml ### common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai +common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp common/autoware_point_types/** taichi.higashide@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp @@ -44,23 +44,23 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -control/lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp +control/autoware_external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/autoware_joy_controller/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +control/autoware_lane_departure_checker/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp control/mpc_lateral_controller/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/obstacle_collision_checker/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/operation_mode_transition_manager/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp -control/pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +control/autoware_pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/predicted_path_checker/** berkay@leodrive.ai control/pure_pursuit/** takamasa.horibe@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp -control/smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com +control/autoware_smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autoware_vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp evaluator/control_evaluator/** daniel.sanchez@tier4.jp takayuki.murooka@tier4.jp evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -144,23 +144,32 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp +planning/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_behavior_path_static_obstacle_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_path_optimizer/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp +planning/autoware_planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp -planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp +planning/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/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/autoware_behavior_path_planner_common/** daniel.sanchez@tier4.jp fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_sampling_planner_module/** daniel.sanchez@tier4.jp maxime.clement@tier4.jp planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_path_side_shift_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_start_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp @@ -169,43 +178,39 @@ planning/behavior_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/autoware_external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_common/** maxime.clement@tier4.jp +planning/motion_velocity_planner/autoware_motion_velocity_planner_node/** maxime.clement@tier4.jp planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp -planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp -planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp +planning/autoware_path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp -planning/autoware_planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp -planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp -planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp -planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp -planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp -planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/autoware_surround_obstacle_checker/** satoshi.ota@tier4.jp +planning/sampling_based_planner/autoware_bezier_sampler/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_frenet_planner/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_path_sampler/** maxime.clement@tier4.jp +planning/sampling_based_planner/autoware_sampler_common/** maxime.clement@tier4.jp +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_surround_obstacle_checker/** go.sakayori@tier4.jp satoshi.ota@tier4.jp sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp sensing/image_transport_decompressor/** kenzo.lobos@tier4.jp yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/livox/livox_tag_filter/** kenzo.lobos@tier4.jp ryohsuke.mitsudome@tier4.jp -sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp +sensing/pointcloud_preprocessor/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp david.wong@tier4.jp kenzo.lobos@tier4.jp kyoichi.sugahara@tier4.jp melike@leodrive.ai shunsuke.miura@tier4.jp yihsiang.fang@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp sensing/radar_scan_to_pointcloud2/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_static_pointcloud_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp sensing/radar_threshold_filter/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taekjin.lee@tier4.jp yoshi.ri@tier4.jp @@ -240,9 +245,10 @@ system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp tools/reaction_analyzer/** berkay@leodrive.ai vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp +vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp vehicle/autoware_steer_offset_estimator/** taiki.tanaka@tier4.jp -vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp +vehicle/autoware_vehicle_info_utils/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp ### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index fb98d321fde88..6745976aa543a 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -74,40 +74,3 @@ jobs: - name: Show disk space after the tasks run: df -h - - clang-tidy-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda - steps: - - name: Check out repository - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - - name: Show disk space before the tasks - run: df -h - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Get modified files - id: get-modified-files - uses: tj-actions/changed-files@v35 - with: - files: | - **/*.cpp - **/*.hpp - - - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 - with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy - build-depends-repos: build_depends.repos diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..26c5314d4b119 --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,53 @@ +name: clang-tidy-differential + +on: + pull_request: + types: + - opened + - synchronize + - labeled + +jobs: + prevent-no-label-execution: + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + with: + label: tag:run-clang-tidy-differential + + clang-tidy-differential: + needs: prevent-no-label-execution + if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} + runs-on: ubuntu-latest + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Show disk space before the tasks + run: df -h + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get modified files + id: get-modified-files + uses: tj-actions/changed-files@v35 + with: + files: | + **/*.cpp + **/*.hpp + + - name: Run clang-tidy + if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy + build-depends-repos: build_depends.repos diff --git a/common/global_parameter_loader/launch/global_params.launch.py b/common/global_parameter_loader/launch/global_params.launch.py index 06f807aaa4609..8f9092b10fa37 100644 --- a/common/global_parameter_loader/launch/global_params.launch.py +++ b/common/global_parameter_loader/launch/global_params.launch.py @@ -33,7 +33,7 @@ def launch_setup(context, *args, **kwargs): load_vehicle_info = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("vehicle_info_util"), "/launch/vehicle_info.launch.py"] + [FindPackageShare("autoware_vehicle_info_utils"), "/launch/vehicle_info.launch.py"] ), launch_arguments={ "vehicle_info_param_file": [vehicle_description_pkg, "/config/vehicle_info.param.yaml"] diff --git a/common/global_parameter_loader/package.xml b/common/global_parameter_loader/package.xml index 4c2568b9aec98..472ef0c063430 100644 --- a/common/global_parameter_loader/package.xml +++ b/common/global_parameter_loader/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + autoware_vehicle_info_utils ament_lint_auto autoware_lint_common diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp index b842261d56cfa..20b67ed13dfb5 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp @@ -27,6 +27,14 @@ namespace tier4_autoware_utils template class InterProcessPollingSubscriber { +public: + using SharedPtr = std::shared_ptr>; + static SharedPtr create_subscription( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) + { + return std::make_shared>(node, topic_name, qos); + } + private: typename rclcpp::Subscription::SharedPtr subscriber_; typename T::SharedPtr data_; diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp index ffc845575a76f..96b1cc6ea6ccc 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/transform_listener.hpp @@ -48,8 +48,9 @@ class TransformListener try { tf = tf_buffer_->lookupTransform(from, to, tf2::TimePointZero); } catch (tf2::TransformException & ex) { - RCLCPP_WARN( - logger_, "failed to get transform from %s to %s: %s", from.c_str(), to.c_str(), ex.what()); + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "failed to get transform from %s to %s: %s", from.c_str(), + to.c_str(), ex.what()); return {}; } @@ -64,8 +65,9 @@ class TransformListener try { tf = tf_buffer_->lookupTransform(from, to, time, duration); } catch (tf2::TransformException & ex) { - RCLCPP_WARN( - logger_, "failed to get transform from %s to %s: %s", from.c_str(), to.c_str(), ex.what()); + RCLCPP_WARN_THROTTLE( + logger_, *clock_, 5000, "failed to get transform from %s to %s: %s", from.c_str(), + to.c_str(), ex.what()); return {}; } diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index f2482b2d45a3b..1c1c4ad6d8ba1 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -11,6 +11,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils geometry_msgs libqt5-core libqt5-gui @@ -20,7 +21,6 @@ rviz_common rviz_default_plugins tf2_ros - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp index c4233bd6305e3..cd32df498bfd4 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -172,7 +172,7 @@ void PoseHistoryFootprint::updateFootprint() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_THROTTLE( diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp index d957054bd479e..44625ad7deb41 100644 --- a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp @@ -15,8 +15,8 @@ #ifndef POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ #define POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ +#include #include -#include #include @@ -39,8 +39,8 @@ class BoolProperty; namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfoUtils; class PoseHistoryFootprint : public rviz_common::MessageFilterDisplay diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index ed79e122c0a01..9cb710c239029 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -15,6 +15,7 @@ #ifndef TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ #define TIER4_PLANNING_RVIZ_PLUGIN__PATH__DISPLAY_BASE_HPP_ +#include #include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include @@ -97,8 +97,8 @@ bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfoUtils; template class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay { diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 5d755d212c859..95edcb7a96c56 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_planning_msgs + autoware_vehicle_info_utils libqt5-core libqt5-gui libqt5-widgets @@ -25,7 +26,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 4a297b47bcfcc..342e72d66c5f9 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -31,7 +31,7 @@ void AutowarePathWithLaneIdDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( @@ -107,7 +107,7 @@ void AutowarePathDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( @@ -124,7 +124,7 @@ void AutowareTrajectoryDisplay::preProcessMessageDetail() if (!vehicle_info_) { try { vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + VehicleInfoUtils(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); updateVehicleInfo(); } catch (const std::exception & e) { RCLCPP_WARN_ONCE( diff --git a/control/autonomous_emergency_braking/CMakeLists.txt b/control/autoware_autonomous_emergency_braking/CMakeLists.txt similarity index 92% rename from control/autonomous_emergency_braking/CMakeLists.txt rename to control/autoware_autonomous_emergency_braking/CMakeLists.txt index 53a629fafa0cc..7f38e4387b452 100644 --- a/control/autonomous_emergency_braking/CMakeLists.txt +++ b/control/autoware_autonomous_emergency_braking/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autonomous_emergency_braking) +project(autoware_autonomous_emergency_braking) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/control/autonomous_emergency_braking/README.md b/control/autoware_autonomous_emergency_braking/README.md similarity index 100% rename from control/autonomous_emergency_braking/README.md rename to control/autoware_autonomous_emergency_braking/README.md diff --git a/control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml b/control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml similarity index 100% rename from control/autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml rename to control/autoware_autonomous_emergency_braking/config/autonomous_emergency_braking.param.yaml diff --git a/control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/obstacle_filtering_1.drawio.svg diff --git a/control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg b/control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/obstacle_filtering_2.drawio.svg diff --git a/control/autonomous_emergency_braking/image/range.drawio.svg b/control/autoware_autonomous_emergency_braking/image/range.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/range.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/range.drawio.svg diff --git a/control/autonomous_emergency_braking/image/rss_check.drawio.svg b/control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg similarity index 100% rename from control/autonomous_emergency_braking/image/rss_check.drawio.svg rename to control/autoware_autonomous_emergency_braking/image/rss_check.drawio.svg diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp similarity index 97% rename from control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp rename to control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp index 81ab2ecff790b..3e72d72cb9946 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autoware_autonomous_emergency_braking/include/autoware_autonomous_emergency_braking/node.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ -#define AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#ifndef AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#define AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#include #include #include #include #include #include #include -#include #include #include @@ -59,11 +59,11 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; using PointCloud = pcl::PointCloud; +using autoware::vehicle_info_utils::VehicleInfo; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Path = std::vector; @@ -340,4 +340,4 @@ class AEB : public rclcpp::Node }; } // namespace autoware::motion::control::autonomous_emergency_braking -#endif // AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ +#endif // AUTOWARE_AUTONOMOUS_EMERGENCY_BRAKING__NODE_HPP_ diff --git a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml similarity index 74% rename from control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml rename to control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml index cf6640ec6be52..75b1a9dcf822d 100644 --- a/control/autonomous_emergency_braking/launch/autonomous_emergency_braking.launch.xml +++ b/control/autoware_autonomous_emergency_braking/launch/autoware_autonomous_emergency_braking.launch.xml @@ -1,12 +1,12 @@ - + - + diff --git a/control/autonomous_emergency_braking/package.xml b/control/autoware_autonomous_emergency_braking/package.xml similarity index 94% rename from control/autonomous_emergency_braking/package.xml rename to control/autoware_autonomous_emergency_braking/package.xml index bf27e6b7e1575..0404eec7ca308 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autoware_autonomous_emergency_braking/package.xml @@ -1,7 +1,7 @@ - autonomous_emergency_braking + autoware_autonomous_emergency_braking 0.1.0 Autonomous Emergency Braking package as a ROS 2 node Takamasa Horibe @@ -17,6 +17,7 @@ autoware_control_msgs autoware_planning_msgs autoware_system_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs @@ -34,7 +35,6 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autoware_autonomous_emergency_braking/src/node.cpp similarity index 99% rename from control/autonomous_emergency_braking/src/node.cpp rename to control/autoware_autonomous_emergency_braking/src/node.cpp index 71eb92a4e95fb..d01855bc8ab5d 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autoware_autonomous_emergency_braking/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autonomous_emergency_braking/node.hpp" +#include "autoware_autonomous_emergency_braking/node.hpp" #include #include @@ -60,7 +60,7 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d createPolygon( const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & next_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) { Polygon2d polygon; @@ -102,7 +102,7 @@ Polygon2d createPolygon( AEB::AEB(const rclcpp::NodeOptions & node_options) : Node("AEB", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), collision_data_keeper_(this->get_clock()) { // Publisher diff --git a/control/autoware_external_cmd_selector/CMakeLists.txt b/control/autoware_external_cmd_selector/CMakeLists.txt new file mode 100644 index 0000000000000..4dccabcd85d8e --- /dev/null +++ b/control/autoware_external_cmd_selector/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_external_cmd_selector) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME}_node SHARED + src/autoware_external_cmd_selector/external_cmd_selector_node.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware::external_cmd_selector::ExternalCmdSelector" + EXECUTABLE autoware_external_cmd_selector +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/control/external_cmd_selector/README.md b/control/autoware_external_cmd_selector/README.md similarity index 91% rename from control/external_cmd_selector/README.md rename to control/autoware_external_cmd_selector/README.md index ca0f4f0954dbf..0495004b68b84 100644 --- a/control/external_cmd_selector/README.md +++ b/control/autoware_external_cmd_selector/README.md @@ -1,8 +1,8 @@ -# external_cmd_selector +# autoware_external_cmd_selector ## Purpose -`external_cmd_selector` is the package to publish `external_control_cmd`, `gear_cmd`, `hazard_lights_cmd`, `heartbeat` and `turn_indicators_cmd`, according to the current mode, which is `remote` or `local`. +`autoware_external_cmd_selector` is the package to publish `external_control_cmd`, `gear_cmd`, `hazard_lights_cmd`, `heartbeat` and `turn_indicators_cmd`, according to the current mode, which is `remote` or `local`. The current mode is set via service, `remote` is remotely operated, `local` is to use the values calculated by Autoware. diff --git a/control/external_cmd_selector/config/external_cmd_selector.param.yaml b/control/autoware_external_cmd_selector/config/external_cmd_selector.param.yaml similarity index 100% rename from control/external_cmd_selector/config/external_cmd_selector.param.yaml rename to control/autoware_external_cmd_selector/config/external_cmd_selector.param.yaml diff --git a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp b/control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp similarity index 93% rename from control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp rename to control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp index 6a4fb897b57bc..1517ca83ad1f0 100644 --- a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp +++ b/control/autoware_external_cmd_selector/include/autoware_external_cmd_selector/external_cmd_selector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ -#define EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +#ifndef AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +#define AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ #include #include @@ -30,7 +30,8 @@ #include #include - +namespace autoware::external_cmd_selector +{ class ExternalCmdSelector : public rclcpp::Node { public: @@ -101,5 +102,5 @@ class ExternalCmdSelector : public rclcpp::Node // Diagnostics Updater diagnostic_updater::Updater updater_{this}; }; - -#endif // EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +} // namespace autoware::external_cmd_selector +#endif // AUTOWARE_EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.py b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py similarity index 97% rename from control/external_cmd_selector/launch/external_cmd_selector.launch.py rename to control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py index 358cc135996f7..95673502e2b51 100644 --- a/control/external_cmd_selector/launch/external_cmd_selector.launch.py +++ b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.py @@ -32,8 +32,8 @@ def launch_setup(context, *args, **kwargs): external_cmd_selector_param = yaml.safe_load(f)["/**"]["ros__parameters"] component = ComposableNode( - package="external_cmd_selector", - plugin="ExternalCmdSelector", + package="autoware_external_cmd_selector", + plugin="autoware::external_cmd_selector::ExternalCmdSelector", name="external_cmd_selector", remappings=[ _create_mapping_tuple("service/select_external_command"), diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.xml b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml similarity index 95% rename from control/external_cmd_selector/launch/external_cmd_selector.launch.xml rename to control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml index cb373802e5183..7e8b32ac68d8d 100644 --- a/control/external_cmd_selector/launch/external_cmd_selector.launch.xml +++ b/control/autoware_external_cmd_selector/launch/external_cmd_selector.launch.xml @@ -26,7 +26,7 @@ - + diff --git a/control/external_cmd_selector/package.xml b/control/autoware_external_cmd_selector/package.xml similarity index 92% rename from control/external_cmd_selector/package.xml rename to control/autoware_external_cmd_selector/package.xml index b3520839e3b51..19881f06c96e5 100644 --- a/control/external_cmd_selector/package.xml +++ b/control/autoware_external_cmd_selector/package.xml @@ -1,9 +1,9 @@ - external_cmd_selector + autoware_external_cmd_selector 0.1.0 - The external_cmd_selector package + The autoware_external_cmd_selector package Taiki Tanaka Tomoya Kimura Shumpei Wakabayashi diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp similarity index 96% rename from control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp rename to control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp index 0039ec9cd1547..59a8caa5c1568 100644 --- a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp +++ b/control/autoware_external_cmd_selector/src/autoware_external_cmd_selector/external_cmd_selector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "external_cmd_selector/external_cmd_selector_node.hpp" +#include "autoware_external_cmd_selector/external_cmd_selector_node.hpp" #include @@ -21,6 +21,8 @@ #include #include +namespace autoware::external_cmd_selector +{ ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_options) : Node("external_cmd_selector", node_options) { @@ -202,6 +204,7 @@ ExternalCmdSelector::InternalHeartbeat ExternalCmdSelector::convert( { return command; } +} // namespace autoware::external_cmd_selector #include -RCLCPP_COMPONENTS_REGISTER_NODE(ExternalCmdSelector) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::external_cmd_selector::ExternalCmdSelector) diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt new file mode 100644 index 0000000000000..9c7d8b3ad475a --- /dev/null +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_joy_controller) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_joy_controller_node SHARED + DIRECTORY src +) + +rclcpp_components_register_node(autoware_joy_controller_node + PLUGIN "autoware::joy_controller::AutowareJoyControllerNode" + EXECUTABLE autoware_joy_controller +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/joy_controller/README.md b/control/autoware_joy_controller/README.md similarity index 94% rename from control/joy_controller/README.md rename to control/autoware_joy_controller/README.md index 73e3644df49ac..a6919356f3dbc 100644 --- a/control/joy_controller/README.md +++ b/control/autoware_joy_controller/README.md @@ -1,8 +1,8 @@ -# joy_controller +# autoware_joy_controller ## Role -`joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. +`autoware_joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. ## Usage @@ -10,13 +10,13 @@ ```bash # With default config (ds4) -ros2 launch joy_controller joy_controller.launch.xml +ros2 launch autoware_joy_controller joy_controller.launch.xml # Default config but select from the existing parameter files -ros2 launch joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox +ros2 launch autoware_joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox # Override the param file -ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml +ros2 launch autoware_joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml ``` ## Input / Output diff --git a/control/joy_controller/config/joy_controller_ds4.param.yaml b/control/autoware_joy_controller/config/joy_controller_ds4.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_ds4.param.yaml rename to control/autoware_joy_controller/config/joy_controller_ds4.param.yaml diff --git a/control/joy_controller/config/joy_controller_g29.param.yaml b/control/autoware_joy_controller/config/joy_controller_g29.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_g29.param.yaml rename to control/autoware_joy_controller/config/joy_controller_g29.param.yaml diff --git a/control/joy_controller/config/joy_controller_p65.param.yaml b/control/autoware_joy_controller/config/joy_controller_p65.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_p65.param.yaml rename to control/autoware_joy_controller/config/joy_controller_p65.param.yaml diff --git a/control/joy_controller/config/joy_controller_xbox.param.yaml b/control/autoware_joy_controller/config/joy_controller_xbox.param.yaml similarity index 100% rename from control/joy_controller/config/joy_controller_xbox.param.yaml rename to control/autoware_joy_controller/config/joy_controller_xbox.param.yaml diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp similarity index 86% rename from control/joy_controller/include/joy_controller/joy_controller.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp index e88f7bcb3904e..88371455def62 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_controller.hpp @@ -12,12 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#define JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include +#include #include #include @@ -36,7 +37,7 @@ #include #include -namespace joy_controller +namespace autoware::joy_controller { using GearShiftType = tier4_external_api_msgs::msg::GearShift::_data_type; using TurnSignalType = tier4_external_api_msgs::msg::TurnSignal::_data_type; @@ -66,19 +67,20 @@ class AutowareJoyControllerNode : public rclcpp::Node double backward_accel_ratio_; // CallbackGroups - rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; rclcpp::CallbackGroup::SharedPtr callback_group_services_; // Subscriber - rclcpp::Subscription::SharedPtr sub_joy_; - rclcpp::Subscription::SharedPtr sub_odom_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_joy_{ + this, "input/joy"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "input/odometry"}; rclcpp::Time last_joy_received_time_; std::shared_ptr joy_; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; - void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + void onJoy(); + void onOdometry(); // Publisher rclcpp::Publisher::SharedPtr pub_control_command_; @@ -118,6 +120,6 @@ class AutowareJoyControllerNode : public rclcpp::Node bool isDataReady(); void onTimer(); }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONTROLLER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONTROLLER_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp similarity index 89% rename from control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp index faa920d493478..7cfac1af8662b 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/ds4_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class DS4JoyConverter : public JoyConverterBase { @@ -90,6 +90,6 @@ class DS4JoyConverter : public JoyConverterBase bool reverse() const { return Share(); } }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp similarity index 88% rename from control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp index 7ba062fe10d19..a36ad3a580287 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/g29_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" -namespace joy_controller +namespace autoware::joy_controller { class G29JoyConverter : public JoyConverterBase { @@ -88,6 +88,6 @@ class G29JoyConverter : public JoyConverterBase bool reverse() const { return Share(); } }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp similarity index 82% rename from control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp index 94587b88e22f5..98a8b799d71a9 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/joy_converter_base.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ #include #include -namespace joy_controller +namespace autoware::joy_controller { class JoyConverterBase { @@ -50,6 +50,6 @@ class JoyConverterBase virtual bool vehicle_engage() const = 0; virtual bool vehicle_disengage() const = 0; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp similarity index 88% rename from control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp index e4cb822846fef..a6f51cb95b44d 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/p65_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class P65JoyConverter : public JoyConverterBase { @@ -76,6 +76,6 @@ class P65JoyConverter : public JoyConverterBase const sensor_msgs::msg::Joy j_; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp similarity index 88% rename from control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp rename to control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp index a009ee1e12975..776b0c98b7835 100644 --- a/control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/xbox_joy_converter.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ -#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#ifndef AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ -#include "joy_controller/joy_converter/joy_converter_base.hpp" +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" #include -namespace joy_controller +namespace autoware::joy_controller { class XBOXJoyConverter : public JoyConverterBase { @@ -76,6 +76,6 @@ class XBOXJoyConverter : public JoyConverterBase const sensor_msgs::msg::Joy j_; }; -} // namespace joy_controller +} // namespace autoware::joy_controller -#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_ diff --git a/control/joy_controller/launch/joy_controller.launch.xml b/control/autoware_joy_controller/launch/joy_controller.launch.xml similarity index 89% rename from control/joy_controller/launch/joy_controller.launch.xml rename to control/autoware_joy_controller/launch/joy_controller.launch.xml index 5236077680d0d..02220f0026cea 100644 --- a/control/joy_controller/launch/joy_controller.launch.xml +++ b/control/autoware_joy_controller/launch/joy_controller.launch.xml @@ -12,9 +12,9 @@ - + - + diff --git a/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml b/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml new file mode 100644 index 0000000000000..3d0aae9a45c4a --- /dev/null +++ b/control/autoware_joy_controller/launch/joy_controller_param_selection.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/control/joy_controller/package.xml b/control/autoware_joy_controller/package.xml similarity index 90% rename from control/joy_controller/package.xml rename to control/autoware_joy_controller/package.xml index f7a5ed805b8ad..10e33d5d6564a 100644 --- a/control/joy_controller/package.xml +++ b/control/autoware_joy_controller/package.xml @@ -1,9 +1,9 @@ - joy_controller + autoware_joy_controller 0.1.0 - The joy_controller package + The autoware_joy_controller package Taiki Tanaka Tomoya Kimura Shumpei Wakabayashi @@ -26,6 +26,7 @@ sensor_msgs std_srvs tier4_api_utils + tier4_autoware_utils tier4_control_msgs tier4_external_api_msgs diff --git a/control/joy_controller/schema/joy_controller.schema.json b/control/autoware_joy_controller/schema/joy_controller.schema.json similarity index 97% rename from control/joy_controller/schema/joy_controller.schema.json rename to control/autoware_joy_controller/schema/joy_controller.schema.json index d4c6351324935..4c95057aec46d 100644 --- a/control/joy_controller/schema/joy_controller.schema.json +++ b/control/autoware_joy_controller/schema/joy_controller.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Joy Controller node", "type": "object", "definitions": { - "joy_controller": { + "autoware_joy_controller": { "type": "object", "properties": { "joy_type": { @@ -112,7 +112,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/joy_controller" + "$ref": "#/definitions/autoware_joy_controller" } }, "required": ["ros__parameters"], diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/autoware_joy_controller/src/joy_controller_node.cpp similarity index 91% rename from control/joy_controller/src/joy_controller/joy_controller_node.cpp rename to control/autoware_joy_controller/src/joy_controller_node.cpp index 39198825f9c4e..2491893a2806c 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/autoware_joy_controller/src/joy_controller_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "joy_controller/joy_controller.hpp" -#include "joy_controller/joy_converter/ds4_joy_converter.hpp" -#include "joy_controller/joy_converter/g29_joy_converter.hpp" -#include "joy_controller/joy_converter/p65_joy_converter.hpp" -#include "joy_controller/joy_converter/xbox_joy_converter.hpp" +#include "autoware_joy_controller/joy_controller.hpp" +#include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/p65_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/xbox_joy_converter.hpp" #include @@ -27,9 +27,9 @@ namespace { -using joy_controller::GateModeType; -using joy_controller::GearShiftType; -using joy_controller::TurnSignalType; +using autoware::joy_controller::GateModeType; +using autoware::joy_controller::GearShiftType; +using autoware::joy_controller::TurnSignalType; using GearShift = tier4_external_api_msgs::msg::GearShift; using TurnSignal = tier4_external_api_msgs::msg::TurnSignal; using GateMode = tier4_control_msgs::msg::GateMode; @@ -146,10 +146,11 @@ double calcMapping(const double input, const double sensitivity) } // namespace -namespace joy_controller +namespace autoware::joy_controller { -void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) +void AutowareJoyControllerNode::onJoy() { + const auto msg = sub_joy_.takeData(); last_joy_received_time_ = msg->header.stamp; if (joy_type_ == "G29") { joy_ = std::make_shared(*msg); @@ -190,8 +191,13 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt } } -void AutowareJoyControllerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void AutowareJoyControllerNode::onOdometry() { + if (raw_control_) { + return; + } + + const auto msg = sub_odom_.takeData(); auto twist = std::make_shared(); twist->header = msg->header; twist->twist = msg->twist.twist; @@ -243,6 +249,9 @@ bool AutowareJoyControllerNode::isDataReady() void AutowareJoyControllerNode::onTimer() { + onOdometry(); + onJoy(); + if (!isDataReady()) { return; } @@ -450,7 +459,7 @@ void AutowareJoyControllerNode::initTimer(double period_s) } AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options) -: Node("joy_controller", node_options) +: Node("autoware_joy_controller", node_options) { // Parameter joy_type_ = declare_parameter("joy_type"); @@ -470,23 +479,11 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); // Callback Groups - callback_group_subscribers_ = - this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); callback_group_services_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - auto subscriber_option = rclcpp::SubscriptionOptions(); - subscriber_option.callback_group = callback_group_subscribers_; // Subscriber - sub_joy_ = this->create_subscription( - "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), - subscriber_option); - if (!raw_control_) { - sub_odom_ = this->create_subscription( - "input/odometry", 1, - std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), - subscriber_option); - } else { + if (raw_control_) { twist_ = std::make_shared(); } @@ -524,7 +521,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & // Timer initTimer(1.0 / update_rate_); } -} // namespace joy_controller +} // namespace autoware::joy_controller #include -RCLCPP_COMPONENTS_REGISTER_NODE(joy_controller::AutowareJoyControllerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::joy_controller::AutowareJoyControllerNode) diff --git a/control/mpc_lateral_controller/CMakeLists.txt b/control/autoware_mpc_lateral_controller/CMakeLists.txt similarity index 96% rename from control/mpc_lateral_controller/CMakeLists.txt rename to control/autoware_mpc_lateral_controller/CMakeLists.txt index e4a3683ea1fdc..dff85d70419a1 100644 --- a/control/mpc_lateral_controller/CMakeLists.txt +++ b/control/autoware_mpc_lateral_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(mpc_lateral_controller) +project(autoware_mpc_lateral_controller) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/control/mpc_lateral_controller/README.md b/control/autoware_mpc_lateral_controller/README.md similarity index 100% rename from control/mpc_lateral_controller/README.md rename to control/autoware_mpc_lateral_controller/README.md diff --git a/control/mpc_lateral_controller/image/vehicle_error_kinematics.png b/control/autoware_mpc_lateral_controller/image/vehicle_error_kinematics.png similarity index 100% rename from control/mpc_lateral_controller/image/vehicle_error_kinematics.png rename to control/autoware_mpc_lateral_controller/image/vehicle_error_kinematics.png diff --git a/control/mpc_lateral_controller/image/vehicle_kinematics.png b/control/autoware_mpc_lateral_controller/image/vehicle_kinematics.png similarity index 100% rename from control/mpc_lateral_controller/image/vehicle_kinematics.png rename to control/autoware_mpc_lateral_controller/image/vehicle_kinematics.png diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp similarity index 95% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp index 227bd0594312b..73cef7cddda39 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/lowpass_filter.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/lowpass_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ #include #include @@ -102,4 +102,4 @@ namespace MoveAverageFilter bool filt_vector(const int num, std::vector & u); } // namespace MoveAverageFilter } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp similarity index 97% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp index 91b803dea36fa..03abae66e4986 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_HPP_ - -#include "mpc_lateral_controller/lowpass_filter.hpp" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" -#include "mpc_lateral_controller/steering_predictor.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ + +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/steering_predictor.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" @@ -525,4 +525,4 @@ class MPC }; // class MPC } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp similarity index 95% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp index eb1d75f9508b3..e0775e1d09def 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_lateral_controller.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ -#include "mpc_lateral_controller/mpc.hpp" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" -#include "mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" +#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp" #include "rclcpp/rclcpp.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" @@ -281,4 +281,4 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_LATERAL_CONTROLLER_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp similarity index 95% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp index eb624f39ae76b..4a0ae16bc9b1c 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_trajectory.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -125,4 +125,4 @@ class MPCTrajectory } }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_TRAJECTORY_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp similarity index 97% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp index 819d7fb89b8a7..e97a004438e4b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/mpc_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ -#define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" @@ -26,7 +26,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #endif -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -230,4 +230,4 @@ void update_param( } // namespace MPCUtils } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp similarity index 89% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp index ca30bd30e4dd1..6447437d6c274 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ #include @@ -51,4 +51,4 @@ class QPSolverInterface virtual double getObjVal() const { return 0.0; } }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp similarity index 88% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp index 2611f94da324f..8c81092960097 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" #include "osqp_interface/osqp_interface.hpp" #include "rclcpp/rclcpp.hpp" @@ -62,4 +62,4 @@ class QPSolverOSQP : public QPSolverInterface rclcpp::Logger logger_; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp similarity index 87% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp index ef337eaaa8528..15c8137b78890 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#define MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ -#include "mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { @@ -62,4 +62,4 @@ class QPSolverEigenLeastSquareLLT : public QPSolverInterface private: }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp similarity index 84% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp index 494961ef679d2..707c1ce56a9a6 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_offset/steering_offset.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ -#define MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ #include @@ -45,4 +45,4 @@ class SteeringOffsetEstimator double steering_offset_ = 0.0; }; -#endif // MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_OFFSET__STEERING_OFFSET_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp index 16e9b165fb1a5..0c5f04497d058 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/steering_predictor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ -#define MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ #include "rclcpp/rclcpp.hpp" @@ -81,4 +81,4 @@ class SteeringPredictor } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__STEERING_PREDICTOR_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp similarity index 92% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp index 488a3c7ab8be2..20787b1f281b5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp @@ -44,10 +44,10 @@ * Tracking", Robotics Institute, Carnegie Mellon University, February 2009. */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -122,4 +122,4 @@ class DynamicsBicycleModel : public VehicleModelInterface double m_cr; //!< @brief rear cornering power [N/rad] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_DYNAMICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp similarity index 90% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp index e2f66e95ab0e3..91d3ea826c7d3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp @@ -38,10 +38,10 @@ * */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -106,4 +106,4 @@ class KinematicsBicycleModel : public VehicleModelInterface double m_steer_tau; //!< @brief steering time constant for 1d-model [s] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_HPP_ diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp similarity index 87% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp index b503ad8eb1d13..5731a2025d833 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp @@ -35,10 +35,10 @@ * */ -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include #include @@ -101,4 +101,6 @@ class KinematicsBicycleModelNoDelay : public VehicleModelInterface double m_steer_lim; //!< @brief steering angle limit [rad] }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ +// clang-format off +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_BICYCLE_KINEMATICS_NO_DELAY_HPP_ // NOLINT +// clang-format on diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp similarity index 93% rename from control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp rename to control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp index 68806ff5a00dc..9e2d14184d5e7 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp +++ b/control/autoware_mpc_lateral_controller/include/autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#define MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#ifndef AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#define AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" #include @@ -149,4 +149,4 @@ class VehicleModelInterface const MPCTrajectory & reference_trajectory, const double dt) const = 0; }; } // namespace autoware::motion::control::mpc_lateral_controller -#endif // MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ +#endif // AUTOWARE_MPC_LATERAL_CONTROLLER__VEHICLE_MODEL__VEHICLE_MODEL_INTERFACE_HPP_ diff --git a/control/mpc_lateral_controller/model_predictive_control_algorithm.md b/control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md similarity index 100% rename from control/mpc_lateral_controller/model_predictive_control_algorithm.md rename to control/autoware_mpc_lateral_controller/model_predictive_control_algorithm.md diff --git a/control/mpc_lateral_controller/package.xml b/control/autoware_mpc_lateral_controller/package.xml similarity index 94% rename from control/mpc_lateral_controller/package.xml rename to control/autoware_mpc_lateral_controller/package.xml index 000bddc65aa1f..673d9d7896745 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/autoware_mpc_lateral_controller/package.xml @@ -1,7 +1,7 @@ - mpc_lateral_controller + autoware_mpc_lateral_controller 1.0.0 MPC-based lateral controller @@ -19,6 +19,7 @@ autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diagnostic_updater @@ -35,7 +36,6 @@ tier4_autoware_utils tier4_debug_msgs trajectory_follower_base - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml similarity index 100% rename from control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml rename to control/autoware_mpc_lateral_controller/param/lateral_controller_defaults.param.yaml diff --git a/control/mpc_lateral_controller/src/lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp similarity index 98% rename from control/mpc_lateral_controller/src/lowpass_filter.cpp rename to control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp index 8fbf3488c5a2e..e3bbe78e2428b 100644 --- a/control/mpc_lateral_controller/src/lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/src/lowpass_filter.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/lowpass_filter.hpp" +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp similarity index 99% rename from control/mpc_lateral_controller/src/mpc.cpp rename to control/autoware_mpc_lateral_controller/src/mpc.cpp index 177c1e0854bfb..e6fa2ce3c0102 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp similarity index 97% rename from control/mpc_lateral_controller/src/mpc_lateral_controller.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp index 85d33a5e9f1c0..cf8a4ed5a251b 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_lateral_controller.hpp" - +#include "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp" + +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "tf2/utils.h" #include "tf2_ros/create_timer_ros.h" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -69,7 +69,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps"); // [rad/s] /* mpc parameters */ - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; constexpr double deg2rad = static_cast(M_PI) / 180.0; m_mpc->m_steer_lim = vehicle_info.max_steer_angle_rad; diff --git a/control/mpc_lateral_controller/src/mpc_trajectory.cpp b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp similarity index 97% rename from control/mpc_lateral_controller/src/mpc_trajectory.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp index b7fed942375e8..e84e6cf79a25a 100644 --- a/control/mpc_lateral_controller/src/mpc_trajectory.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_trajectory.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp similarity index 99% rename from control/mpc_lateral_controller/src/mpc_utils.cpp rename to control/autoware_mpc_lateral_controller/src/mpc_utils.cpp index 01a81d9015b47..1a0062372eb22 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/mpc_utils.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp similarity index 97% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp rename to control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp index 7c76d67e75aa3..66981b0c5c6d4 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp +++ b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_osqp.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp similarity index 93% rename from control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp rename to control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp index b0357138cd646..315e29d063aad 100644 --- a/control/mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp +++ b/control/autoware_mpc_lateral_controller/src/qp_solver/qp_solver_unconstraint_fast.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" #include diff --git a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp similarity index 95% rename from control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp rename to control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp index 60d4dd7901394..6425d5da5e941 100644 --- a/control/mpc_lateral_controller/src/steering_offset/steering_offset.cpp +++ b/control/autoware_mpc_lateral_controller/src/steering_offset/steering_offset.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/steering_offset/steering_offset.hpp" +#include "autoware_mpc_lateral_controller/steering_offset/steering_offset.hpp" #include #include diff --git a/control/mpc_lateral_controller/src/steering_predictor.cpp b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp similarity index 98% rename from control/mpc_lateral_controller/src/steering_predictor.cpp rename to control/autoware_mpc_lateral_controller/src/steering_predictor.cpp index 48d8fa8c47a97..759a36a16de62 100644 --- a/control/mpc_lateral_controller/src/steering_predictor.cpp +++ b/control/autoware_mpc_lateral_controller/src/steering_predictor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/steering_predictor.hpp" +#include "autoware_mpc_lateral_controller/steering_predictor.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp index 2467b1f0c2311..d5272c48fcbdf 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_dynamics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" #include diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp index cd52dd4f79314..97bcafb161cb0 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" #include diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp similarity index 98% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp index 8f4510510aca9..d2857d8b42182 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include diff --git a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp similarity index 94% rename from control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp rename to control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp index f54a5e325bead..e1b17d3d96278 100644 --- a/control/mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp +++ b/control/autoware_mpc_lateral_controller/src/vehicle_model/vehicle_model_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" namespace autoware::motion::control::mpc_lateral_controller { diff --git a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp similarity index 98% rename from control/mpc_lateral_controller/test/test_lowpass_filter.cpp rename to control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp index c68513586847b..bfb695cdeee8a 100644 --- a/control/mpc_lateral_controller/test/test_lowpass_filter.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_lowpass_filter.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/lowpass_filter.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/lowpass_filter.hpp" #include diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp similarity index 97% rename from control/mpc_lateral_controller/test/test_mpc.cpp rename to control/autoware_mpc_lateral_controller/test/test_mpc.cpp index 7644fb28b0788..3fd4b010852c0 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/mpc.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" +#include "autoware_mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" +#include "autoware_mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/mpc.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp" -#include "mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp" -#include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "rclcpp/rclcpp.hpp" #include "autoware_control_msgs/msg/lateral.hpp" diff --git a/control/mpc_lateral_controller/test/test_mpc_utils.cpp b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp similarity index 95% rename from control/mpc_lateral_controller/test/test_mpc_utils.cpp rename to control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp index 51cc7d55e4560..b55435dffd024 100644 --- a/control/mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/autoware_mpc_lateral_controller/test/test_mpc_utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_mpc_lateral_controller/mpc_trajectory.hpp" +#include "autoware_mpc_lateral_controller/mpc_utils.hpp" #include "gtest/gtest.h" -#include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "mpc_lateral_controller/mpc_utils.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" diff --git a/control/pid_longitudinal_controller/CMakeLists.txt b/control/autoware_pid_longitudinal_controller/CMakeLists.txt similarity index 80% rename from control/pid_longitudinal_controller/CMakeLists.txt rename to control/autoware_pid_longitudinal_controller/CMakeLists.txt index 3e7a992b15e00..f0dce81eb54e1 100644 --- a/control/pid_longitudinal_controller/CMakeLists.txt +++ b/control/autoware_pid_longitudinal_controller/CMakeLists.txt @@ -1,15 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(pid_longitudinal_controller) +project(autoware_pid_longitudinal_controller) find_package(autoware_cmake REQUIRED) autoware_package() set(PID_LON_CON_LIB ${PROJECT_NAME}_lib) ament_auto_add_library(${PID_LON_CON_LIB} SHARED - src/pid_longitudinal_controller.cpp - src/pid.cpp - src/smooth_stop.cpp - src/longitudinal_controller_utils.cpp + DIRECTORY src ) if(BUILD_TESTING) diff --git a/control/pid_longitudinal_controller/README.md b/control/autoware_pid_longitudinal_controller/README.md similarity index 100% rename from control/pid_longitudinal_controller/README.md rename to control/autoware_pid_longitudinal_controller/README.md diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp similarity index 92% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp index 409af46b16ed1..bd682f9566134 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/debug_values.hpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ #include +#include namespace autoware::motion::control::pid_longitudinal_controller { @@ -92,4 +93,4 @@ class DebugValues }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__DEBUG_VALUES_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp similarity index 96% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp index 4ea844a140f4f..171571b493774 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spherical_linear_interpolation.hpp" @@ -155,4 +155,4 @@ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp similarity index 89% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp index 06fe1793c8123..a743be573cf60 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/lowpass_filter.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/lowpass_filter.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ #include #include @@ -63,4 +63,4 @@ class LowpassFilter1d } }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp similarity index 94% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp index 8b981c0485ed9..8323e35b5da27 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__PID_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ #include @@ -91,4 +91,4 @@ class PIDController }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__PID_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp similarity index 95% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp index dee2e580e6911..08a6a51d3098b 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ - +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ + +#include "autoware_pid_longitudinal_controller/debug_values.hpp" +#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "autoware_pid_longitudinal_controller/lowpass_filter.hpp" +#include "autoware_pid_longitudinal_controller/pid.hpp" +#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "diagnostic_updater/diagnostic_updater.hpp" -#include "pid_longitudinal_controller/debug_values.hpp" -#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" -#include "pid_longitudinal_controller/lowpass_filter.hpp" -#include "pid_longitudinal_controller/pid.hpp" -#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/ros/marker_helper.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -408,4 +408,4 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__PID_LONGITUDINAL_CONTROLLER_HPP_ diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp similarity index 95% rename from control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp rename to control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp index 9a0a36123e143..e433fd158026b 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/smooth_stop.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware_pid_longitudinal_controller/smooth_stop.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ -#define PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#ifndef AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#define AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ #include "rclcpp/rclcpp.hpp" @@ -112,4 +112,4 @@ class SmoothStop }; } // namespace autoware::motion::control::pid_longitudinal_controller -#endif // PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ +#endif // AUTOWARE_PID_LONGITUDINAL_CONTROLLER__SMOOTH_STOP_HPP_ diff --git a/control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg b/control/autoware_pid_longitudinal_controller/media/BrakeKeeping.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/BrakeKeeping.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/BrakeKeeping.drawio.svg diff --git a/control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/LongitudinalControllerDiagram.drawio.svg diff --git a/control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg diff --git a/control/pid_longitudinal_controller/media/slope_definition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/slope_definition.drawio.svg similarity index 100% rename from control/pid_longitudinal_controller/media/slope_definition.drawio.svg rename to control/autoware_pid_longitudinal_controller/media/slope_definition.drawio.svg diff --git a/control/pid_longitudinal_controller/package.xml b/control/autoware_pid_longitudinal_controller/package.xml similarity index 94% rename from control/pid_longitudinal_controller/package.xml rename to control/autoware_pid_longitudinal_controller/package.xml index 82b297a69fee6..66f6a6dfcda8b 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/autoware_pid_longitudinal_controller/package.xml @@ -1,7 +1,7 @@ - pid_longitudinal_controller + autoware_pid_longitudinal_controller 1.0.0 PID-based longitudinal controller @@ -21,6 +21,7 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diagnostic_updater @@ -36,7 +37,6 @@ tier4_autoware_utils tier4_debug_msgs trajectory_follower_base - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml similarity index 100% rename from control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml rename to control/autoware_pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp similarity index 98% rename from control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp rename to control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 4e9ef52a4969e..0d8707afccad8 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" +#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Matrix3x3.h" #include "tf2/LinearMath/Quaternion.h" diff --git a/control/pid_longitudinal_controller/src/pid.cpp b/control/autoware_pid_longitudinal_controller/src/pid.cpp similarity index 98% rename from control/pid_longitudinal_controller/src/pid.cpp rename to control/autoware_pid_longitudinal_controller/src/pid.cpp index 530b5cd15e754..6d2c61f639746 100644 --- a/control/pid_longitudinal_controller/src/pid.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pid_longitudinal_controller/pid.hpp" +#include "autoware_pid_longitudinal_controller/pid.hpp" #include #include diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp similarity index 99% rename from control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp rename to control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 9a8223c610f9a..4cca525f6ebf3 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -38,9 +38,11 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // parameters timer m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double(); - m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m; - m_vehicle_width = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().vehicle_width_m; - m_front_overhang = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().front_overhang_m; + m_wheel_base = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().wheel_base_m; + m_vehicle_width = + autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().vehicle_width_m; + m_front_overhang = + autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo().front_overhang_m; // parameters for delay compensation m_delay_compensation_time = node.declare_parameter("delay_compensation_time"); // [s] @@ -1148,7 +1150,7 @@ void PidLongitudinalController::updateDebugVelAcc(const ControlData & control_da void PidLongitudinalController::setupDiagnosticUpdater() { - diagnostic_updater_.setHardwareID("pid_longitudinal_controller"); + diagnostic_updater_.setHardwareID("autoware_pid_longitudinal_controller"); diagnostic_updater_.add("control_state", this, &PidLongitudinalController::checkControlState); } diff --git a/control/pid_longitudinal_controller/src/smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp similarity index 98% rename from control/pid_longitudinal_controller/src/smooth_stop.cpp rename to control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp index 27fab43fa506f..9d8ad05235f11 100644 --- a/control/pid_longitudinal_controller/src/smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/src/smooth_stop.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pid_longitudinal_controller/smooth_stop.hpp" +#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" #include // NOLINT diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp similarity index 99% rename from control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp rename to control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index bd79d7a8c3ac3..de4d32008aa28 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "gtest/gtest.h" #include "interpolation/spherical_linear_interpolation.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" #include "autoware_planning_msgs/msg/trajectory.hpp" diff --git a/control/pid_longitudinal_controller/test/test_pid.cpp b/control/autoware_pid_longitudinal_controller/test/test_pid.cpp similarity index 97% rename from control/pid_longitudinal_controller/test/test_pid.cpp rename to control/autoware_pid_longitudinal_controller/test/test_pid.cpp index 82d01e0028a9c..0e3e0c70945dc 100644 --- a/control/pid_longitudinal_controller/test/test_pid.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_pid.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_pid_longitudinal_controller/pid.hpp" #include "gtest/gtest.h" -#include "pid_longitudinal_controller/pid.hpp" #include diff --git a/control/pid_longitudinal_controller/test/test_smooth_stop.cpp b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp similarity index 98% rename from control/pid_longitudinal_controller/test/test_smooth_stop.cpp rename to control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp index d658a5271d249..4f4fbb2354fa1 100644 --- a/control/pid_longitudinal_controller/test/test_smooth_stop.cpp +++ b/control/autoware_pid_longitudinal_controller/test/test_smooth_stop.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "autoware_pid_longitudinal_controller/smooth_stop.hpp" #include "gtest/gtest.h" -#include "pid_longitudinal_controller/smooth_stop.hpp" #include "rclcpp/rclcpp.hpp" #include diff --git a/control/smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/.gitignore diff --git a/control/smart_mpc_trajectory_follower/CMakeLists.txt b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt similarity index 60% rename from control/smart_mpc_trajectory_follower/CMakeLists.txt rename to control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt index 7aecab2597dcd..57e7b596790c2 100644 --- a/control/smart_mpc_trajectory_follower/CMakeLists.txt +++ b/control/autoware_smart_mpc_trajectory_follower/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 3.14) -project(smart_mpc_trajectory_follower) +project(autoware_smart_mpc_trajectory_follower) find_package(autoware_cmake REQUIRED) autoware_package() install(PROGRAMS - smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py + autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py DESTINATION lib/${PROJECT_NAME} ) ament_auto_package( diff --git a/control/smart_mpc_trajectory_follower/README.md b/control/autoware_smart_mpc_trajectory_follower/README.md similarity index 85% rename from control/smart_mpc_trajectory_follower/README.md rename to control/autoware_smart_mpc_trajectory_follower/README.md index 8fcc13142d68b..fd135319ff900 100644 --- a/control/smart_mpc_trajectory_follower/README.md +++ b/control/autoware_smart_mpc_trajectory_follower/README.md @@ -19,7 +19,7 @@ This technology makes it relatively easy to operate model predictive control, wh ## Setup -After building autoware, move to `control/smart_mpc_trajectory_follower` and run the following command: +After building autoware, move to `control/autoware_smart_mpc_trajectory_follower` and run the following command: ```bash pip3 install . @@ -37,7 +37,7 @@ This package provides smart MPC logic for path-following control as well as mech ### Trajectory following control based on iLQR/MPPI -The control mode can be selected from "ilqr", "mppi", or "mppi_ilqr", and can be set as `mpc_parameter:system:mode` in [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml). +The control mode can be selected from "ilqr", "mppi", or "mppi_ilqr", and can be set as `mpc_parameter:system:mode` in [mpc_param.yaml](./autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml). In "mppi_ilqr" mode, the initial value of iLQR is given by the MPPI solution. > [!NOTE] @@ -50,7 +50,7 @@ ros2 launch autoware_launch planning_simulator.launch.xml map_path:=$HOME/autowa ``` > [!NOTE] -> When running with the nominal model set in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml), set `trained_model_parameter:control_application:use_trained_model` to `false` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). To run using the trained model, set `trained_model_parameter:control_application:use_trained_model` to `true`, but the trained model must have been generated according to the following procedure. +> When running with the nominal model set in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml), set `trained_model_parameter:control_application:use_trained_model` to `false` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). To run using the trained model, set `trained_model_parameter:control_application:use_trained_model` to `true`, but the trained model must have been generated according to the following procedure. ### Training of model and reflection in control @@ -60,7 +60,7 @@ To obtain training data, start autoware, perform a drive, and record rosbag data ros2 bag record /localization/kinematic_state /localization/acceleration /vehicle/status/steering_status /control/command/control_cmd /control/trajectory_follower/control_cmd /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw /system/operation_mode/state /vehicle/status/control_mode /sensing/imu/imu_data /debug_mpc_x_des /debug_mpc_y_des /debug_mpc_v_des /debug_mpc_yaw_des /debug_mpc_acc_des /debug_mpc_steer_des /debug_mpc_X_des_converted /debug_mpc_x_current /debug_mpc_error_prediction /debug_mpc_max_trajectory_err /debug_mpc_emergency_stop_mode /debug_mpc_goal_stop_mode /debug_mpc_total_ctrl_time /debug_mpc_calc_u_opt_time ``` -Move [rosbag2.bash](./smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash) to the rosbag directory recorded above and execute the following command on the directory +Move [rosbag2.bash](./autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash) to the rosbag directory recorded above and execute the following command on the directory ```bash bash rosbag2.bash @@ -75,7 +75,7 @@ This converts rosbag data into CSV format for training models. Instead, the same result can be obtained by executing the following command in a python environment: ```python -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model model_trainer = train_drive_NN_model.train_drive_NN_model() model_trainer.transform_rosbag_to_csv(rosbag_dir) ``` @@ -86,7 +86,7 @@ At this time, all CSV files in `rosbag_dir` are automatically deleted first. The paths of the rosbag directories used for training, `dir_0`, `dir_1`, `dir_2`,... and the directory `save_dir` where you save the models, the model can be saved in the python environment as follows: ```python -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model model_trainer = train_drive_NN_model.train_drive_NN_model() model_trainer.add_data_from_csv(dir_0) model_trainer.add_data_from_csv(dir_1) @@ -112,12 +112,12 @@ If only polynomial regression is performed and no NN model is used, run the foll model_trainer.get_trained_model(use_polynomial_reg=True,force_NN_model_to_zero=True) ``` -Move `model_for_test_drive.pth` and `polynomial_reg_info.npz` saved in `save_dir` to the home directory and set `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml) to `true` to reflect the trained model in the control. +Move `model_for_test_drive.pth` and `polynomial_reg_info.npz` saved in `save_dir` to the home directory and set `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) to `true` to reflect the trained model in the control. ### Performance evaluation Here, as an example, we describe the verification of the adaptive performance when the wheel base of the sample_vehicle is 2.79 m, but an incorrect value of 2.0 m is given to the controller side. -To give the controller 2.0 m as the wheel base, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.0, and run the following command: +To give the controller 2.0 m as the wheel base, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.0, and run the following command: ```bash python3 -m smart_mpc_trajectory_follower.clear_pycache @@ -125,7 +125,7 @@ python3 -m smart_mpc_trajectory_follower.clear_pycache #### Test on autoware -To perform a control test on autoware with the nominal model before training, make sure that `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml) is `false` and launch autoware in the manner described in "Trajectory following control based on iLQR/MPPI". This time, the following route will be used for the test: +To perform a control test on autoware with the nominal model before training, make sure that `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) is `false` and launch autoware in the manner described in "Trajectory following control based on iLQR/MPPI". This time, the following route will be used for the test:

@@ -136,7 +136,7 @@ Record rosbag and train the model in the manner described in "Training of model To control using the trained model obtained here, set `trained_model_parameter:control_application:use_trained_model` to `true`, start autoware in the same way, and drive the same route recording rosbag. After the driving is complete, convert the rosbag file to CSV format using the method described in "Training of model and reflection in control". -A plot of the lateral deviation is obtained by running the `lateral_error_visualize` function in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb` for the nominal and training model rosbag files `rosbag_nominal` and `rosbag_trained`, respectively, as follows: +A plot of the lateral deviation is obtained by running the `lateral_error_visualize` function in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb` for the nominal and training model rosbag files `rosbag_nominal` and `rosbag_trained`, respectively, as follows: ```python lateral_error_visualize(dir_name=rosbag_nominal,ylim=[-1.2,1.2]) @@ -152,7 +152,7 @@ The following results were obtained. #### Test on python simulator -First, to give wheel base 2.79 m in the python simulator, create the following file and save it in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator` with the name `sim_setting.json`: +First, to give wheel base 2.79 m in the python simulator, create the following file and save it in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` with the name `sim_setting.json`: ```json { "wheel_base": 2.79 } @@ -162,7 +162,7 @@ Next, run the following commands to test the slalom driving on the python simula ```python import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model initial_error = [0.0, 0.03, 0.01, -0.01, 0.0, 0.0] save_dir = "test_python_sim" python_simulator.slalom_drive(save_dir=save_dir,use_trained_model=False,initial_error=initial_error) @@ -172,7 +172,7 @@ Here, `initial_error` is the initial error from the target trajectory, in the or and `save_dir` is the directory where the driving test results are saved. > [!NOTE] -> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). +> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). Run the following commands to perform training using driving data of the nominal model. @@ -211,21 +211,21 @@ It can be seen that the lateral deviation has improved significantly. Here we have described wheel base, but the parameters that can be passed to the python simulator are as follows. -| Parameter | Type | Description | -| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| steer_bias | float | steer bias [rad] | -| steer_rate_lim | float | steer rate limit [rad/s] | -| vel_rate_lim | float | acceleration limit [m/s^2] | -| wheel_base | float | wheel base [m] | -| steer_dead_band | float | steer dead band [rad] | -| adaptive_gear_ratio_coef | list[float] | List of floats of length 6 specifying information on speed-dependent gear ratios from tire angle to steering wheel angle. | -| acc_time_delay | float | acceleration time delay [s] | -| steer_time_delay | float | steer time delay [s] | -| acc_time_constant | float | acceleration time constant [s] | -| steer_time_constant | float | steer time constant [s] | -| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. | -| acc_scaling | float | acceleration scaling | -| steer_scaling | float | steer scaling | +| Parameter | Type | Description | +| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| steer_bias | float | steer bias [rad] | +| steer_rate_lim | float | steer rate limit [rad/s] | +| vel_rate_lim | float | acceleration limit [m/s^2] | +| wheel_base | float | wheel base [m] | +| steer_dead_band | float | steer dead band [rad] | +| adaptive_gear_ratio_coef | list[float] | List of floats of length 6 specifying information on speed-dependent gear ratios from tire angle to steering wheel angle. | +| acc_time_delay | float | acceleration time delay [s] | +| steer_time_delay | float | steer time delay [s] | +| acc_time_constant | float | acceleration time constant [s] | +| steer_time_constant | float | steer time constant [s] | +| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. | +| acc_scaling | float | acceleration scaling | +| steer_scaling | float | steer scaling | For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad] of steer dead band, edit the `sim_setting.json` as follows. @@ -237,13 +237,13 @@ For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad Here, we describe a method for testing adaptive performance by giving the simulation side a predefined range of model parameters while the control side is given constant model parameters. -First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command: +First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command: ```bash python3 -m smart_mpc_trajectory_follower.clear_pycache ``` -To run a driving experiment within the parameter change range set in [run_sim.py](./smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator` and run the following command: +To run a driving experiment within the parameter change range set in [run_sim.py](./autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` and run the following command: ```bash python3 run_sim.py --param_name steer_bias @@ -270,11 +270,11 @@ In `run_sim.py`, the following parameters can be set: | USE_INTERCEPT | bool | When a polynomial regression including bias is performed, whether to use or discard the resulting bias information.
It is meaningful only if FIT_INTERCEPT is True.
If it is False, discard the bias in the polynomial regression in the hope that the NN model can remove the bias term, even if the polynomial regression is performed with the bias included. | > [!NOTE] -> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). +> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). ## Change of nominal parameters and their reloading -The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./smart_mpc_trajectory_follower/param/nominal_param.yaml). +The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml). After changing the nominal parameters, the cache must be deleted by running the following command: ```bash @@ -293,7 +293,7 @@ The nominal parameters include the following: ## Change of control parameters and their reloading -The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./smart_mpc_trajectory_follower/param/trained_model_param.yaml). +The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml). Although it is possible to reflect parameter changes by restarting autoware, the following command allows us to do so without leaving autoware running: ```bash diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py similarity index 88% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py index fafcb5bdecf05..2458de3a80f99 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/clear_pycache.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/clear_pycache.py @@ -16,10 +16,10 @@ from pathlib import Path import shutil -import smart_mpc_trajectory_follower +import autoware_smart_mpc_trajectory_follower if __name__ == "__main__": - package_dir = str(Path(smart_mpc_trajectory_follower.__file__).parent) + package_dir = str(Path(autoware_smart_mpc_trajectory_follower.__file__).parent) remove_dirs = [ package_dir + "/__pycache__", diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/mpc_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/nominal_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/param/trained_model_param.yaml rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/accel_map.csv rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py index d706c342f04ec..0eaa6ce1c0fbf 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/python_simulator.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py @@ -18,14 +18,14 @@ import datetime import os +from autoware_smart_mpc_trajectory_follower.scripts import drive_controller +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions import matplotlib.pyplot as plt from numba import njit import numpy as np import pandas as pd import scipy.interpolate import simplejson as json -from smart_mpc_trajectory_follower.scripts import drive_controller -from smart_mpc_trajectory_follower.scripts import drive_functions print("\n\n### import python_simulator.py ###") diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py similarity index 92% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py index 6b477bb804ac9..68d8a9f64e73f 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py @@ -12,9 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model import numpy as np import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model initial_error = np.array( [0.001, 0.03, 0.01, -0.001, 0, 2 * python_simulator.measurement_steer_bias] diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py index 3125719556559..d2bb4ea26f0a6 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/run_sim.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py @@ -23,9 +23,9 @@ import traceback from typing import Dict +from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model import numpy as np import python_simulator -from smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model parser = argparse.ArgumentParser() parser.add_argument("--param_name", default=None) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/slalom_course_data.csv diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py index e75eedcb4bf65..4d36afd3d4324 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_GP.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_GP.py @@ -16,9 +16,9 @@ from functools import partial +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions sqrt_mpc_time_step = np.sqrt(drive_functions.mpc_time_step) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py index f35789ee39fd3..f13aa07b0f795 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_NN.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py @@ -13,9 +13,9 @@ # limitations under the License. +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.scripts import proxima_calc import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.scripts import proxima_calc import torch from torch import nn diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py index b21fcf72da931..228b0c37c5e7b 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_controller.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py @@ -21,14 +21,14 @@ import threading import time +from autoware_smart_mpc_trajectory_follower.scripts import drive_GP +from autoware_smart_mpc_trajectory_follower.scripts import drive_NN +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.scripts import drive_iLQR +from autoware_smart_mpc_trajectory_follower.scripts import drive_mppi import numpy as np import scipy.interpolate # type: ignore from sklearn.preprocessing import PolynomialFeatures -from smart_mpc_trajectory_follower.scripts import drive_GP -from smart_mpc_trajectory_follower.scripts import drive_NN -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.scripts import drive_iLQR -from smart_mpc_trajectory_follower.scripts import drive_mppi import torch ctrl_index_for_polynomial_reg = np.concatenate( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py index c11b5c886b49a..fb96e134f7746 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_functions.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py @@ -31,7 +31,9 @@ with open(package_path_json, "r") as file: package_path = json.load(file) -mpc_param_path = package_path["path"] + "/smart_mpc_trajectory_follower/param/mpc_param.yaml" +mpc_param_path = ( + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml" +) with open(mpc_param_path, "r") as yml: mpc_param = yaml.safe_load(yml) @@ -101,7 +103,7 @@ cap_pred_error = np.array(mpc_param["mpc_parameter"]["preprocessing"]["cap_pred_error"]) nominal_param_path = ( - package_path["path"] + "/smart_mpc_trajectory_follower/param/nominal_param.yaml" + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml" ) with open(nominal_param_path, "r") as yml: nominal_param = yaml.safe_load(yml) @@ -172,7 +174,7 @@ mpc_param["mpc_parameter"]["preprocessing"]["sg_window_size_for_nominal_inputs"] ) trained_model_param_path = ( - package_path["path"] + "/smart_mpc_trajectory_follower/param/trained_model_param.yaml" + package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml" ) with open(trained_model_param_path, "r") as yml: trained_model_param = yaml.safe_load(yml) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py index 261a72e680778..2307586f23552 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_iLQR.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py @@ -21,9 +21,9 @@ import time from typing import Callable +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions index_cost = np.concatenate( ( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py index ca6e6f15f42ef..5e88c79172097 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/drive_mppi.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py @@ -16,9 +16,9 @@ from typing import Callable +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from numba import njit import numpy as np -from smart_mpc_trajectory_follower.scripts import drive_functions index_cost = np.concatenate( ( diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/proxima_calc.cpp b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/proxima_calc.cpp rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 992a5fd53a10c..78a120fe601f2 100755 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -22,6 +22,8 @@ from autoware_control_msgs.msg import Control from autoware_planning_msgs.msg import Trajectory from autoware_planning_msgs.msg import TrajectoryPoint +from autoware_smart_mpc_trajectory_follower.scripts import drive_controller +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions from autoware_vehicle_msgs.msg import SteeringReport from builtin_interfaces.msg import Duration from geometry_msgs.msg import AccelWithCovarianceStamped @@ -34,8 +36,6 @@ import scipy.interpolate from scipy.spatial.transform import Rotation as R from scipy.spatial.transform import Slerp -from smart_mpc_trajectory_follower.scripts import drive_controller -from smart_mpc_trajectory_follower.scripts import drive_functions from std_msgs.msg import String from tier4_debug_msgs.msg import BoolStamped from tier4_debug_msgs.msg import Float32MultiArrayStamped diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/.gitignore similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/.gitignore rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/.gitignore diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/__init__.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/__init__.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/__init__.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/__init__.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py similarity index 98% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py index 3f6ef7a9f78d1..5338f3cbaddce 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py @@ -23,11 +23,11 @@ import os from pathlib import Path +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions import numpy as np import scipy.interpolate from scipy.ndimage import gaussian_filter from scipy.spatial.transform import Rotation -from smart_mpc_trajectory_follower.scripts import drive_functions def data_smoothing(data: np.ndarray, sigma: float) -> np.ndarray: @@ -76,7 +76,7 @@ def transform_rosbag_to_csv(self, dir_name: str, delete_csv_first: bool = True) os.system( "cp " + package_path["path"] - + "/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash " + + "/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash " + dir_name ) os.chdir(dir_name) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py similarity index 100% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py similarity index 99% rename from control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py rename to control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py index 269b577f83c25..449ddda45f460 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py +++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py @@ -15,13 +15,15 @@ # cspell: ignore optim savez suptitle """Class for training neural nets from driving data.""" +from autoware_smart_mpc_trajectory_follower.scripts import drive_NN +from autoware_smart_mpc_trajectory_follower.scripts import drive_functions +from autoware_smart_mpc_trajectory_follower.training_and_data_check import ( + add_training_data_from_csv, +) import matplotlib.pyplot as plt import numpy as np from sklearn import linear_model from sklearn.preprocessing import PolynomialFeatures -from smart_mpc_trajectory_follower.scripts import drive_NN -from smart_mpc_trajectory_follower.scripts import drive_functions -from smart_mpc_trajectory_follower.training_and_data_check import add_training_data_from_csv import torch from torch import nn from torch.utils.data import DataLoader diff --git a/control/smart_mpc_trajectory_follower/images/autoware_smart_mpc.png b/control/autoware_smart_mpc_trajectory_follower/images/autoware_smart_mpc.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/autoware_smart_mpc.png rename to control/autoware_smart_mpc_trajectory_follower/images/autoware_smart_mpc.png diff --git a/control/smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png diff --git a/control/smart_mpc_trajectory_follower/images/lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/lateral_error_trained_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png diff --git a/control/smart_mpc_trajectory_follower/images/proxima_logo.png b/control/autoware_smart_mpc_trajectory_follower/images/proxima_logo.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/proxima_logo.png rename to control/autoware_smart_mpc_trajectory_follower/images/proxima_logo.png diff --git a/control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png diff --git a/control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png rename to control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png diff --git a/control/smart_mpc_trajectory_follower/images/test_route.png b/control/autoware_smart_mpc_trajectory_follower/images/test_route.png similarity index 100% rename from control/smart_mpc_trajectory_follower/images/test_route.png rename to control/autoware_smart_mpc_trajectory_follower/images/test_route.png diff --git a/control/smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml similarity index 84% rename from control/smart_mpc_trajectory_follower/package.xml rename to control/autoware_smart_mpc_trajectory_follower/package.xml index 6cdd689c8bd5a..a05f06cd865ff 100644 --- a/control/smart_mpc_trajectory_follower/package.xml +++ b/control/autoware_smart_mpc_trajectory_follower/package.xml @@ -1,12 +1,15 @@ - smart_mpc_trajectory_follower + autoware_smart_mpc_trajectory_follower 1.0.0 Nodes to follow a trajectory by generating control commands using smart mpc Masayuki Aino + Kosuke Takeuchi + Takamasa Horibe + Takayuki Murooka Apache License 2.0 diff --git a/control/smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py similarity index 60% rename from control/smart_mpc_trajectory_follower/setup.py rename to control/autoware_smart_mpc_trajectory_follower/setup.py index 4dca4d72929a8..fee1d04e826c0 100644 --- a/control/smart_mpc_trajectory_follower/setup.py +++ b/control/autoware_smart_mpc_trajectory_follower/setup.py @@ -13,25 +13,25 @@ os.system("pip3 install torch") package_path = {} package_path["path"] = str(Path(__file__).parent) -with open("smart_mpc_trajectory_follower/package_path.json", "w") as f: +with open("autoware_smart_mpc_trajectory_follower/package_path.json", "w") as f: json.dump(package_path, f) build_cpp_command = "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) " -build_cpp_command += "smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " -build_cpp_command += ( - "-o smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " -) +build_cpp_command += "autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp " +build_cpp_command += "-o autoware_smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) " build_cpp_command += "-lrt -I/usr/include/eigen3" os.system(build_cpp_command) so_path = ( "scripts/" - + glob.glob("smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[-1] + + glob.glob("autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.*.so")[0].split("/")[ + -1 + ] ) setup( name="smart_mpc_trajectory_follower", version="1.0.0", packages=find_packages(), package_data={ - "smart_mpc_trajectory_follower": ["package_path.json", so_path], + "autoware_smart_mpc_trajectory_follower": ["package_path.json", so_path], }, ) diff --git a/control/vehicle_cmd_gate/CMakeLists.txt b/control/autoware_vehicle_cmd_gate/CMakeLists.txt similarity index 93% rename from control/vehicle_cmd_gate/CMakeLists.txt rename to control/autoware_vehicle_cmd_gate/CMakeLists.txt index c5db663ccc77e..7233608a5d25f 100644 --- a/control/vehicle_cmd_gate/CMakeLists.txt +++ b/control/autoware_vehicle_cmd_gate/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(vehicle_cmd_gate) +project(autoware_vehicle_cmd_gate) find_package(autoware_cmake REQUIRED) autoware_package() @@ -12,7 +12,7 @@ ament_auto_add_library(vehicle_cmd_gate_node SHARED ) rclcpp_components_register_node(vehicle_cmd_gate_node - PLUGIN "vehicle_cmd_gate::VehicleCmdGate" + PLUGIN "autoware::vehicle_cmd_gate::VehicleCmdGate" EXECUTABLE vehicle_cmd_gate_exe ) diff --git a/control/vehicle_cmd_gate/README.md b/control/autoware_vehicle_cmd_gate/README.md similarity index 100% rename from control/vehicle_cmd_gate/README.md rename to control/autoware_vehicle_cmd_gate/README.md diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml similarity index 100% rename from control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml rename to control/autoware_vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml diff --git a/control/vehicle_cmd_gate/image/filter.png b/control/autoware_vehicle_cmd_gate/image/filter.png similarity index 100% rename from control/vehicle_cmd_gate/image/filter.png rename to control/autoware_vehicle_cmd_gate/image/filter.png diff --git a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml b/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml similarity index 89% rename from control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml rename to control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml index c5368276b488a..c9df4e67c2647 100644 --- a/control/vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml +++ b/control/autoware_vehicle_cmd_gate/launch/vehicle_cmd_gate.launch.xml @@ -1,11 +1,11 @@ - + - + - + diff --git a/control/vehicle_cmd_gate/msg/IsFilterActivated.msg b/control/autoware_vehicle_cmd_gate/msg/IsFilterActivated.msg similarity index 100% rename from control/vehicle_cmd_gate/msg/IsFilterActivated.msg rename to control/autoware_vehicle_cmd_gate/msg/IsFilterActivated.msg diff --git a/control/vehicle_cmd_gate/package.xml b/control/autoware_vehicle_cmd_gate/package.xml similarity index 95% rename from control/vehicle_cmd_gate/package.xml rename to control/autoware_vehicle_cmd_gate/package.xml index e01d566c37df9..f67d01a5593ae 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/autoware_vehicle_cmd_gate/package.xml @@ -1,7 +1,7 @@ - vehicle_cmd_gate + autoware_vehicle_cmd_gate 0.1.0 The vehicle_cmd_gate package Takamasa Horibe @@ -17,6 +17,7 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -33,7 +34,6 @@ tier4_external_api_msgs tier4_system_msgs tier4_vehicle_msgs - vehicle_info_util visualization_msgs rosidl_default_runtime diff --git a/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json b/control/autoware_vehicle_cmd_gate/schema/vehicle_cmd_gate.json similarity index 100% rename from control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json rename to control/autoware_vehicle_cmd_gate/schema/vehicle_cmd_gate.json diff --git a/control/vehicle_cmd_gate/script/delays_checker.py b/control/autoware_vehicle_cmd_gate/script/delays_checker.py similarity index 100% rename from control/vehicle_cmd_gate/script/delays_checker.py rename to control/autoware_vehicle_cmd_gate/script/delays_checker.py diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp similarity index 95% rename from control/vehicle_cmd_gate/src/adapi_pause_interface.cpp rename to control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp index f4cf28d337a09..f14a62fba9661 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp +++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.cpp @@ -14,7 +14,7 @@ #include "adapi_pause_interface.hpp" -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { AdapiPauseInterface::AdapiPauseInterface(rclcpp::Node * node) : node_(node) @@ -65,4 +65,4 @@ void AdapiPauseInterface::on_pause( res->status.success = true; } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp similarity index 95% rename from control/vehicle_cmd_gate/src/adapi_pause_interface.hpp rename to control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp index 1d5f05e98975e..d75b544752e3f 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp +++ b/control/autoware_vehicle_cmd_gate/src/adapi_pause_interface.hpp @@ -21,7 +21,7 @@ #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { class AdapiPauseInterface @@ -55,6 +55,6 @@ class AdapiPauseInterface const SetPause::Service::Response::SharedPtr res); }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // ADAPI_PAUSE_INTERFACE_HPP_ diff --git a/control/vehicle_cmd_gate/src/marker_helper.hpp b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp similarity index 97% rename from control/vehicle_cmd_gate/src/marker_helper.hpp rename to control/autoware_vehicle_cmd_gate/src/marker_helper.hpp index 44c43a3630151..c9d7e86fa23dd 100644 --- a/control/vehicle_cmd_gate/src/marker_helper.hpp +++ b/control/autoware_vehicle_cmd_gate/src/marker_helper.hpp @@ -21,7 +21,7 @@ #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z) { @@ -114,6 +114,6 @@ inline void appendMarkerArray( marker_array->markers.push_back(marker); } } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // MARKER_HELPER_HPP_ diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp similarity index 95% rename from control/vehicle_cmd_gate/src/moderate_stop_interface.cpp rename to control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp index 72dbdfd26a6b3..0fe90adfd0a72 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.cpp +++ b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.cpp @@ -14,7 +14,7 @@ #include "moderate_stop_interface.hpp" -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { ModerateStopInterface::ModerateStopInterface(rclcpp::Node * node) : node_(node) @@ -66,4 +66,4 @@ void ModerateStopInterface::update_stop_state() } } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp similarity index 95% rename from control/vehicle_cmd_gate/src/moderate_stop_interface.hpp rename to control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp index b643afc212f61..012e75d0c207e 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp +++ b/control/autoware_vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -22,7 +22,7 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { class ModerateStopInterface @@ -53,6 +53,6 @@ class ModerateStopInterface void update_stop_state(); }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // MODERATE_STOP_INTERFACE_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp similarity index 99% rename from control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index bd9955e773020..cec23b05b8191 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -17,7 +17,7 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { VehicleCmdFilter::VehicleCmdFilter() @@ -319,4 +319,4 @@ double VehicleCmdFilter::getSteerDiffLim() const return interpolateFromSpeed(param_.actual_steer_diff_lim); } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp similarity index 94% rename from control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index d9b8383a1de51..96663474f47a4 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -15,17 +15,17 @@ #ifndef VEHICLE_CMD_FILTER_HPP_ #define VEHICLE_CMD_FILTER_HPP_ +#include #include -#include #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { using autoware_control_msgs::msg::Control; -using vehicle_cmd_gate::msg::IsFilterActivated; +using autoware_vehicle_cmd_gate::msg::IsFilterActivated; using LimitArray = std::vector; struct VehicleCmdFilterParam @@ -98,6 +98,6 @@ class VehicleCmdFilter double getSteerRateLim() const; double getSteerDiffLim() const; }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // VEHICLE_CMD_FILTER_HPP_ diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp similarity index 98% rename from control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index cc9d7813083f4..bb50dcb586e83 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -28,7 +28,7 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { namespace @@ -170,7 +170,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) declare_parameter("filter_activated_velocity_threshold"); // Vehicle Parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); { VehicleCmdFilterParam p; p.wheel_base = vehicle_info.wheel_base_m; @@ -276,7 +276,7 @@ rcl_interfaces::msg::SetParametersResult VehicleCmdGate::onParameter( parameters, "filter_activated_velocity_threshold", filter_activated_velocity_threshold_); // Vehicle Parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); { VehicleCmdFilterParam p = filter_.getParam(); p.wheel_base = vehicle_info.wheel_base_m; @@ -906,7 +906,7 @@ void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated) filter_activated_flag_pub_->publish(filter_activated_flag); filter_activated_marker_raw_pub_->publish(createMarkerArray(filter_activated)); } -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #include -RCLCPP_COMPONENTS_REGISTER_NODE(vehicle_cmd_gate::VehicleCmdGate) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::vehicle_cmd_gate::VehicleCmdGate) diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp similarity index 97% rename from control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp rename to control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index f45280d9d2e48..06dad536d842d 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -20,12 +20,12 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_cmd_filter.hpp" +#include +#include #include #include #include #include -#include -#include #include #include @@ -51,13 +51,14 @@ #include #include -namespace vehicle_cmd_gate +namespace autoware::vehicle_cmd_gate { using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_control_msgs::msg::Control; using autoware_control_msgs::msg::Longitudinal; +using autoware_vehicle_cmd_gate::msg::IsFilterActivated; using autoware_vehicle_msgs::msg::GearCommand; using autoware_vehicle_msgs::msg::HazardLightsCommand; using autoware_vehicle_msgs::msg::SteeringReport; @@ -71,7 +72,6 @@ using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; -using vehicle_cmd_gate::msg::IsFilterActivated; using visualization_msgs::msg::MarkerArray; using diagnostic_msgs::msg::DiagnosticStatus; @@ -257,5 +257,5 @@ class VehicleCmdGate : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace vehicle_cmd_gate +} // namespace autoware::vehicle_cmd_gate #endif // VEHICLE_CMD_GATE_HPP_ diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp similarity index 98% rename from control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index d51db90c8a260..46130fc8f9941 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -54,7 +54,7 @@ const std::vector lat_jerk_lim = {1.7, 1.3, 0.9, 0.6}; const std::vector actual_steer_diff_lim = {0.5, 0.4, 0.2, 0.1}; const double wheelbase = 2.89; -using vehicle_cmd_gate::VehicleCmdGate; +using autoware::vehicle_cmd_gate::VehicleCmdGate; using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; @@ -369,9 +369,9 @@ std::shared_ptr generateNode() auto node_options = rclcpp::NodeOptions{}; const auto vehicle_cmd_gate_dir = - ament_index_cpp::get_package_share_directory("vehicle_cmd_gate"); + ament_index_cpp::get_package_share_directory("autoware_vehicle_cmd_gate"); const auto vehicle_info_util_dir = - ament_index_cpp::get_package_share_directory("vehicle_info_util"); + ament_index_cpp::get_package_share_directory("autoware_vehicle_info_utils"); node_options.arguments( {"--ros-args", "--params-file", vehicle_cmd_gate_dir + "/config/vehicle_cmd_gate.param.yaml", diff --git a/control/vehicle_cmd_gate/test/src/test_main.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_main.cpp similarity index 100% rename from control/vehicle_cmd_gate/test/src/test_main.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_main.cpp diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp similarity index 98% rename from control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp rename to control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 5fbab1c5cfb4f..528253671b38a 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/autoware_vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -24,17 +24,17 @@ #define ASSERT_LT_NEAR(x, y) ASSERT_LT(x, y + THRESHOLD) #define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) +using autoware::vehicle_cmd_gate::LimitArray; using autoware_control_msgs::msg::Control; -using vehicle_cmd_gate::LimitArray; constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( - vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, + autoware::vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, LimitArray steer_lim, LimitArray steer_rate_lim, const double wheelbase) { - vehicle_cmd_gate::VehicleCmdFilterParam p; + autoware::vehicle_cmd_gate::VehicleCmdFilterParam p; p.vel_lim = v; p.wheel_base = wheelbase; p.reference_speed_points = speed_points; @@ -105,7 +105,7 @@ void test_1d_limit( const double WHEELBASE = 3.0; const double DT = 0.1; // [s] - vehicle_cmd_gate::VehicleCmdFilter filter; + autoware::vehicle_cmd_gate::VehicleCmdFilter filter; filter.setCurrentSpeed(ego_v); setFilterParams( filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, {STEER_LIM}, @@ -275,9 +275,9 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) { constexpr double WHEELBASE = 2.8; - vehicle_cmd_gate::VehicleCmdFilter filter; + autoware::vehicle_cmd_gate::VehicleCmdFilter filter; - vehicle_cmd_gate::VehicleCmdFilterParam p; + autoware::vehicle_cmd_gate::VehicleCmdFilterParam p; p.wheel_base = WHEELBASE; p.vel_lim = 20.0; p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; diff --git a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml index be2b30bdf1bf9..f2997f0e10b83 100644 --- a/control/control_performance_analysis/launch/control_performance_analysis.launch.xml +++ b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml @@ -8,7 +8,7 @@ - + diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index a26f4164a177b..b18fb8c98ccdc 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -26,6 +26,7 @@ autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs libboost-dev @@ -41,7 +42,6 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - vehicle_info_util builtin_interfaces global_parameter_loader rosidl_default_runtime diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 693194e8b70a6..47ce3ce07e83c 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -17,7 +17,7 @@ #include "control_performance_analysis/msg/driving_monitor_stamped.hpp" #include "control_performance_analysis/msg/error_stamped.hpp" -#include +#include #include #include @@ -32,7 +32,7 @@ using control_performance_analysis::msg::ErrorStamped; namespace control_performance_analysis { -using vehicle_info_util::VehicleInfoUtil; +using autoware::vehicle_info_utils::VehicleInfoUtils; ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( const rclcpp::NodeOptions & node_options) @@ -41,7 +41,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( using std::placeholders::_1; // Implement Reading Global and Local Variables. - const auto & vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto & vehicle_info = VehicleInfoUtils(*this).getVehicleInfo(); param_.wheelbase_ = vehicle_info.wheel_base_m; // Node Parameters. diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index e048ef03bf11a..56125cc40b74e 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -15,9 +15,9 @@ #ifndef CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ #define CONTROL_VALIDATOR__CONTROL_VALIDATOR_HPP_ +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "control_validator/debug_marker.hpp" #include "control_validator/msg/control_validator_status.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -83,7 +83,7 @@ class ControlValidator : public rclcpp::Node ControlValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool isAllValid(const ControlValidatorStatus & status); diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index 2f4400d6ebc57..93779d28d6218 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -21,6 +21,7 @@ rosidl_default_generators autoware_planning_msgs + autoware_vehicle_info_utils diagnostic_updater geometry_msgs motion_utils @@ -28,7 +29,6 @@ rclcpp rclcpp_components tier4_autoware_utils - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index 5282e31fef898..be103647f1b36 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -61,7 +61,7 @@ void ControlValidator::setupParameters() } try { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); } catch (...) { RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); vehicle_info_.front_overhang_m = 0.5; diff --git a/control/external_cmd_selector/CMakeLists.txt b/control/external_cmd_selector/CMakeLists.txt deleted file mode 100644 index 5c4860b7441f3..0000000000000 --- a/control/external_cmd_selector/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(external_cmd_selector) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(external_cmd_selector_node SHARED - src/external_cmd_selector/external_cmd_selector_node.cpp -) - -rclcpp_components_register_node(external_cmd_selector_node - PLUGIN "ExternalCmdSelector" - EXECUTABLE external_cmd_selector -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/control/joy_controller/CMakeLists.txt b/control/joy_controller/CMakeLists.txt deleted file mode 100644 index f000c8c0909f4..0000000000000 --- a/control/joy_controller/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(joy_controller) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(joy_controller_node SHARED - src/joy_controller/joy_controller_node.cpp -) - -rclcpp_components_register_node(joy_controller_node - PLUGIN "joy_controller::AutowareJoyControllerNode" - EXECUTABLE joy_controller -) - -ament_auto_package( - INSTALL_TO_SHARE - launch - config -) diff --git a/control/joy_controller/launch/joy_controller_param_selection.launch.xml b/control/joy_controller/launch/joy_controller_param_selection.launch.xml deleted file mode 100644 index d8e3d0bfe8b25..0000000000000 --- a/control/joy_controller/launch/joy_controller_param_selection.launch.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/control/lane_departure_checker/CMakeLists.txt b/control/lane_departure_checker/CMakeLists.txt index 874670c478240..d545e1cbb5ad1 100644 --- a/control/lane_departure_checker/CMakeLists.txt +++ b/control/lane_departure_checker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(lane_departure_checker) +project(autoware_lane_departure_checker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -10,13 +10,13 @@ include_directories( ${EIGEN3_INCLUDE_DIRS} ) -ament_auto_add_library(lane_departure_checker SHARED +ament_auto_add_library(autoware_lane_departure_checker SHARED src/lane_departure_checker_node/lane_departure_checker.cpp src/lane_departure_checker_node/lane_departure_checker_node.cpp ) -rclcpp_components_register_node(lane_departure_checker - PLUGIN "lane_departure_checker::LaneDepartureCheckerNode" +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::lane_departure_checker::LaneDepartureCheckerNode" EXECUTABLE lane_departure_checker_node ) diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp similarity index 87% rename from control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp rename to control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp index 967cb65c8efa8..e29623bf69891 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ -#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#include #include #include #include -#include #include #include @@ -45,7 +45,7 @@ #include #include -namespace lane_departure_checker +namespace autoware::lane_departure_checker { using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; @@ -103,17 +103,17 @@ class LaneDepartureChecker public: Output update(const Input & input); - void setParam(const Param & param, const vehicle_info_util::VehicleInfo vehicle_info) + void setParam(const Param & param, const autoware::vehicle_info_utils::VehicleInfo vehicle_info) { param_ = param; - vehicle_info_ptr_ = std::make_shared(vehicle_info); + vehicle_info_ptr_ = std::make_shared(vehicle_info); } void setParam(const Param & param) { param_ = param; } - void setVehicleInfo(const vehicle_info_util::VehicleInfo vehicle_info) + void setVehicleInfo(const autoware::vehicle_info_utils::VehicleInfo vehicle_info) { - vehicle_info_ptr_ = std::make_shared(vehicle_info); + vehicle_info_ptr_ = std::make_shared(vehicle_info); } bool checkPathWillLeaveLane( @@ -137,7 +137,7 @@ class LaneDepartureChecker private: Param param_; - std::shared_ptr vehicle_info_ptr_; + std::shared_ptr vehicle_info_ptr_; static PoseDeviation calcTrajectoryDeviation( const Trajectory & trajectory, const geometry_msgs::msg::Pose & pose, @@ -170,6 +170,6 @@ class LaneDepartureChecker const std::vector & vehicle_footprints, const SegmentRtree & uncrossable_segments); }; -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker -#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp similarity index 81% rename from control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp rename to control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp index 0f0e15d0a4f62..d37ef30d6b8e8 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/lane_departure_checker_node.hpp @@ -12,10 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#define LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ -#include "lane_departure_checker/lane_departure_checker.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include @@ -40,7 +41,7 @@ #include #include -namespace lane_departure_checker +namespace autoware::lane_departure_checker { using autoware_map_msgs::msg::LaneletMapBin; @@ -66,11 +67,16 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; - rclcpp::Subscription::SharedPtr sub_route_; - rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_lanelet_map_bin_{ + this, "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local()}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_route_{ + this, "~/input/route"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_reference_trajectory_{ + this, "~/input/reference_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_trajectory_{ + this, "~/input/predicted_trajectory"}; // Data Buffer nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; @@ -149,6 +155,6 @@ class LaneDepartureCheckerNode : public rclcpp::Node lanelet::Lanelets getRightOppositeLanelets(const lanelet::ConstLanelet & lanelet); }; -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker -#endif // LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__LANE_DEPARTURE_CHECKER_NODE_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp b/control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp similarity index 89% rename from control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp rename to control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp index edc62cd6659fd..c85e37212b16f 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/util/create_vehicle_footprint.hpp +++ b/control/lane_departure_checker/include/autoware_lane_departure_checker/util/create_vehicle_footprint.hpp @@ -25,12 +25,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#ifndef AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ #include +#include #include -#include #include @@ -63,4 +63,4 @@ inline FootprintMargin calcFootprintMargin( return FootprintMargin{Cov_xy_vehicle(0, 0) * scale, Cov_xy_vehicle(1, 1) * scale}; } -#endif // LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ +#endif // AUTOWARE_LANE_DEPARTURE_CHECKER__UTIL__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml index 62799c1187651..b9f820f66690b 100644 --- a/control/lane_departure_checker/launch/lane_departure_checker.launch.xml +++ b/control/lane_departure_checker/launch/lane_departure_checker.launch.xml @@ -4,12 +4,12 @@ - + - + - + diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 73955613d21a9..7a08cd2907120 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -1,11 +1,13 @@ - lane_departure_checker + autoware_lane_departure_checker 0.1.0 - The lane_departure_checker package + The autoware_lane_departure_checker package Kyoichi Sugahara Makoto Kurihara + Zulfaqar Azmi + Apache License 2.0 ament_cmake_auto @@ -13,6 +15,7 @@ autoware_map_msgs autoware_planning_msgs + autoware_vehicle_info_utils boost diagnostic_updater eigen @@ -28,7 +31,6 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index eb5d674705aaa..5e90df6773b87 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lane_departure_checker/lane_departure_checker.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker.hpp" -#include "lane_departure_checker/util/create_vehicle_footprint.hpp" +#include "autoware_lane_departure_checker/util/create_vehicle_footprint.hpp" #include #include @@ -96,7 +96,7 @@ lanelet::ConstLanelets getCandidateLanelets( } // namespace -namespace lane_departure_checker +namespace autoware::lane_departure_checker { Output LaneDepartureChecker::update(const Input & input) { @@ -460,4 +460,4 @@ bool LaneDepartureChecker::willCrossBoundary( return false; } -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index c3fd1d314e371..b25b3fd843f4b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "lane_departure_checker/lane_departure_checker_node.hpp" +#include "autoware_lane_departure_checker/lane_departure_checker_node.hpp" #include #include @@ -120,7 +120,7 @@ void update_param( } // namespace -namespace lane_departure_checker +namespace autoware::lane_departure_checker { LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & options) : Node("lane_departure_checker_node", options) @@ -145,7 +145,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o declare_parameter>("boundary_types_to_detect"); // Vehicle Info - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_length_m_ = vehicle_info.vehicle_length_m; // Core Parameter @@ -169,22 +169,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o lane_departure_checker_ = std::make_unique(); lane_departure_checker_->setParam(param_, vehicle_info); - // Subscriber - sub_odom_ = this->create_subscription( - "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1)); - sub_lanelet_map_bin_ = this->create_subscription( - "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(), - std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1)); - sub_route_ = this->create_subscription( - "~/input/route", rclcpp::QoS{1}.transient_local(), - std::bind(&LaneDepartureCheckerNode::onRoute, this, _1)); - sub_reference_trajectory_ = this->create_subscription( - "~/input/reference_trajectory", 1, - std::bind(&LaneDepartureCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = this->create_subscription( - "~/input/predicted_trajectory", 1, - std::bind(&LaneDepartureCheckerNode::onPredictedTrajectory, this, _1)); - // Publisher // Nothing @@ -201,36 +185,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o this, get_clock(), period_ns, std::bind(&LaneDepartureCheckerNode::onTimer, this)); } -void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - current_odom_ = msg; -} - -void LaneDepartureCheckerNode::onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg) -{ - lanelet_map_ = std::make_shared(); - lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); - - // get all shoulder lanes - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); - shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); -} - -void LaneDepartureCheckerNode::onRoute(const LaneletRoute::ConstSharedPtr msg) -{ - route_ = msg; -} - -void LaneDepartureCheckerNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr msg) -{ - reference_trajectory_ = msg; -} - -void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstSharedPtr msg) -{ - predicted_trajectory_ = msg; -} - bool LaneDepartureCheckerNode::isDataReady() { if (!current_odom_) { @@ -300,6 +254,22 @@ void LaneDepartureCheckerNode::onTimer() tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("Total"); + current_odom_ = sub_odom_.takeData(); + route_ = sub_route_.takeData(); + reference_trajectory_ = sub_reference_trajectory_.takeData(); + predicted_trajectory_ = sub_predicted_trajectory_.takeData(); + + const auto lanelet_map_bin_msg = sub_lanelet_map_bin_.takeData(); + if (lanelet_map_bin_msg) { + lanelet_map_ = std::make_shared(); + lanelet::utils::conversion::fromBinMsg( + *lanelet_map_bin_msg, lanelet_map_, &traffic_rules_, &routing_graph_); + + // get all shoulder lanes + lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_); + shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets); + } + if (!isDataReady()) { return; } @@ -759,7 +729,7 @@ lanelet::Lanelets LaneDepartureCheckerNode::getRightOppositeLanelets( return opposite_lanelets; } -} // namespace lane_departure_checker +} // namespace autoware::lane_departure_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(lane_departure_checker::LaneDepartureCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::lane_departure_checker::LaneDepartureCheckerNode) diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index a65d818056bd7..efaefd765e793 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -15,8 +15,8 @@ #ifndef OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ #define OBSTACLE_COLLISION_CHECKER__OBSTACLE_COLLISION_CHECKER_HPP_ +#include #include -#include #include #include @@ -75,7 +75,7 @@ class ObstacleCollisionChecker private: Param param_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; //! This function assumes the input trajectory is sampled dense enough static autoware_planning_msgs::msg::Trajectory resampleTrajectory( @@ -86,7 +86,7 @@ class ObstacleCollisionChecker static std::vector createVehicleFootprints( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); static std::vector createVehiclePassingAreas( const std::vector & vehicle_footprints); diff --git a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml index 078993d064064..f82384534e040 100755 --- a/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml +++ b/control/obstacle_collision_checker/launch/obstacle_collision_checker.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index ef7560755a122..795302abe75f4 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -19,6 +19,7 @@ autoware_cmake autoware_planning_msgs + autoware_vehicle_info_utils boost diagnostic_updater eigen @@ -33,7 +34,6 @@ tf2_geometry_msgs tf2_ros tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 6887be4cedd77..a844fe6a50cbf 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -83,7 +83,7 @@ double calcBrakingDistance( namespace obstacle_collision_checker { ObstacleCollisionChecker::ObstacleCollisionChecker(rclcpp::Node & node) -: vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) +: vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { } @@ -188,7 +188,7 @@ autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( std::vector ObstacleCollisionChecker::createVehicleFootprints( const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // Create vehicle footprint in base_link coordinate const auto local_vehicle_footprint = vehicle_info.createFootprint(param.footprint_margin); diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index cfde4ee849728..402123c81d361 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -14,10 +14,10 @@ #include "obstacle_collision_checker/obstacle_collision_checker_node.hpp" +#include #include #include #include -#include #include #include diff --git a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml index 569d743ba2c4f..c6f03db0fcacc 100644 --- a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml +++ b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index 99439aa7e3ab7..40232469fdb6a 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -4,6 +4,7 @@ The vehicle_cmd_gate package Takamasa Horibe Tomoya Kimura + Takayuki Murooka Apache License 2.0 Takamasa Horibe @@ -12,6 +13,7 @@ rosidl_default_generators autoware_control_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -24,7 +26,6 @@ tier4_control_msgs tier4_system_msgs tier4_vehicle_msgs - vehicle_info_util ament_cmake_gtest ament_lint_auto diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 635ead2430677..82270875b2e17 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -33,7 +33,7 @@ using tier4_autoware_utils::calcYawDeviation; AutonomousMode::AutonomousMode(rclcpp::Node * node) : logger_(node->get_logger()), clock_(node->get_clock()) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); sub_control_cmd_ = node->create_subscription( "control_cmd", 1, [this](const Control::SharedPtr msg) { control_cmd_ = *msg; }); diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index e5abd4285ad5f..54434690f2147 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -18,8 +18,8 @@ #include "data.hpp" #include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" +#include #include -#include #include #include @@ -83,7 +83,7 @@ class AutonomousMode : public ModeChangeBase Control trajectory_follower_control_cmd_; Odometry kinematics_; Trajectory trajectory_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; DebugInfo debug_info_; std::shared_ptr stable_start_time_; // Reset every transition start. diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index 8410e7c78f723..2a7a552c1973d 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -15,6 +15,7 @@ #ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ #define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_ +#include #include #include #include @@ -23,7 +24,6 @@ #include #include #include -#include #include #include @@ -120,7 +120,7 @@ class CollisionChecker // Variables std::shared_ptr debug_ptr_; rclcpp::Node * node_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::vector predicted_object_history_{}; }; } // namespace autoware::motion::control::predicted_path_checker diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index 23696887c7051..6c4832a0dac3e 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -15,6 +15,8 @@ #ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ #define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_ +#include +#include #include #include #include @@ -24,8 +26,6 @@ #include #include #include -#include -#include #include #include @@ -116,7 +116,7 @@ class PredictedPathCheckerNode : public rclcpp::Node // Variables State current_state_{State::DRIVE}; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool is_calling_set_stop_{false}; bool is_stopped_by_node_{false}; diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 957800ad04305..d9299f09dfb6b 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -15,10 +15,10 @@ #ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_ #define PREDICTED_PATH_CHECKER__UTILS_HPP_ +#include #include #include #include -#include #include #include @@ -39,6 +39,7 @@ namespace utils { +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -48,7 +49,6 @@ using geometry_msgs::msg::TransformStamped; using std_msgs::msg::Header; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; using PointArray = std::vector; using TrajectoryPoints = std::vector; @@ -57,7 +57,7 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width); TrajectoryPoint calcInterpolatedPoint( const TrajectoryPoints & trajectory, const geometry_msgs::msg::Point & target_point, @@ -65,7 +65,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & predicted_trajectory_array, const size_t collision_idx, - const double stop_margin, vehicle_info_util::VehicleInfo & vehicle_info); + const double stop_margin, autoware::vehicle_info_utils::VehicleInfo & vehicle_info); bool isInBrakeDistance( const TrajectoryPoints & trajectory, const size_t stop_idx, const double relative_velocity, diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml index a35c11b80c1f7..6af1372a5bb4a 100755 --- a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -7,7 +7,7 @@ - + diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index bca65302dba55..911344c578414 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -14,6 +14,7 @@ autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils boost component_interface_specs component_interface_utils @@ -34,7 +35,6 @@ tier4_control_msgs tier4_external_api_msgs tier4_planning_msgs - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp index 5e8d96805b832..7d392571546b4 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp @@ -27,7 +27,7 @@ CollisionChecker::CollisionChecker( rclcpp::Node * node, std::shared_ptr debug_ptr) : debug_ptr_(std::move(debug_ptr)), node_(node), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo()) + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo()) { } diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index 28ea21f639a0e..38717a1849ad9 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -38,7 +38,7 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); adaptor.init_cli(cli_set_stop_, group_cli_); adaptor.init_sub(sub_stop_state_, this, &PredictedPathCheckerNode::onIsStopped); - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // Node Parameter node_param_.update_rate = declare_parameter("update_rate", 10.0); diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index f1c76ce6eef8f..a9c3e2b24f9a5 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -29,7 +29,7 @@ using tier4_autoware_utils::getRPY; // Utils Functions Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double expand_width) { Polygon2d polygon; @@ -146,7 +146,7 @@ TrajectoryPoint calcInterpolatedPoint( std::pair findStopPoint( TrajectoryPoints & trajectory_array, const size_t collision_idx, const double stop_margin, - vehicle_info_util::VehicleInfo & vehicle_info) + autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { // It returns the stop point and segment of the point on trajectory. diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index b4820aee5cec4..32744d744f95f 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -5,6 +5,7 @@ 0.1.0 The pure_pursuit package Takamasa Horibe + Takayuki Murooka Apache License 2.0 Berkay Karaman @@ -15,6 +16,7 @@ autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils boost geometry_msgs libboost-dev @@ -31,7 +33,6 @@ tier4_autoware_utils tier4_debug_msgs trajectory_follower_base - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index a8232cce5d08d..eddcdb5e362df 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -34,7 +34,7 @@ #include "pure_pursuit/util/planning_utils.hpp" #include "pure_pursuit/util/tf_utils.hpp" -#include +#include #include #include @@ -65,7 +65,7 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) pure_pursuit_ = std::make_unique(); // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); param_.wheel_base = vehicle_info.wheel_base_m; param_.max_steering_angle = vehicle_info.max_steer_angle_rad; diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp index a4b491930df1e..d629c8f549f52 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp @@ -34,7 +34,7 @@ #include "pure_pursuit/util/planning_utils.hpp" #include "pure_pursuit/util/tf_utils.hpp" -#include +#include #include #include @@ -59,7 +59,7 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) pure_pursuit_ = std::make_unique(); // Vehicle Parameters - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); param_.wheel_base = vehicle_info.wheel_base_m; // Node Parameters diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index 885e780c90bcc..a175a6ae9e599 100644 --- a/control/shift_decider/package.xml +++ b/control/shift_decider/package.xml @@ -5,6 +5,7 @@ 0.1.0 The shift_decider package Takamasa Horibe + Takayuki Murooka Apache License 2.0 Takamasa Horibe diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml index 6f2e9c3e8ebc2..b36b4669be04e 100644 --- a/control/trajectory_follower_base/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -22,6 +22,7 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_msgs diagnostic_updater @@ -37,7 +38,6 @@ tf2_ros tier4_autoware_utils tier4_debug_msgs - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md index 3496284efd670..7b9e93e39fad9 100644 --- a/control/trajectory_follower_node/README.md +++ b/control/trajectory_follower_node/README.md @@ -45,7 +45,7 @@ struct LateralSyncData { } } -package mpc_lateral_controller { +package autoware_mpc_lateral_controller { class MPCLateralController { isReady(InputData) override run(InputData) override diff --git a/control/trajectory_follower_node/design/media/Controller.drawio.svg b/control/trajectory_follower_node/design/media/Controller.drawio.svg index 1152ef2b17f59..db4961922bd10 100644 --- a/control/trajectory_follower_node/design/media/Controller.drawio.svg +++ b/control/trajectory_follower_node/design/media/Controller.drawio.svg @@ -140,7 +140,7 @@
trajectory_follower/
- mpc_lateral_controller + autoware_mpc_lateral_controller
diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index 8e9752ba19f66..b342aace69bde 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -15,6 +15,7 @@ #ifndef TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ #define TRAJECTORY_FOLLOWER_NODE__CONTROLLER_NODE_HPP_ +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tf2_ros/buffer.h" @@ -24,7 +25,6 @@ #include "trajectory_follower_base/lateral_controller_base.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" #include "trajectory_follower_node/visibility_control.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 65446dfb3dd01..c57a07155128a 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -21,17 +21,17 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_mpc_lateral_controller + autoware_pid_longitudinal_controller autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs motion_utils - mpc_lateral_controller - pid_longitudinal_controller pure_pursuit rclcpp rclcpp_components tier4_autoware_utils trajectory_follower_base - vehicle_info_util visualization_msgs ros2launch diff --git a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/trajectory_follower_node/param/longitudinal/pid.param.yaml index c39088753fe64..ad6217663fbae 100644 --- a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -69,8 +69,9 @@ max_jerk: 2.0 min_jerk: -5.0 - # pitch - use_trajectory_for_pitch_calculation: false + # slope compensation lpf_pitch_gain: 0.95 + slope_source: "raw_pitch" # raw_pitch, trajectory_pitch or trajectory_adaptive + adaptive_trajectory_velocity_th: 1.0 max_pitch_rad: 0.1 min_pitch_rad: -0.1 diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 6fe63ca07de6f..4be04af5fd184 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -14,8 +14,8 @@ #include "trajectory_follower_node/controller_node.hpp" -#include "mpc_lateral_controller/mpc_lateral_controller.hpp" -#include "pid_longitudinal_controller/pid_longitudinal_controller.hpp" +#include "autoware_mpc_lateral_controller/mpc_lateral_controller.hpp" +#include "autoware_pid_longitudinal_controller/pid_longitudinal_controller.hpp" #include "pure_pursuit/pure_pursuit_lateral_controller.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index 9bdf625226134..dcd3434705ec0 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -51,9 +51,9 @@ rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_c // Pass default parameter file to the node const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_node"); const auto longitudinal_share_dir = - ament_index_cpp::get_package_share_directory("pid_longitudinal_controller"); + ament_index_cpp::get_package_share_directory("autoware_pid_longitudinal_controller"); const auto lateral_share_dir = - ament_index_cpp::get_package_share_directory("mpc_lateral_controller"); + ament_index_cpp::get_package_share_directory("autoware_mpc_lateral_controller"); rclcpp::NodeOptions node_options; node_options.append_parameter_override("lateral_controller_mode", "mpc"); node_options.append_parameter_override("longitudinal_controller_mode", "pid"); diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 926fbb7435f3a..eaa07f2317940 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace perception_diagnostics @@ -36,7 +37,11 @@ enum class Metric { SIZE, }; +// Each metric has a different return type. (statistic or just a one value etc). +// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; +using MetricValueMap = std::unordered_map; +using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index ec435b0a5e17f..a9b1388281ce8 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricStatMap calcObjectsCountMetrics() const; + MetricValueMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index daaea56178873..1bc427e667a2a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; + DiagnosticStatus generateDiagnosticStatus( + const std::string & metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index be335a2d78fa3..b05bb5e1d8ad3 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -15,7 +15,7 @@ #ifndef PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ #define PERCEPTION_ONLINE_EVALUATOR__UTILS__MARKER_UTILS_HPP_ -#include +#include #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include @@ -45,10 +45,10 @@ inline int64_t bitShift(int64_t original_id) void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); MarkerArray createPointsMarkerArray( diff --git a/evaluator/perception_online_evaluator/package.xml b/evaluator/perception_online_evaluator/package.xml index 3f855e2f1f887..d64d3a5ec4957 100644 --- a/evaluator/perception_online_evaluator/package.xml +++ b/evaluator/perception_online_evaluator/package.xml @@ -18,6 +18,7 @@ autoware_cmake autoware_perception_msgs + autoware_vehicle_info_utils diagnostic_msgs eigen geometry_msgs @@ -32,7 +33,6 @@ tf2 tf2_ros tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 9c1e0667e4fef..cc455445ca9f8 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -26,7 +26,7 @@ namespace perception_diagnostics using object_recognition_utils::convertLabelToString; using tier4_autoware_utils::inverseTransformPoint; -std::optional MetricsCalculator::calculate(const Metric & metric) const +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const +MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricStatMap metric_stat_map; + MetricValueMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } // calculate the average number of objects in the detection area in the past @@ -472,8 +471,8 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] - .add(count); + metric_stat_map + ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } @@ -481,8 +480,7 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 0fcdd77f4d515..2933afdab3d08 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); - if (!metric_stat_map.has_value()) { + const auto metric_result = metrics_calculator_.calculate(Metric(metric)); + if (!metric_result.has_value()) { continue; } - for (const auto & [metric, stat] : metric_stat_map.value()) { - if (stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); - } - } + std::visit( + [&metrics_msg, this](auto && arg) { + using T = std::decay_t; + for (const auto & [metric, value] : arg) { + if constexpr (std::is_same_v) { + if (value.count() > 0) { + metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + } + } else if constexpr (std::is_same_v) { + metrics_msg.status.emplace_back(generateDiagnosticStatus(metric, value)); + } + } + }, + metric_result.value()); } // publish metrics @@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string & metric, const double value) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(value); + status.values.push_back(key_value); + + return status; +} + void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); diff --git a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp index 523e11883e755..eddeadc331101 100644 --- a/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp +++ b/evaluator/perception_online_evaluator/src/utils/marker_utils.cpp @@ -39,7 +39,7 @@ using visualization_msgs::msg::Marker; void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const double half_width = vehicle_info.vehicle_width_m / 2.0; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; @@ -57,7 +57,7 @@ void addFootprintMarker( } MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index a408821466fb9..3db487f3167ae 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - metric_value_ = boost::lexical_cast(it->values[2].value); + const auto mean_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "mean"; }); + if (mean_it != it->values.end()) { + metric_value_ = boost::lexical_cast(mean_it->value); + } else { + const auto metric_value_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "metric_value"; }); + if (metric_value_it != it->values.end()) { + metric_value_ = boost::lexical_cast(metric_value_it->value); + } + } metric_updated_ = true; } }); diff --git a/launch/tier4_control_launch/control_launch.drawio.svg b/launch/tier4_control_launch/control_launch.drawio.svg index 5d13b84a0b34b..21d97ba08c0b3 100644 --- a/launch/tier4_control_launch/control_launch.drawio.svg +++ b/launch/tier4_control_launch/control_launch.drawio.svg @@ -259,17 +259,17 @@ >
- lane_departure_checker_node + autoware_lane_departure_checker_node

- package: lane_departure_checker_node + package: autoware_lane_departure_checker_node
- lane_departure_checker_node... + autoware_lane_departure_checker_node... diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 77140b0e0f630..1183514e9a3cb 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -102,8 +102,8 @@ def launch_setup(context, *args, **kwargs): # lane departure checker lane_departure_component = ComposableNode( - package="lane_departure_checker", - plugin="lane_departure_checker::LaneDepartureCheckerNode", + package="autoware_lane_departure_checker", + plugin="autoware::lane_departure_checker::LaneDepartureCheckerNode", name="lane_departure_checker_node", namespace="trajectory_follower", remappings=[ @@ -139,7 +139,7 @@ def launch_setup(context, *args, **kwargs): # autonomous emergency braking autonomous_emergency_braking = ComposableNode( - package="autonomous_emergency_braking", + package="autoware_autonomous_emergency_braking", plugin="autoware::motion::control::autonomous_emergency_braking::AEB", name="autonomous_emergency_braking", remappings=[ @@ -194,8 +194,8 @@ def launch_setup(context, *args, **kwargs): # vehicle cmd gate vehicle_cmd_gate_component = ComposableNode( - package="vehicle_cmd_gate", - plugin="vehicle_cmd_gate::VehicleCmdGate", + package="autoware_vehicle_cmd_gate", + plugin="autoware::vehicle_cmd_gate::VehicleCmdGate", name="vehicle_cmd_gate", remappings=[ ("input/steering", "/vehicle/status/steering_status"), @@ -272,7 +272,10 @@ def launch_setup(context, *args, **kwargs): # external cmd selector external_cmd_selector_loader = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("external_cmd_selector"), "/launch/external_cmd_selector.launch.py"] + [ + FindPackageShare("autoware_external_cmd_selector"), + "/launch/external_cmd_selector.launch.py", + ] ), launch_arguments=[ ("use_intra_process", LaunchConfiguration("use_intra_process")), @@ -432,7 +435,7 @@ def launch_setup(context, *args, **kwargs): ) smart_mpc_trajectory_follower = Node( - package="smart_mpc_trajectory_follower", + package="autoware_smart_mpc_trajectory_follower", executable="pympc_trajectory_follower.py", name="pympc_trajectory_follower", ) diff --git a/launch/tier4_control_launch/package.xml b/launch/tier4_control_launch/package.xml index 801fa274dd086..f02ec0aee47b6 100644 --- a/launch/tier4_control_launch/package.xml +++ b/launch/tier4_control_launch/package.xml @@ -11,13 +11,13 @@ ament_cmake_auto autoware_cmake + autoware_external_cmd_selector + autoware_lane_departure_checker + autoware_vehicle_cmd_gate control_evaluator external_cmd_converter - external_cmd_selector - lane_departure_checker shift_decider trajectory_follower_node - vehicle_cmd_gate ament_lint_auto autoware_lint_common diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 9c3e815e8abb4..7e389f5790051 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml index f941bc8d771b1..274b39be681b8 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detector/lidar_dnn_detector.launch.xml @@ -2,16 +2,41 @@ - + - + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml index fa343f49840ad..a9ba980b3ce47 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/prediction/prediction.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index e1a2260f3ee6d..3ce78e3b29f29 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -47,7 +47,7 @@ - + diff --git a/launch/tier4_perception_launch/package.xml b/launch/tier4_perception_launch/package.xml index 2d007ae8c5ff0..c74ef297aba6b 100644 --- a/launch/tier4_perception_launch/package.xml +++ b/launch/tier4_perception_launch/package.xml @@ -12,6 +12,7 @@ ament_cmake_auto autoware_cmake + autoware_map_based_prediction cluster_merger compare_map_segmentation crosswalk_traffic_light_estimator @@ -23,7 +24,6 @@ image_projection_based_fusion image_transport_decompressor lidar_apollo_instance_segmentation - map_based_prediction multi_object_tracker object_merger object_range_splitter diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 97bf6414189ab..1d01732454daf 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -32,7 +32,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 4cf0c541510ed..463740d978ca6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -32,7 +32,6 @@ - @@ -92,7 +91,7 @@ /> @@ -101,7 +100,7 @@ - @@ -261,7 +255,6 @@ - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 5fcbfe512e3e1..eba79b905db16 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -2,8 +2,8 @@ - - + + @@ -11,13 +11,13 @@ - + + + + + @@ -28,7 +28,7 @@ - + @@ -62,7 +62,7 @@ - + @@ -80,7 +80,7 @@ - + @@ -146,7 +146,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 98315919b540a..e0fdcb1e30408 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -4,7 +4,7 @@ - + @@ -18,7 +18,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 6a7e00154256a..a252986f9d957 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -3,7 +3,7 @@ - + @@ -27,7 +27,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 7b03097e55e10..1c881091b0e7d 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,23 +58,23 @@ autoware_cmake autoware_behavior_velocity_planner + autoware_costmap_generator + autoware_external_cmd_selector autoware_external_velocity_limit_selector + autoware_freespace_planner autoware_path_optimizer autoware_planning_topic_converter + autoware_planning_validator autoware_remaining_distance_time_calculator + autoware_scenario_selector autoware_surround_obstacle_checker autoware_velocity_smoother behavior_path_planner - costmap_generator - external_cmd_selector - freespace_planner glog_component mission_planner obstacle_cruise_planner obstacle_stop_planner planning_evaluator - planning_validator - scenario_selector ament_lint_auto autoware_lint_common diff --git a/launch/tier4_sensing_launch/launch/sensing.launch.xml b/launch/tier4_sensing_launch/launch/sensing.launch.xml index 391a1f8bad641..9981c99a8f3fc 100644 --- a/launch/tier4_sensing_launch/launch/sensing.launch.xml +++ b/launch/tier4_sensing_launch/launch/sensing.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/tier4_sensing_launch/package.xml b/launch/tier4_sensing_launch/package.xml index a1f10ee743db5..410f685285b7d 100644 --- a/launch/tier4_sensing_launch/package.xml +++ b/launch/tier4_sensing_launch/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - vehicle_info_util + autoware_vehicle_info_utils ament_lint_auto autoware_lint_common diff --git a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp index 737c25f8ce024..880ab82418e06 100644 --- a/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp @@ -22,11 +22,11 @@ template class AgedObjectQueue { public: - explicit AgedObjectQueue(const int max_age) : max_age_(max_age) {} + explicit AgedObjectQueue(const size_t max_age) : max_age_(max_age) {} - bool empty() const { return this->size() == 0; } + [[nodiscard]] bool empty() const { return this->size() == 0; } - size_t size() const { return objects_.size(); } + [[nodiscard]] size_t size() const { return objects_.size(); } Object back() const { return objects_.back(); } @@ -39,7 +39,7 @@ class AgedObjectQueue Object pop_increment_age() { const Object object = objects_.front(); - const int age = ages_.front() + 1; + const size_t age = ages_.front() + 1; objects_.pop(); ages_.pop(); @@ -54,13 +54,13 @@ class AgedObjectQueue void clear() { objects_ = std::queue(); - ages_ = std::queue(); + ages_ = std::queue(); } private: - const int max_age_; + const size_t max_age_; std::queue objects_; - std::queue ages_; + std::queue ages_; }; #endif // EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/covariance.hpp b/localization/ekf_localizer/include/ekf_localizer/covariance.hpp index a4448ecda2f45..713877c1307d2 100644 --- a/localization/ekf_localizer/include/ekf_localizer/covariance.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/covariance.hpp @@ -17,7 +17,7 @@ #include "ekf_localizer/matrix_types.hpp" -std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P); -std::array ekfCovarianceToTwistMessageCovariance(const Matrix6d & P); +std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P); +std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P); #endif // EKF_LOCALIZER__COVARIANCE_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp index f4dc6436f6a40..4c92b2947642b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp @@ -20,21 +20,21 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated); +diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( const std::string & measurement_type, const size_t queue_size); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, const double delay_time_threshold); -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( const std::string & measurement_type, const bool is_passed_mahalanobis_gate, const double mahalanobis_distance, const double mahalanobis_distance_threshold); -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array); #endif // EKF_LOCALIZER__DIAGNOSTICS_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index 8ed925c1bc228..b78e9b1ee0469 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -58,17 +58,15 @@ class Simple1DFilter x_ = 0; dev_ = 1e9; proc_dev_x_c_ = 0.0; - return; }; - void init(const double init_obs, const double obs_dev, const rclcpp::Time time) + void init(const double init_obs, const double obs_dev, const rclcpp::Time & time) { x_ = init_obs; dev_ = obs_dev; latest_time_ = time; initialized_ = true; - return; }; - void update(const double obs, const double obs_dev, const rclcpp::Time time) + void update(const double obs, const double obs_dev, const rclcpp::Time & time) { if (!initialized_) { init(obs, obs_dev, time); @@ -86,10 +84,9 @@ class Simple1DFilter dev_ = (1 - kalman_gain) * dev_; latest_time_ = time; - return; }; void set_proc_dev(const double proc_dev) { proc_dev_x_c_ = proc_dev; } - double get_x() const { return x_; } + [[nodiscard]] double get_x() const { return x_; } private: bool initialized_; @@ -158,10 +155,9 @@ class EKFLocalizer : public rclcpp::Node double ekf_dt_; /* process noise variance for discrete model */ - double proc_cov_yaw_d_; //!< @brief discrete yaw process noise - double proc_cov_yaw_bias_d_; //!< @brief discrete yaw bias process noise - double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 - double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 + double proc_cov_yaw_d_; //!< @brief discrete yaw process noise + double proc_cov_vx_d_; //!< @brief discrete process noise in d_vx=0 + double proc_cov_wz_d_; //!< @brief discrete process noise in d_wz=0 bool is_activated_; @@ -174,44 +170,45 @@ class EKFLocalizer : public rclcpp::Node /** * @brief computes update & prediction of EKF for each ekf_dt_[s] time */ - void timerCallback(); + void timer_callback(); /** * @brief publish tf for tf_rate [Hz] */ - void timerTFCallback(); + void timer_tf_callback(); /** - * @brief set poseWithCovariance measurement + * @brief set pose with covariance measurement */ - void callbackPoseWithCovariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void callback_pose_with_covariance(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** - * @brief set twistWithCovariance measurement + * @brief set twist with covariance measurement */ - void callbackTwistWithCovariance(geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void callback_twist_with_covariance( + geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); /** * @brief set initial_pose to current EKF pose */ - void callbackInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); + void callback_initial_pose(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); /** * @brief update predict frequency */ - void updatePredictFrequency(const rclcpp::Time & current_time); + void update_predict_frequency(const rclcpp::Time & current_time); /** * @brief get transform from frame_id */ - bool getTransformFromTF( + bool get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform); /** * @brief publish current EKF estimation result */ - void publishEstimateResult( + void publish_estimate_result( const geometry_msgs::msg::PoseStamped & current_ekf_pose, const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist); @@ -219,23 +216,23 @@ class EKFLocalizer : public rclcpp::Node /** * @brief publish diagnostics message */ - void publishDiagnostics(const rclcpp::Time & current_time); + void publish_diagnostics(const rclcpp::Time & current_time); /** - * @brief update simple1DFilter + * @brief update simple 1d filter */ - void updateSimple1DFilters( + void update_simple_1d_filters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step); /** - * @brief initialize simple1DFilter + * @brief initialize simple 1d filter */ - void initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + void init_simple_1d_filters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); /** * @brief trigger node */ - void serviceTriggerNode( + void service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res); diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp index e88a59ffdfab9..ee360e798f492 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp @@ -34,24 +34,13 @@ struct EKFDiagnosticInfo { - EKFDiagnosticInfo() - : no_update_count(0), - queue_size(0), - is_passed_delay_gate(true), - delay_time(0), - delay_time_threshold(0), - is_passed_mahalanobis_gate(true), - mahalanobis_distance(0) - { - } - - size_t no_update_count; - size_t queue_size; - bool is_passed_delay_gate; - double delay_time; - double delay_time_threshold; - bool is_passed_mahalanobis_gate; - double mahalanobis_distance; + size_t no_update_count{0}; + size_t queue_size{0}; + bool is_passed_delay_gate{true}; + double delay_time{0.0}; + double delay_time_threshold{0.0}; + bool is_passed_mahalanobis_gate{true}; + double mahalanobis_distance{0.0}; }; class EKFModule @@ -63,31 +52,32 @@ class EKFModule using Twist = geometry_msgs::msg::TwistStamped; public: - EKFModule(std::shared_ptr warning, const HyperParameters params); + EKFModule(std::shared_ptr warning, const HyperParameters & params); void initialize( const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform); - geometry_msgs::msg::PoseStamped getCurrentPose( + [[nodiscard]] geometry_msgs::msg::PoseStamped get_current_pose( const rclcpp::Time & current_time, const double z, const double roll, const double pitch, bool get_biased_yaw) const; - geometry_msgs::msg::TwistStamped getCurrentTwist(const rclcpp::Time & current_time) const; - double getYawBias() const; - std::array getCurrentPoseCovariance() const; - std::array getCurrentTwistCovariance() const; + [[nodiscard]] geometry_msgs::msg::TwistStamped get_current_twist( + const rclcpp::Time & current_time) const; + [[nodiscard]] double get_yaw_bias() const; + [[nodiscard]] std::array get_current_pose_covariance() const; + [[nodiscard]] std::array get_current_twist_covariance() const; - size_t find_closest_delay_time_index(double target_value) const; + [[nodiscard]] size_t find_closest_delay_time_index(double target_value) const; void accumulate_delay_time(const double dt); - void predictWithDelay(const double dt); - bool measurementUpdatePose( + void predict_with_delay(const double dt); + bool measurement_update_pose( const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info); - bool measurementUpdateTwist( + bool measurement_update_twist( const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info); - geometry_msgs::msg::PoseWithCovarianceStamped compensatePoseWithZDelay( + geometry_msgs::msg::PoseWithCovarianceStamped compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time); private: diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index 56a13d1ecaf55..8d76102e5d64d 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -68,14 +68,14 @@ class HyperParameters const double tf_rate_; const bool publish_tf_; const bool enable_yaw_bias_estimation; - const int extend_state_step; + const size_t extend_state_step; const std::string pose_frame_id; const double pose_additional_delay; const double pose_gate_dist; - const int pose_smoothing_steps; + const size_t pose_smoothing_steps; const double twist_additional_delay; const double twist_gate_dist; - const int twist_smoothing_steps; + const size_t twist_smoothing_steps; const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise diff --git a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp index f2b9dc626e67a..1f0d75c5958d4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp @@ -18,7 +18,7 @@ #include #include -double squaredMahalanobis( +double squared_mahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C); diff --git a/localization/ekf_localizer/include/ekf_localizer/measurement.hpp b/localization/ekf_localizer/include/ekf_localizer/measurement.hpp index 4b2169f1b182f..396aaf7b9a1b4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/measurement.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/measurement.hpp @@ -17,11 +17,11 @@ #include -Eigen::Matrix poseMeasurementMatrix(); -Eigen::Matrix twistMeasurementMatrix(); -Eigen::Matrix3d poseMeasurementCovariance( +Eigen::Matrix pose_measurement_matrix(); +Eigen::Matrix twist_measurement_matrix(); +Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step); -Eigen::Matrix2d twistMeasurementCovariance( +Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step); #endif // EKF_LOCALIZER__MEASUREMENT_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp index 6554f80aee014..358a2750bd3a8 100644 --- a/localization/ekf_localizer/include/ekf_localizer/numeric.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/numeric.hpp @@ -19,12 +19,12 @@ #include -inline bool hasInf(const Eigen::MatrixXd & v) +inline bool has_inf(const Eigen::MatrixXd & v) { return v.array().isInf().any(); } -inline bool hasNan(const Eigen::MatrixXd & v) +inline bool has_nan(const Eigen::MatrixXd & v) { return v.array().isNaN().any(); } diff --git a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp index 85a65828e4ee4..09a87e5fe154b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/state_transition.hpp @@ -17,10 +17,10 @@ #include "ekf_localizer/matrix_types.hpp" -double normalizeYaw(const double & yaw); -Vector6d predictNextState(const Vector6d & X_curr, const double dt); -Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt); -Matrix6d processNoiseCovariance( +double normalize_yaw(const double & yaw); +Vector6d predict_next_state(const Vector6d & X_curr, const double dt); +Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt); +Matrix6d process_noise_covariance( const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d); #endif // EKF_LOCALIZER__STATE_TRANSITION_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/string.hpp b/localization/ekf_localizer/include/ekf_localizer/string.hpp index a4cd1f320367c..0154e84b88004 100644 --- a/localization/ekf_localizer/include/ekf_localizer/string.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/string.hpp @@ -17,7 +17,7 @@ #include -inline std::string eraseLeadingSlash(const std::string & s) +inline std::string erase_leading_slash(const std::string & s) { std::string a = s; if (a.front() == '/') { diff --git a/localization/ekf_localizer/include/ekf_localizer/warning.hpp b/localization/ekf_localizer/include/ekf_localizer/warning.hpp index a3c8800242e2b..e7456d73ecfdd 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning.hpp @@ -29,7 +29,7 @@ class Warning RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); } - void warnThrottle(const std::string & message, const int duration_milliseconds) const + void warn_throttle(const std::string & message, const int duration_milliseconds) const { RCLCPP_WARN_THROTTLE( node_->get_logger(), *(node_->get_clock()), diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index e1eafdc6f7948..36c0eabd588fa 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,11 +17,12 @@ #include -std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold); -std::string twistDelayStepWarningMessage( +std::string pose_delay_step_warning_message( const double delay_time, const double delay_time_threshold); -std::string poseDelayTimeWarningMessage(const double delay_time); -std::string twistDelayTimeWarningMessage(const double delay_time); -std::string mahalanobisWarningMessage(const double distance, const double max_distance); +std::string twist_delay_step_warning_message( + const double delay_time, const double delay_time_threshold); +std::string pose_delay_time_warning_message(const double delay_time); +std::string twist_delay_time_warning_message(const double delay_time); +std::string mahalanobis_warning_message(const double distance, const double max_distance); #endif // EKF_LOCALIZER__WARNING_MESSAGE_HPP_ diff --git a/localization/ekf_localizer/src/covariance.cpp b/localization/ekf_localizer/src/covariance.cpp index 0c98ac6857ea5..fc16abf429d93 100644 --- a/localization/ekf_localizer/src/covariance.cpp +++ b/localization/ekf_localizer/src/covariance.cpp @@ -19,9 +19,9 @@ using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; -std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P) +std::array ekf_covariance_to_pose_message_covariance(const Matrix6d & P) { - std::array covariance; + std::array covariance{}; covariance.fill(0.); covariance[COV_IDX::X_X] = P(IDX::X, IDX::X); @@ -37,9 +37,9 @@ std::array ekfCovarianceToPoseMessageCovariance(const Matrix6d & P) return covariance; } -std::array ekfCovarianceToTwistMessageCovariance(const Matrix6d & P) +std::array ekf_covariance_to_twist_message_covariance(const Matrix6d & P) { - std::array covariance; + std::array covariance{}; covariance.fill(0.); covariance[COV_IDX::X_X] = P(IDX::VX, IDX::VX); diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp index 9ff36561abaa9..ecbeaf8b2445a 100644 --- a/localization/ekf_localizer/src/diagnostics.cpp +++ b/localization/ekf_localizer/src/diagnostics.cpp @@ -19,7 +19,7 @@ #include #include -diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated) +diagnostic_msgs::msg::DiagnosticStatus check_process_activated(const bool is_activated) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -38,7 +38,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activ return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_updated( const std::string & measurement_type, const size_t no_update_count, const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) { @@ -69,7 +69,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_queue_size( const std::string & measurement_type, const size_t queue_size) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -85,7 +85,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_delay_gate( const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, const double delay_time_threshold) { @@ -112,7 +112,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( return stat; } -diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( +diagnostic_msgs::msg::DiagnosticStatus check_measurement_mahalanobis_gate( const std::string & measurement_type, const bool is_passed_mahalanobis_gate, const double mahalanobis_distance, const double mahalanobis_distance_threshold) { @@ -141,7 +141,7 @@ diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( // The highest level within the stat_array will be reflected in the merged_stat. // When all stat_array entries are 'OK,' the message of merged_stat will be "OK" -diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( +diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status( const std::vector & stat_array) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 687e812679574..8e7121ec73a12 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -34,8 +34,8 @@ #include // clang-format off -#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl -#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} +#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl // NOLINT +#define DEBUG_INFO(...) {if (params_.show_debug_info) {RCLCPP_INFO(__VA_ARGS__);}} // NOLINT // clang-format on using std::placeholders::_1; @@ -58,12 +58,12 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) /* initialize ros system */ timer_control_ = rclcpp::create_timer( this, get_clock(), rclcpp::Duration::from_seconds(ekf_dt_), - std::bind(&EKFLocalizer::timerCallback, this)); + std::bind(&EKFLocalizer::timer_callback, this)); if (params_.publish_tf_) { timer_tf_ = rclcpp::create_timer( this, get_clock(), rclcpp::Rate(params_.tf_rate_).period(), - std::bind(&EKFLocalizer::timerTFCallback, this)); + std::bind(&EKFLocalizer::timer_tf_callback, this)); } pub_pose_ = create_publisher("ekf_pose", 1); @@ -79,15 +79,17 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) "ekf_biased_pose_with_covariance", 1); pub_diag_ = this->create_publisher("/diagnostics", 10); sub_initialpose_ = create_subscription( - "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); + "initialpose", 1, std::bind(&EKFLocalizer::callback_initial_pose, this, _1)); sub_pose_with_cov_ = create_subscription( - "in_pose_with_covariance", 1, std::bind(&EKFLocalizer::callbackPoseWithCovariance, this, _1)); + "in_pose_with_covariance", 1, + std::bind(&EKFLocalizer::callback_pose_with_covariance, this, _1)); sub_twist_with_cov_ = create_subscription( - "in_twist_with_covariance", 1, std::bind(&EKFLocalizer::callbackTwistWithCovariance, this, _1)); + "in_twist_with_covariance", 1, + std::bind(&EKFLocalizer::callback_twist_with_covariance, this, _1)); service_trigger_node_ = create_service( "trigger_node_srv", std::bind( - &EKFLocalizer::serviceTriggerNode, this, std::placeholders::_1, std::placeholders::_2), + &EKFLocalizer::service_trigger_node, this, std::placeholders::_1, std::placeholders::_2), rclcpp::ServicesQoS().get_rmw_qos_profile()); tf_br_ = std::make_shared( @@ -102,9 +104,9 @@ EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) } /* - * updatePredictFrequency + * update_predict_frequency */ -void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) +void EKFLocalizer::update_predict_frequency(const rclcpp::Time & current_time) { if (last_predict_time_) { if (current_time < *last_predict_time_) { @@ -119,7 +121,7 @@ void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) ekf_dt_ = 10.0; RCLCPP_WARN( get_logger(), "Large ekf_dt_ detected!! (%f sec) Capped to 10.0 seconds", ekf_dt_); - } else if (ekf_dt_ > params_.pose_smoothing_steps / params_.ekf_rate) { + } else if (ekf_dt_ > static_cast(params_.pose_smoothing_steps) / params_.ekf_rate) { RCLCPP_WARN( get_logger(), "EKF period may be too slow to finish pose smoothing!! (%f sec) ", ekf_dt_); } @@ -137,28 +139,28 @@ void EKFLocalizer::updatePredictFrequency(const rclcpp::Time & current_time) } /* - * timerCallback + * timer_callback */ -void EKFLocalizer::timerCallback() +void EKFLocalizer::timer_callback() { const rclcpp::Time current_time = this->now(); if (!is_activated_) { - warning_->warnThrottle( + warning_->warn_throttle( "The node is not activated. Provide initial pose to pose_initializer", 2000); - publishDiagnostics(current_time); + publish_diagnostics(current_time); return; } DEBUG_INFO(get_logger(), "========================= timer called ========================="); /* update predict frequency with measured timer rate */ - updatePredictFrequency(current_time); + update_predict_frequency(current_time); /* predict model in EKF */ stop_watch_.tic(); DEBUG_INFO(get_logger(), "------------------------- start prediction -------------------------"); - ekf_module_->predictWithDelay(ekf_dt_); + ekf_module_->predict_with_delay(ekf_dt_); DEBUG_INFO(get_logger(), "[EKF] predictKinematicsModel calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); @@ -177,22 +179,22 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = current_time; const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdatePose(*pose, t_curr, pose_diag_info_); + bool is_updated = ekf_module_->measurement_update_pose(*pose, current_time, pose_diag_info_); if (is_updated) { pose_is_updated = true; // Update Simple 1D filter with considering change of z value due to measurement pose delay const double delay_time = - (t_curr - pose->header.stamp).seconds() + params_.pose_additional_delay; - const auto pose_with_z_delay = ekf_module_->compensatePoseWithZDelay(*pose, delay_time); - updateSimple1DFilters(pose_with_z_delay, params_.pose_smoothing_steps); + (current_time - pose->header.stamp).seconds() + params_.pose_additional_delay; + const auto pose_with_z_delay = ekf_module_->compensate_pose_with_z_delay(*pose, delay_time); + update_simple_1d_filters(pose_with_z_delay, params_.pose_smoothing_steps); } } - DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO( + get_logger(), "[EKF] measurement_update_pose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } pose_diag_info_.no_update_count = pose_is_updated ? 0 : (pose_diag_info_.no_update_count + 1); @@ -212,16 +214,17 @@ void EKFLocalizer::timerCallback() stop_watch_.tic(); // save the initial size because the queue size can change in the loop - const auto t_curr = current_time; const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - bool is_updated = ekf_module_->measurementUpdateTwist(*twist, t_curr, twist_diag_info_); + bool is_updated = + ekf_module_->measurement_update_twist(*twist, current_time, twist_diag_info_); if (is_updated) { twist_is_updated = true; } } - DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); + DEBUG_INFO( + get_logger(), "[EKF] measurement_update_twist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } twist_diag_info_.no_update_count = twist_is_updated ? 0 : (twist_diag_info_.no_update_count + 1); @@ -230,27 +233,27 @@ void EKFLocalizer::timerCallback() const double roll = roll_filter_.get_x(); const double pitch = pitch_filter_.get_x(); const geometry_msgs::msg::PoseStamped current_ekf_pose = - ekf_module_->getCurrentPose(current_time, z, roll, pitch, false); + ekf_module_->get_current_pose(current_time, z, roll, pitch, false); const geometry_msgs::msg::PoseStamped current_biased_ekf_pose = - ekf_module_->getCurrentPose(current_time, z, roll, pitch, true); + ekf_module_->get_current_pose(current_time, z, roll, pitch, true); const geometry_msgs::msg::TwistStamped current_ekf_twist = - ekf_module_->getCurrentTwist(current_time); + ekf_module_->get_current_twist(current_time); /* publish ekf result */ - publishEstimateResult(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); - publishDiagnostics(current_time); + publish_estimate_result(current_ekf_pose, current_biased_ekf_pose, current_ekf_twist); + publish_diagnostics(current_time); } /* - * timerTFCallback + * timer_tf_callback */ -void EKFLocalizer::timerTFCallback() +void EKFLocalizer::timer_tf_callback() { if (!is_activated_) { return; } - if (params_.pose_frame_id == "") { + if (params_.pose_frame_id.empty()) { return; } @@ -262,15 +265,15 @@ void EKFLocalizer::timerTFCallback() geometry_msgs::msg::TransformStamped transform_stamped; transform_stamped = tier4_autoware_utils::pose2transform( - ekf_module_->getCurrentPose(current_time, z, roll, pitch, false), "base_link"); + ekf_module_->get_current_pose(current_time, z, roll, pitch, false), "base_link"); transform_stamped.header.stamp = current_time; tf_br_->sendTransform(transform_stamped); } /* - * getTransformFromTF + * get_transform_from_tf */ -bool EKFLocalizer::getTransformFromTF( +bool EKFLocalizer::get_transform_from_tf( std::string parent_frame, std::string child_frame, geometry_msgs::msg::TransformStamped & transform) { @@ -278,8 +281,8 @@ bool EKFLocalizer::getTransformFromTF( tf2_ros::TransformListener tf_listener(tf_buffer); rclcpp::sleep_for(std::chrono::milliseconds(100)); - parent_frame = eraseLeadingSlash(parent_frame); - child_frame = eraseLeadingSlash(child_frame); + parent_frame = erase_leading_slash(parent_frame); + child_frame = erase_leading_slash(child_frame); for (int i = 0; i < 50; ++i) { try { @@ -294,25 +297,25 @@ bool EKFLocalizer::getTransformFromTF( } /* - * callbackInitialPose + * callback_initial_pose */ -void EKFLocalizer::callbackInitialPose( - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr initialpose) +void EKFLocalizer::callback_initial_pose( + geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { geometry_msgs::msg::TransformStamped transform; - if (!getTransformFromTF(params_.pose_frame_id, initialpose->header.frame_id, transform)) { + if (!get_transform_from_tf(params_.pose_frame_id, msg->header.frame_id, transform)) { RCLCPP_ERROR( get_logger(), "[EKF] TF transform failed. parent = %s, child = %s", - params_.pose_frame_id.c_str(), initialpose->header.frame_id.c_str()); + params_.pose_frame_id.c_str(), msg->header.frame_id.c_str()); } - ekf_module_->initialize(*initialpose, transform); - initSimple1DFilters(*initialpose); + ekf_module_->initialize(*msg, transform); + init_simple_1d_filters(*msg); } /* - * callbackPoseWithCovariance + * callback_pose_with_covariance */ -void EKFLocalizer::callbackPoseWithCovariance( +void EKFLocalizer::callback_pose_with_covariance( geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { if (!is_activated_) { @@ -323,9 +326,9 @@ void EKFLocalizer::callbackPoseWithCovariance( } /* - * callbackTwistWithCovariance + * callback_twist_with_covariance */ -void EKFLocalizer::callbackTwistWithCovariance( +void EKFLocalizer::callback_twist_with_covariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { // Ignore twist if velocity is too small. @@ -337,9 +340,9 @@ void EKFLocalizer::callbackTwistWithCovariance( } /* - * publishEstimateResult + * publish_estimate_result */ -void EKFLocalizer::publishEstimateResult( +void EKFLocalizer::publish_estimate_result( const geometry_msgs::msg::PoseStamped & current_ekf_pose, const geometry_msgs::msg::PoseStamped & current_biased_ekf_pose, const geometry_msgs::msg::TwistStamped & current_ekf_twist) @@ -353,7 +356,7 @@ void EKFLocalizer::publishEstimateResult( pose_cov.header.stamp = current_ekf_pose.header.stamp; pose_cov.header.frame_id = current_ekf_pose.header.frame_id; pose_cov.pose.pose = current_ekf_pose.pose; - pose_cov.pose.covariance = ekf_module_->getCurrentPoseCovariance(); + pose_cov.pose.covariance = ekf_module_->get_current_pose_covariance(); pub_pose_cov_->publish(pose_cov); geometry_msgs::msg::PoseWithCovarianceStamped biased_pose_cov = pose_cov; @@ -368,13 +371,13 @@ void EKFLocalizer::publishEstimateResult( twist_cov.header.stamp = current_ekf_twist.header.stamp; twist_cov.header.frame_id = current_ekf_twist.header.frame_id; twist_cov.twist.twist = current_ekf_twist.twist; - twist_cov.twist.covariance = ekf_module_->getCurrentTwistCovariance(); + twist_cov.twist.covariance = ekf_module_->get_current_twist_covariance(); pub_twist_cov_->publish(twist_cov); /* publish yaw bias */ tier4_debug_msgs::msg::Float64Stamped yawb; yawb.stamp = current_ekf_twist.header.stamp; - yawb.data = ekf_module_->getYawBias(); + yawb.data = ekf_module_->get_yaw_bias(); pub_yaw_bias_->publish(yawb); /* publish latest odometry */ @@ -387,38 +390,38 @@ void EKFLocalizer::publishEstimateResult( pub_odom_->publish(odometry); } -void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) +void EKFLocalizer::publish_diagnostics(const rclcpp::Time & current_time) { std::vector diag_status_array; - diag_status_array.push_back(checkProcessActivated(is_activated_)); + diag_status_array.push_back(check_process_activated(is_activated_)); if (is_activated_) { - diag_status_array.push_back(checkMeasurementUpdated( + diag_status_array.push_back(check_measurement_updated( "pose", pose_diag_info_.no_update_count, params_.pose_no_update_count_threshold_warn, params_.pose_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_diag_info_.queue_size)); - diag_status_array.push_back(checkMeasurementDelayGate( + diag_status_array.push_back(check_measurement_queue_size("pose", pose_diag_info_.queue_size)); + diag_status_array.push_back(check_measurement_delay_gate( "pose", pose_diag_info_.is_passed_delay_gate, pose_diag_info_.delay_time, pose_diag_info_.delay_time_threshold)); - diag_status_array.push_back(checkMeasurementMahalanobisGate( + diag_status_array.push_back(check_measurement_mahalanobis_gate( "pose", pose_diag_info_.is_passed_mahalanobis_gate, pose_diag_info_.mahalanobis_distance, params_.pose_gate_dist)); - diag_status_array.push_back(checkMeasurementUpdated( + diag_status_array.push_back(check_measurement_updated( "twist", twist_diag_info_.no_update_count, params_.twist_no_update_count_threshold_warn, params_.twist_no_update_count_threshold_error)); - diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_diag_info_.queue_size)); - diag_status_array.push_back(checkMeasurementDelayGate( + diag_status_array.push_back(check_measurement_queue_size("twist", twist_diag_info_.queue_size)); + diag_status_array.push_back(check_measurement_delay_gate( "twist", twist_diag_info_.is_passed_delay_gate, twist_diag_info_.delay_time, twist_diag_info_.delay_time_threshold)); - diag_status_array.push_back(checkMeasurementMahalanobisGate( + diag_status_array.push_back(check_measurement_mahalanobis_gate( "twist", twist_diag_info_.is_passed_mahalanobis_gate, twist_diag_info_.mahalanobis_distance, params_.twist_gate_dist)); } diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; - diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status = merge_diagnostic_status(diag_status_array); diag_merged_status.name = "localization: " + std::string(this->get_name()); diag_merged_status.hardware_id = this->get_name(); @@ -428,7 +431,7 @@ void EKFLocalizer::publishDiagnostics(const rclcpp::Time & current_time) pub_diag_->publish(diag_msg); } -void EKFLocalizer::updateSimple1DFilters( +void EKFLocalizer::update_simple_1d_filters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) { double z = pose.pose.pose.position.z; @@ -446,7 +449,8 @@ void EKFLocalizer::updateSimple1DFilters( pitch_filter_.update(rpy.y, pitch_dev, pose.header.stamp); } -void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void EKFLocalizer::init_simple_1d_filters( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { double z = pose.pose.pose.position.z; @@ -465,7 +469,7 @@ void EKFLocalizer::initSimple1DFilters(const geometry_msgs::msg::PoseWithCovaria /** * @brief trigger node */ -void EKFLocalizer::serviceTriggerNode( +void EKFLocalizer::service_trigger_node( const std_srvs::srv::SetBool::Request::SharedPtr req, std_srvs::srv::SetBool::Response::SharedPtr res) { @@ -477,7 +481,6 @@ void EKFLocalizer::serviceTriggerNode( is_activated_ = false; } res->success = true; - return; } #include diff --git a/localization/ekf_localizer/src/ekf_module.cpp b/localization/ekf_localizer/src/ekf_module.cpp index 8977d82f34138..8703d8754eaaf 100644 --- a/localization/ekf_localizer/src/ekf_module.cpp +++ b/localization/ekf_localizer/src/ekf_module.cpp @@ -30,58 +30,56 @@ #include // clang-format off -#define DEBUG_PRINT_MAT(X) {\ - if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}\ -} +#define DEBUG_PRINT_MAT(X) {if (params_.show_debug_info) {std::cout << #X << ": " << X << std::endl;}} // NOLINT // clang-format on -EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters params) +EKFModule::EKFModule(std::shared_ptr warning, const HyperParameters & params) : warning_(std::move(warning)), dim_x_(6), // x, y, yaw, yaw_bias, vx, wz accumulated_delay_times_(params.extend_state_step, 1.0E15), params_(params) { - Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y - P(IDX::YAW, IDX::YAW) = 50.0; // for yaw + Eigen::MatrixXd x = Eigen::MatrixXd::Zero(dim_x_, 1); + Eigen::MatrixXd p = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y + p(IDX::YAW, IDX::YAW) = 50.0; // for yaw if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias + p(IDX::YAWB, IDX::YAWB) = 50.0; // for yaw bias } - P(IDX::VX, IDX::VX) = 1000.0; // for vx - P(IDX::WZ, IDX::WZ) = 50.0; // for wz + p(IDX::VX, IDX::VX) = 1000.0; // for vx + p(IDX::WZ, IDX::WZ) = 50.0; // for wz - kalman_filter_.init(X, P, params_.extend_state_step); + kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); } void EKFModule::initialize( const PoseWithCovariance & initial_pose, const geometry_msgs::msg::TransformStamped & transform) { - Eigen::MatrixXd X(dim_x_, 1); - Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_); + Eigen::MatrixXd x(dim_x_, 1); + Eigen::MatrixXd p = Eigen::MatrixXd::Zero(dim_x_, dim_x_); - X(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; - X(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; - X(IDX::YAW) = + x(IDX::X) = initial_pose.pose.pose.position.x + transform.transform.translation.x; + x(IDX::Y) = initial_pose.pose.pose.position.y + transform.transform.translation.y; + x(IDX::YAW) = tf2::getYaw(initial_pose.pose.pose.orientation) + tf2::getYaw(transform.transform.rotation); - X(IDX::YAWB) = 0.0; - X(IDX::VX) = 0.0; - X(IDX::WZ) = 0.0; + x(IDX::YAWB) = 0.0; + x(IDX::VX) = 0.0; + x(IDX::WZ) = 0.0; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - P(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; - P(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; - P(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; + p(IDX::X, IDX::X) = initial_pose.pose.covariance[COV_IDX::X_X]; + p(IDX::Y, IDX::Y) = initial_pose.pose.covariance[COV_IDX::Y_Y]; + p(IDX::YAW, IDX::YAW) = initial_pose.pose.covariance[COV_IDX::YAW_YAW]; if (params_.enable_yaw_bias_estimation) { - P(IDX::YAWB, IDX::YAWB) = 0.0001; + p(IDX::YAWB, IDX::YAWB) = 0.0001; } - P(IDX::VX, IDX::VX) = 0.01; - P(IDX::WZ, IDX::WZ) = 0.01; + p(IDX::VX, IDX::VX) = 0.01; + p(IDX::WZ, IDX::WZ) = 0.01; - kalman_filter_.init(X, P, params_.extend_state_step); + kalman_filter_.init(x, p, static_cast(params_.extend_state_step)); } -geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( +geometry_msgs::msg::PoseStamped EKFModule::get_current_pose( const rclcpp::Time & current_time, const double z, const double roll, const double pitch, bool get_biased_yaw) const { @@ -110,7 +108,8 @@ geometry_msgs::msg::PoseStamped EKFModule::getCurrentPose( return current_ekf_pose; } -geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & current_time) const +geometry_msgs::msg::TwistStamped EKFModule::get_current_twist( + const rclcpp::Time & current_time) const { const double vx = kalman_filter_.getXelement(IDX::VX); const double wz = kalman_filter_.getXelement(IDX::WZ); @@ -123,17 +122,17 @@ geometry_msgs::msg::TwistStamped EKFModule::getCurrentTwist(const rclcpp::Time & return current_ekf_twist; } -std::array EKFModule::getCurrentPoseCovariance() const +std::array EKFModule::get_current_pose_covariance() const { - return ekfCovarianceToPoseMessageCovariance(kalman_filter_.getLatestP()); + return ekf_covariance_to_pose_message_covariance(kalman_filter_.getLatestP()); } -std::array EKFModule::getCurrentTwistCovariance() const +std::array EKFModule::get_current_twist_covariance() const { - return ekfCovarianceToTwistMessageCovariance(kalman_filter_.getLatestP()); + return ekf_covariance_to_twist_message_covariance(kalman_filter_.getLatestP()); } -double EKFModule::getYawBias() const +double EKFModule::get_yaw_bias() const { return kalman_filter_.getLatestX()(IDX::YAWB); } @@ -153,17 +152,17 @@ size_t EKFModule::find_closest_delay_time_index(double target_value) const // If else, take the closest element. if (lower == accumulated_delay_times_.begin()) { return 0; - } else if (lower == accumulated_delay_times_.end()) { + } + if (lower == accumulated_delay_times_.end()) { return accumulated_delay_times_.size() - 1; - } else { - // Compare the target with the lower bound and the previous element. - auto prev = lower - 1; - bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); - - // Return the index of the closer element. - return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) - : std::distance(accumulated_delay_times_.begin(), lower); } + // Compare the target with the lower bound and the previous element. + auto prev = lower - 1; + bool is_closer_to_prev = (target_value - *prev) < (*lower - target_value); + + // Return the index of the closer element. + return is_closer_to_prev ? std::distance(accumulated_delay_times_.begin(), prev) + : std::distance(accumulated_delay_times_.begin(), lower); } void EKFModule::accumulate_delay_time(const double dt) @@ -180,69 +179,70 @@ void EKFModule::accumulate_delay_time(const double dt) } } -void EKFModule::predictWithDelay(const double dt) +void EKFModule::predict_with_delay(const double dt) { - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); const double proc_cov_vx_d = std::pow(params_.proc_stddev_vx_c * dt, 2.0); const double proc_cov_wz_d = std::pow(params_.proc_stddev_wz_c * dt, 2.0); const double proc_cov_yaw_d = std::pow(params_.proc_stddev_yaw_c * dt, 2.0); - const Vector6d X_next = predictNextState(X_curr, dt); - const Matrix6d A = createStateTransitionMatrix(X_curr, dt); - const Matrix6d Q = processNoiseCovariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); - kalman_filter_.predictWithDelay(X_next, A, Q); + const Vector6d x_next = predict_next_state(x_curr, dt); + const Matrix6d a = create_state_transition_matrix(x_curr, dt); + const Matrix6d q = process_noise_covariance(proc_cov_yaw_d, proc_cov_vx_d, proc_cov_wz_d); + kalman_filter_.predictWithDelay(x_next, a, q); } -bool EKFModule::measurementUpdatePose( +bool EKFModule::measurement_update_pose( const PoseWithCovariance & pose, const rclcpp::Time & t_curr, EKFDiagnosticInfo & pose_diag_info) { if (pose.header.frame_id != params_.pose_frame_id) { - warning_->warnThrottle( + warning_->warn_throttle( fmt::format( "pose frame_id is %s, but pose_frame is set as %s. They must be same.", pose.header.frame_id.c_str(), params_.pose_frame_id.c_str()), 2000); } - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_curr.transpose()); constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output /* Calculate delay step */ double delay_time = (t_curr - pose.header.stamp).seconds() + params_.pose_additional_delay; if (delay_time < 0.0) { - warning_->warnThrottle(poseDelayTimeWarningMessage(delay_time), 1000); + warning_->warn_throttle(pose_delay_time_warning_message(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); - const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); + const size_t delay_step = find_closest_delay_time_index(delay_time); pose_diag_info.delay_time = std::max(delay_time, pose_diag_info.delay_time); pose_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { pose_diag_info.is_passed_delay_gate = false; - warning_->warnThrottle( - poseDelayStepWarningMessage(pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), + warning_->warn_throttle( + pose_delay_step_warning_message( + pose_diag_info.delay_time, pose_diag_info.delay_time_threshold), 2000); return false; } /* Since the kalman filter cannot handle the rotation angle directly, - offset the yaw angle so that the difference from the yaw angle that ekf holds internally is less - than 2 pi. */ + offset the yaw angle so that the difference from the yaw angle that ekf holds internally + is less than 2 pi. */ double yaw = tf2::getYaw(pose.pose.pose.orientation); const double ekf_yaw = kalman_filter_.getXelement(delay_step * dim_x_ + IDX::YAW); - const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi + const double yaw_error = normalize_yaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi yaw = yaw_error + ekf_yaw; /* Set measurement matrix */ Eigen::MatrixXd y(dim_y, 1); y << pose.pose.pose.position.x, pose.pose.pose.position.y, yaw; - if (hasNan(y) || hasInf(y)) { + if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); return false; @@ -252,15 +252,15 @@ bool EKFModule::measurementUpdatePose( const Eigen::Vector3d y_ekf( kalman_filter_.getXelement(delay_step * dim_x_ + IDX::X), kalman_filter_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd p_y = p_curr.block(0, 0, dim_y, dim_y); - const double distance = mahalanobis(y_ekf, y, P_y); + const double distance = mahalanobis(y_ekf, y, p_y); pose_diag_info.mahalanobis_distance = std::max(distance, pose_diag_info.mahalanobis_distance); if (distance > params_.pose_gate_dist) { pose_diag_info.is_passed_mahalanobis_gate = false; - warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); - warning_->warnThrottle("Ignore the measurement data.", 2000); + warning_->warn_throttle(mahalanobis_warning_message(distance, params_.pose_gate_dist), 2000); + warning_->warn_throttle("Ignore the measurement data.", 2000); return false; } @@ -268,21 +268,21 @@ bool EKFModule::measurementUpdatePose( DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); - const Eigen::Matrix C = poseMeasurementMatrix(); - const Eigen::Matrix3d R = - poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps); + const Eigen::Matrix c = pose_measurement_matrix(); + const Eigen::Matrix3d r = + pose_measurement_covariance(pose.pose.covariance, params_.pose_smoothing_steps); - kalman_filter_.updateWithDelay(y, C, R, delay_step); + kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); // debug - const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_result.transpose()); + DEBUG_PRINT_MAT((x_result - x_curr).transpose()); return true; } -geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDelay( +geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensate_pose_with_z_delay( const PoseWithCovariance & pose, const double delay_time) { const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation); @@ -293,34 +293,34 @@ geometry_msgs::msg::PoseWithCovarianceStamped EKFModule::compensatePoseWithZDela return pose_with_z_delay; } -bool EKFModule::measurementUpdateTwist( +bool EKFModule::measurement_update_twist( const TwistWithCovariance & twist, const rclcpp::Time & t_curr, EKFDiagnosticInfo & twist_diag_info) { if (twist.header.frame_id != "base_link") { - warning_->warnThrottle("twist frame_id must be base_link", 2000); + warning_->warn_throttle("twist frame_id must be base_link", 2000); } - const Eigen::MatrixXd X_curr = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_curr.transpose()); + const Eigen::MatrixXd x_curr = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_curr.transpose()); constexpr int dim_y = 2; // vx, wz /* Calculate delay step */ double delay_time = (t_curr - twist.header.stamp).seconds() + params_.twist_additional_delay; if (delay_time < 0.0) { - warning_->warnThrottle(twistDelayTimeWarningMessage(delay_time), 1000); + warning_->warn_throttle(twist_delay_time_warning_message(delay_time), 1000); } delay_time = std::max(delay_time, 0.0); - const int delay_step = static_cast(find_closest_delay_time_index(delay_time)); + const size_t delay_step = find_closest_delay_time_index(delay_time); twist_diag_info.delay_time = std::max(delay_time, twist_diag_info.delay_time); twist_diag_info.delay_time_threshold = accumulated_delay_times_.back(); if (delay_step >= params_.extend_state_step) { twist_diag_info.is_passed_delay_gate = false; - warning_->warnThrottle( - twistDelayStepWarningMessage( + warning_->warn_throttle( + twist_delay_step_warning_message( twist_diag_info.delay_time, twist_diag_info.delay_time_threshold), 2000); return false; @@ -330,7 +330,7 @@ bool EKFModule::measurementUpdateTwist( Eigen::MatrixXd y(dim_y, 1); y << twist.twist.twist.linear.x, twist.twist.twist.angular.z; - if (hasNan(y) || hasInf(y)) { + if (has_nan(y) || has_inf(y)) { warning_->warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); return false; @@ -339,15 +339,15 @@ bool EKFModule::measurementUpdateTwist( const Eigen::Vector2d y_ekf( kalman_filter_.getXelement(delay_step * dim_x_ + IDX::VX), kalman_filter_.getXelement(delay_step * dim_x_ + IDX::WZ)); - const Eigen::MatrixXd P_curr = kalman_filter_.getLatestP(); - const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); + const Eigen::MatrixXd p_curr = kalman_filter_.getLatestP(); + const Eigen::MatrixXd p_y = p_curr.block(4, 4, dim_y, dim_y); - const double distance = mahalanobis(y_ekf, y, P_y); + const double distance = mahalanobis(y_ekf, y, p_y); twist_diag_info.mahalanobis_distance = std::max(distance, twist_diag_info.mahalanobis_distance); if (distance > params_.twist_gate_dist) { twist_diag_info.is_passed_mahalanobis_gate = false; - warning_->warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); - warning_->warnThrottle("Ignore the measurement data.", 2000); + warning_->warn_throttle(mahalanobis_warning_message(distance, params_.twist_gate_dist), 2000); + warning_->warn_throttle("Ignore the measurement data.", 2000); return false; } @@ -355,16 +355,16 @@ bool EKFModule::measurementUpdateTwist( DEBUG_PRINT_MAT(y_ekf.transpose()); DEBUG_PRINT_MAT((y - y_ekf).transpose()); - const Eigen::Matrix C = twistMeasurementMatrix(); - const Eigen::Matrix2d R = - twistMeasurementCovariance(twist.twist.covariance, params_.twist_smoothing_steps); + const Eigen::Matrix c = twist_measurement_matrix(); + const Eigen::Matrix2d r = + twist_measurement_covariance(twist.twist.covariance, params_.twist_smoothing_steps); - kalman_filter_.updateWithDelay(y, C, R, delay_step); + kalman_filter_.updateWithDelay(y, c, r, static_cast(delay_step)); // debug - const Eigen::MatrixXd X_result = kalman_filter_.getLatestX(); - DEBUG_PRINT_MAT(X_result.transpose()); - DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + const Eigen::MatrixXd x_result = kalman_filter_.getLatestX(); + DEBUG_PRINT_MAT(x_result.transpose()); + DEBUG_PRINT_MAT((x_result - x_curr).transpose()); return true; } diff --git a/localization/ekf_localizer/src/mahalanobis.cpp b/localization/ekf_localizer/src/mahalanobis.cpp index ff5ef13892b73..a71482ab696f6 100644 --- a/localization/ekf_localizer/src/mahalanobis.cpp +++ b/localization/ekf_localizer/src/mahalanobis.cpp @@ -14,7 +14,7 @@ #include "ekf_localizer/mahalanobis.hpp" -double squaredMahalanobis( +double squared_mahalanobis( const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { const Eigen::VectorXd d = x - y; @@ -23,5 +23,5 @@ double squaredMahalanobis( double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const Eigen::MatrixXd & C) { - return std::sqrt(squaredMahalanobis(x, y, C)); + return std::sqrt(squared_mahalanobis(x, y, C)); } diff --git a/localization/ekf_localizer/src/measurement.cpp b/localization/ekf_localizer/src/measurement.cpp index 0b3e65bd60f5d..4533bacbee991 100644 --- a/localization/ekf_localizer/src/measurement.cpp +++ b/localization/ekf_localizer/src/measurement.cpp @@ -17,40 +17,40 @@ #include "ekf_localizer/state_index.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" -Eigen::Matrix poseMeasurementMatrix() +Eigen::Matrix pose_measurement_matrix() { - Eigen::Matrix C = Eigen::Matrix::Zero(); - C(0, IDX::X) = 1.0; // for pos x - C(1, IDX::Y) = 1.0; // for pos y - C(2, IDX::YAW) = 1.0; // for yaw - return C; + Eigen::Matrix c = Eigen::Matrix::Zero(); + c(0, IDX::X) = 1.0; // for pos x + c(1, IDX::Y) = 1.0; // for pos y + c(2, IDX::YAW) = 1.0; // for yaw + return c; } -Eigen::Matrix twistMeasurementMatrix() +Eigen::Matrix twist_measurement_matrix() { - Eigen::Matrix C = Eigen::Matrix::Zero(); - C(0, IDX::VX) = 1.0; // for vx - C(1, IDX::WZ) = 1.0; // for wz - return C; + Eigen::Matrix c = Eigen::Matrix::Zero(); + c(0, IDX::VX) = 1.0; // for vx + c(1, IDX::WZ) = 1.0; // for wz + return c; } -Eigen::Matrix3d poseMeasurementCovariance( +Eigen::Matrix3d pose_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { - Eigen::Matrix3d R; + Eigen::Matrix3d r; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), + r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_Y), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::Y_X), covariance.at(COV_IDX::Y_Y), covariance.at(COV_IDX::Y_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_Y), covariance.at(COV_IDX::YAW_YAW); - return R * static_cast(smoothing_step); + return r * static_cast(smoothing_step); } -Eigen::Matrix2d twistMeasurementCovariance( +Eigen::Matrix2d twist_measurement_covariance( const std::array & covariance, const size_t smoothing_step) { - Eigen::Matrix2d R; + Eigen::Matrix2d r; using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; - R << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), + r << covariance.at(COV_IDX::X_X), covariance.at(COV_IDX::X_YAW), covariance.at(COV_IDX::YAW_X), covariance.at(COV_IDX::YAW_YAW); - return R * static_cast(smoothing_step); + return r * static_cast(smoothing_step); } diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp index 88fc9f76d7168..22b1f2f8a1c67 100644 --- a/localization/ekf_localizer/src/state_transition.cpp +++ b/localization/ekf_localizer/src/state_transition.cpp @@ -17,7 +17,7 @@ #include -double normalizeYaw(const double & yaw) +double normalize_yaw(const double & yaw) { // FIXME(IshitaTakeshi) I think the computation here can be simplified // FIXME(IshitaTakeshi) Rename the function. This is not normalization @@ -36,7 +36,7 @@ double normalizeYaw(const double & yaw) * (b_k : yaw_bias_k) */ -Vector6d predictNextState(const Vector6d & X_curr, const double dt) +Vector6d predict_next_state(const Vector6d & X_curr, const double dt) { const double x = X_curr(IDX::X); const double y = X_curr(IDX::Y); @@ -45,14 +45,14 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt) const double vx = X_curr(IDX::VX); const double wz = X_curr(IDX::WZ); - Vector6d X_next; - X_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) - X_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) - X_next(IDX::YAW) = normalizeYaw(yaw + wz * dt); // dyaw = omega + omega_bias - X_next(IDX::YAWB) = yaw_bias; - X_next(IDX::VX) = vx; - X_next(IDX::WZ) = wz; - return X_next; + Vector6d x_next; + x_next(IDX::X) = x + vx * std::cos(yaw + yaw_bias) * dt; // dx = v * cos(yaw) + x_next(IDX::Y) = y + vx * std::sin(yaw + yaw_bias) * dt; // dy = v * sin(yaw) + x_next(IDX::YAW) = normalize_yaw(yaw + wz * dt); // dyaw = omega + omega_bias + x_next(IDX::YAWB) = yaw_bias; + x_next(IDX::VX) = vx; + x_next(IDX::WZ) = wz; + return x_next; } /* == Linearized model == @@ -64,32 +64,32 @@ Vector6d predictNextState(const Vector6d & X_curr, const double dt) * [ 0, 0, 0, 0, 1, 0] * [ 0, 0, 0, 0, 0, 1] */ -Matrix6d createStateTransitionMatrix(const Vector6d & X_curr, const double dt) +Matrix6d create_state_transition_matrix(const Vector6d & X_curr, const double dt) { const double yaw = X_curr(IDX::YAW); const double yaw_bias = X_curr(IDX::YAWB); const double vx = X_curr(IDX::VX); - Matrix6d A = Matrix6d::Identity(); - A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; - A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; - A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; - A(IDX::YAW, IDX::WZ) = dt; - return A; + Matrix6d a = Matrix6d::Identity(); + a(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt; + a(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt; + a(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt; + a(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt; + a(IDX::YAW, IDX::WZ) = dt; + return a; } -Matrix6d processNoiseCovariance( +Matrix6d process_noise_covariance( const double proc_cov_yaw_d, const double proc_cov_vx_d, const double proc_cov_wz_d) { - Matrix6d Q = Matrix6d::Zero(); - Q(IDX::X, IDX::X) = 0.0; - Q(IDX::Y, IDX::Y) = 0.0; - Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw - Q(IDX::YAWB, IDX::YAWB) = 0.0; - Q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx - Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz - return Q; + Matrix6d q = Matrix6d::Zero(); + q(IDX::X, IDX::X) = 0.0; + q(IDX::Y, IDX::Y) = 0.0; + q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d; // for yaw + q(IDX::YAWB, IDX::YAWB) = 0.0; + q(IDX::VX, IDX::VX) = proc_cov_vx_d; // for vx + q(IDX::WZ, IDX::WZ) = proc_cov_wz_d; // for wz + return q; } diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index c69f30b2d6601..ae74c6bfb5fbc 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,7 +18,8 @@ #include -std::string poseDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) +std::string pose_delay_step_warning_message( + const double delay_time, const double delay_time_threshold) { const std::string s = "Pose delay exceeds the compensation limit, ignored. " @@ -26,7 +27,8 @@ std::string poseDelayStepWarningMessage(const double delay_time, const double de return fmt::format(s, delay_time, delay_time_threshold); } -std::string twistDelayStepWarningMessage(const double delay_time, const double delay_time_threshold) +std::string twist_delay_step_warning_message( + const double delay_time, const double delay_time_threshold) { const std::string s = "Twist delay exceeds the compensation limit, ignored. " @@ -34,19 +36,19 @@ std::string twistDelayStepWarningMessage(const double delay_time, const double d return fmt::format(s, delay_time, delay_time_threshold); } -std::string poseDelayTimeWarningMessage(const double delay_time) +std::string pose_delay_time_warning_message(const double delay_time) { const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; return fmt::format(s, delay_time); } -std::string twistDelayTimeWarningMessage(const double delay_time) +std::string twist_delay_time_warning_message(const double delay_time) { const std::string s = "Twist time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; return fmt::format(s, delay_time); } -std::string mahalanobisWarningMessage(const double distance, const double max_distance) +std::string mahalanobis_warning_message(const double distance, const double max_distance) { const std::string s = "The Mahalanobis distance {:.4f} is over the limit {:.4f}."; return fmt::format(s, distance, max_distance); diff --git a/localization/ekf_localizer/test/test_covariance.cpp b/localization/ekf_localizer/test/test_covariance.cpp index 23458fb18a838..ed87ebdea029b 100644 --- a/localization/ekf_localizer/test/test_covariance.cpp +++ b/localization/ekf_localizer/test/test_covariance.cpp @@ -19,18 +19,18 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) { { - Matrix6d P = Matrix6d::Zero(); - P(0, 0) = 1.; - P(0, 1) = 2.; - P(0, 2) = 3.; - P(1, 0) = 4.; - P(1, 1) = 5.; - P(1, 2) = 6.; - P(2, 0) = 7.; - P(2, 1) = 8.; - P(2, 2) = 9.; + Matrix6d p = Matrix6d::Zero(); + p(0, 0) = 1.; + p(0, 1) = 2.; + p(0, 2) = 3.; + p(1, 0) = 4.; + p(1, 1) = 5.; + p(1, 2) = 6.; + p(2, 0) = 7.; + p(2, 1) = 8.; + p(2, 2) = 9.; - std::array covariance = ekfCovarianceToPoseMessageCovariance(P); + std::array covariance = ekf_covariance_to_pose_message_covariance(p); EXPECT_EQ(covariance[0], 1.); EXPECT_EQ(covariance[1], 2.); EXPECT_EQ(covariance[5], 3.); @@ -44,8 +44,8 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) // ensure other elements are zero { - Matrix6d P = Matrix6d::Zero(); - std::array covariance = ekfCovarianceToPoseMessageCovariance(P); + Matrix6d p = Matrix6d::Zero(); + std::array covariance = ekf_covariance_to_pose_message_covariance(p); for (double e : covariance) { EXPECT_EQ(e, 0.); } @@ -55,13 +55,13 @@ TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest) TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) { { - Matrix6d P = Matrix6d::Zero(); - P(4, 4) = 1.; - P(4, 5) = 2.; - P(5, 4) = 3.; - P(5, 5) = 4.; + Matrix6d p = Matrix6d::Zero(); + p(4, 4) = 1.; + p(4, 5) = 2.; + p(5, 4) = 3.; + p(5, 5) = 4.; - std::array covariance = ekfCovarianceToTwistMessageCovariance(P); + std::array covariance = ekf_covariance_to_twist_message_covariance(p); EXPECT_EQ(covariance[0], 1.); EXPECT_EQ(covariance[5], 2.); EXPECT_EQ(covariance[30], 3.); @@ -70,8 +70,8 @@ TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest) // ensure other elements are zero { - Matrix6d P = Matrix6d::Zero(); - std::array covariance = ekfCovarianceToTwistMessageCovariance(P); + Matrix6d p = Matrix6d::Zero(); + std::array covariance = ekf_covariance_to_twist_message_covariance(p); for (double e : covariance) { EXPECT_EQ(e, 0.); } diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp index f506dca1cb230..ef0d7a6493756 100644 --- a/localization/ekf_localizer/test/test_diagnostics.cpp +++ b/localization/ekf_localizer/test/test_diagnostics.cpp @@ -16,20 +16,20 @@ #include -TEST(TestEkfDiagnostics, CheckProcessActivated) +TEST(TestEkfDiagnostics, check_process_activated) { diagnostic_msgs::msg::DiagnosticStatus stat; bool is_activated = true; - stat = checkProcessActivated(is_activated); + stat = check_process_activated(is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_activated = false; - stat = checkProcessActivated(is_activated); + stat = check_process_activated(is_activated); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestEkfDiagnostics, checkMeasurementUpdated) +TEST(TestEkfDiagnostics, check_measurement_updated) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -38,58 +38,58 @@ TEST(TestEkfDiagnostics, checkMeasurementUpdated) const size_t no_update_count_threshold_error = 250; size_t no_update_count = 0; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 1; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 49; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); no_update_count = 50; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); no_update_count = 249; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); no_update_count = 250; - stat = checkMeasurementUpdated( + stat = check_measurement_updated( measurement_type, no_update_count, no_update_count_threshold_warn, no_update_count_threshold_error); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } -TEST(TestEkfDiagnostics, CheckMeasurementQueueSize) +TEST(TestEkfDiagnostics, check_measurement_queue_size) { diagnostic_msgs::msg::DiagnosticStatus stat; const std::string measurement_type = "pose"; // not effect for stat.level size_t queue_size = 0; // not effect for stat.level - stat = checkMeasurementQueueSize(measurement_type, queue_size); + stat = check_measurement_queue_size(measurement_type, queue_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); queue_size = 1; // not effect for stat.level - stat = checkMeasurementQueueSize(measurement_type, queue_size); + stat = check_measurement_queue_size(measurement_type, queue_size); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); } -TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) +TEST(TestEkfDiagnostics, check_measurement_delay_gate) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -98,17 +98,17 @@ TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) const double delay_time_threshold = 1.0; // not effect for stat.level bool is_passed_delay_gate = true; - stat = checkMeasurementDelayGate( + stat = check_measurement_delay_gate( measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_passed_delay_gate = false; - stat = checkMeasurementDelayGate( + stat = check_measurement_delay_gate( measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) +TEST(TestEkfDiagnostics, check_measurement_mahalanobis_gate) { diagnostic_msgs::msg::DiagnosticStatus stat; @@ -117,19 +117,19 @@ TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level bool is_passed_mahalanobis_gate = true; - stat = checkMeasurementMahalanobisGate( + stat = check_measurement_mahalanobis_gate( measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, mahalanobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); is_passed_mahalanobis_gate = false; - stat = checkMeasurementMahalanobisGate( + stat = check_measurement_mahalanobis_gate( measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, mahalanobis_distance_threshold); EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +TEST(TestLocalizationErrorMonitorDiagnostics, merge_diagnostic_status) { diagnostic_msgs::msg::DiagnosticStatus merged_stat; std::vector stat_array(2); @@ -138,7 +138,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); EXPECT_EQ(merged_stat.message, "OK"); @@ -146,7 +146,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; stat_array.at(1).message = "OK"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0"); @@ -154,7 +154,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN1"); @@ -162,7 +162,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; stat_array.at(1).message = "WARN1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); @@ -170,7 +170,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "OK"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR1"); @@ -178,7 +178,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "WARN0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); @@ -186,7 +186,7 @@ TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) stat_array.at(0).message = "ERROR0"; stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; stat_array.at(1).message = "ERROR1"; - merged_stat = mergeDiagnosticStatus(stat_array); + merged_stat = merge_diagnostic_status(stat_array); EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); } diff --git a/localization/ekf_localizer/test/test_mahalanobis.cpp b/localization/ekf_localizer/test/test_mahalanobis.cpp index d208c1e8da06b..db7d538b533c3 100644 --- a/localization/ekf_localizer/test/test_mahalanobis.cpp +++ b/localization/ekf_localizer/test/test_mahalanobis.cpp @@ -18,44 +18,44 @@ constexpr double tolerance = 1e-8; -TEST(SquaredMahalanobis, SmokeTest) +TEST(squared_mahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; + Eigen::Matrix2d c; + c << 10, 0, 0, 10; - EXPECT_NEAR(squaredMahalanobis(x, y, C), 1.0, tolerance); + EXPECT_NEAR(squared_mahalanobis(x, y, c), 1.0, tolerance); } { Eigen::Vector2d x(4, 1); Eigen::Vector2d y(1, 5); - Eigen::Matrix2d C; - C << 5, 0, 0, 5; + Eigen::Matrix2d c; + c << 5, 0, 0, 5; - EXPECT_NEAR(squaredMahalanobis(x, y, C), 5.0, tolerance); + EXPECT_NEAR(squared_mahalanobis(x, y, c), 5.0, tolerance); } } -TEST(Mahalanobis, SmokeTest) +TEST(mahalanobis, SmokeTest) { { Eigen::Vector2d x(0, 1); Eigen::Vector2d y(3, 2); - Eigen::Matrix2d C; - C << 10, 0, 0, 10; + Eigen::Matrix2d c; + c << 10, 0, 0, 10; - EXPECT_NEAR(mahalanobis(x, y, C), 1.0, tolerance); + EXPECT_NEAR(mahalanobis(x, y, c), 1.0, tolerance); } { Eigen::Vector2d x(4, 1); Eigen::Vector2d y(1, 5); - Eigen::Matrix2d C; - C << 5, 0, 0, 5; + Eigen::Matrix2d c; + c << 5, 0, 0, 5; - EXPECT_NEAR(mahalanobis(x, y, C), std::sqrt(5.0), tolerance); + EXPECT_NEAR(mahalanobis(x, y, c), std::sqrt(5.0), tolerance); } } diff --git a/localization/ekf_localizer/test/test_measurement.cpp b/localization/ekf_localizer/test/test_measurement.cpp index 9d63cb056d9d3..c77e39a67d15c 100644 --- a/localization/ekf_localizer/test/test_measurement.cpp +++ b/localization/ekf_localizer/test/test_measurement.cpp @@ -16,66 +16,66 @@ #include -TEST(Measurement, PoseMeasurementMatrix) +TEST(Measurement, pose_measurement_matrix) { - const Eigen::Matrix M = poseMeasurementMatrix(); + const Eigen::Matrix m = pose_measurement_matrix(); Eigen::Matrix expected; expected << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0; - EXPECT_EQ((M - expected).norm(), 0); + EXPECT_EQ((m - expected).norm(), 0); } -TEST(Measurement, TwistMeasurementMatrix) +TEST(Measurement, twist_measurement_matrix) { - const Eigen::Matrix M = twistMeasurementMatrix(); + const Eigen::Matrix m = twist_measurement_matrix(); Eigen::Matrix expected; expected << 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - EXPECT_EQ((M - expected).norm(), 0); + EXPECT_EQ((m - expected).norm(), 0); } -TEST(Measurement, PoseMeasurementCovariance) +TEST(Measurement, pose_measurement_covariance) { { const std::array covariance = {1, 2, 0, 0, 0, 3, 4, 5, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 7, 8, 0, 0, 0, 9}; - const Eigen::Matrix3d M = poseMeasurementCovariance(covariance, 2); + const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2); Eigen::Matrix3d expected; expected << 2, 4, 6, 8, 10, 12, 14, 16, 18; - EXPECT_EQ((M - expected).norm(), 0.); + EXPECT_EQ((m - expected).norm(), 0.); } { // Make sure that other elements are not changed - std::array covariance; + std::array covariance{}; covariance.fill(0); - const Eigen::Matrix3d M = poseMeasurementCovariance(covariance, 2.); - EXPECT_EQ(M.norm(), 0); + const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2.); + EXPECT_EQ(m.norm(), 0); } } -TEST(Measurement, TwistMeasurementCovariance) +TEST(Measurement, twist_measurement_covariance) { { const std::array covariance = {1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 4}; - const Eigen::Matrix2d M = twistMeasurementCovariance(covariance, 2); + const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2); Eigen::Matrix2d expected; expected << 2, 4, 6, 8; - EXPECT_EQ((M - expected).norm(), 0.); + EXPECT_EQ((m - expected).norm(), 0.); } { // Make sure that other elements are not changed - std::array covariance; + std::array covariance{}; covariance.fill(0); - const Eigen::Matrix2d M = twistMeasurementCovariance(covariance, 2.); - EXPECT_EQ(M.norm(), 0); + const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2.); + EXPECT_EQ(m.norm(), 0); } } diff --git a/localization/ekf_localizer/test/test_numeric.cpp b/localization/ekf_localizer/test/test_numeric.cpp index baf4f46db79a9..f84c9aa1d0bf2 100644 --- a/localization/ekf_localizer/test/test_numeric.cpp +++ b/localization/ekf_localizer/test/test_numeric.cpp @@ -18,30 +18,30 @@ #include -TEST(Numeric, HasNan) +TEST(Numeric, has_nan) { const Eigen::VectorXd empty(0); const double inf = std::numeric_limits::infinity(); const double nan = std::nan(""); - EXPECT_FALSE(hasNan(empty)); - EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(hasNan(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(hasNan(Eigen::Vector3d(0., 1., inf))); + EXPECT_FALSE(has_nan(empty)); + EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(has_nan(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 1., inf))); - EXPECT_TRUE(hasNan(Eigen::Vector3d(nan, 1., 0.))); + EXPECT_TRUE(has_nan(Eigen::Vector3d(nan, 1., 0.))); } -TEST(Numeric, HasInf) +TEST(Numeric, has_inf) { const Eigen::VectorXd empty(0); const double inf = std::numeric_limits::infinity(); const double nan = std::nan(""); - EXPECT_FALSE(hasInf(empty)); - EXPECT_FALSE(hasInf(Eigen::Vector3d(0., 0., 1.))); - EXPECT_FALSE(hasInf(Eigen::Vector3d(1e16, 0., 1.))); - EXPECT_FALSE(hasInf(Eigen::Vector3d(nan, 1., 0.))); + EXPECT_FALSE(has_inf(empty)); + EXPECT_FALSE(has_inf(Eigen::Vector3d(0., 0., 1.))); + EXPECT_FALSE(has_inf(Eigen::Vector3d(1e16, 0., 1.))); + EXPECT_FALSE(has_inf(Eigen::Vector3d(nan, 1., 0.))); - EXPECT_TRUE(hasInf(Eigen::Vector3d(0., 1., inf))); + EXPECT_TRUE(has_inf(Eigen::Vector3d(0., 1., inf))); } diff --git a/localization/ekf_localizer/test/test_state_transition.cpp b/localization/ekf_localizer/test/test_state_transition.cpp index 9cb7975a964a9..3b6bd93fd3dd8 100644 --- a/localization/ekf_localizer/test/test_state_transition.cpp +++ b/localization/ekf_localizer/test/test_state_transition.cpp @@ -17,43 +17,44 @@ #include "ekf_localizer/state_transition.hpp" #include -#include -TEST(StateTransition, NormalizeYaw) +#include + +TEST(StateTransition, normalize_yaw) { const double tolerance = 1e-6; - EXPECT_NEAR(normalizeYaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalizeYaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); - EXPECT_NEAR(normalizeYaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); - EXPECT_NEAR(normalizeYaw(M_PI * 4), M_PI * 0, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalize_yaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance); + EXPECT_NEAR(normalize_yaw(M_PI * 4), M_PI * 0, tolerance); } -TEST(PredictNextState, PredictNextState) +TEST(predict_next_state, predict_next_state) { // This function is the definition of state transition so we just check // if the calculation is done according to the formula - Vector6d X_curr; - X_curr(0) = 2.; - X_curr(1) = 3.; - X_curr(2) = M_PI / 2.; - X_curr(3) = M_PI / 4.; - X_curr(4) = 10.; - X_curr(5) = 2. * M_PI / 3.; + Vector6d x_curr; + x_curr(0) = 2.; + x_curr(1) = 3.; + x_curr(2) = M_PI / 2.; + x_curr(3) = M_PI / 4.; + x_curr(4) = 10.; + x_curr(5) = 2. * M_PI / 3.; const double dt = 0.5; - const Vector6d X_next = predictNextState(X_curr, dt); + const Vector6d x_next = predict_next_state(x_curr, dt); const double tolerance = 1e-10; - EXPECT_NEAR(X_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(X_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); - EXPECT_NEAR(X_next(2), normalizeYaw(M_PI / 2. + M_PI / 3.), tolerance); - EXPECT_NEAR(X_next(3), X_curr(3), tolerance); - EXPECT_NEAR(X_next(4), X_curr(4), tolerance); - EXPECT_NEAR(X_next(5), X_curr(5), tolerance); + EXPECT_NEAR(x_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(x_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance); + EXPECT_NEAR(x_next(2), normalize_yaw(M_PI / 2. + M_PI / 3.), tolerance); + EXPECT_NEAR(x_next(3), x_curr(3), tolerance); + EXPECT_NEAR(x_next(4), x_curr(4), tolerance); + EXPECT_NEAR(x_next(5), x_curr(5), tolerance); } -TEST(CreateStateTransitionMatrix, NumericalApproximation) +TEST(create_state_transition_matrix, NumericalApproximation) { // The transition matrix A = df / dx // We check if df = A * dx approximates f(x + dx) - f(x) @@ -64,10 +65,10 @@ TEST(CreateStateTransitionMatrix, NumericalApproximation) const Vector6d dx = 0.1 * Vector6d::Ones(); const Vector6d x = Vector6d::Zero(); - const Matrix6d A = createStateTransitionMatrix(x, dt); - const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + const Matrix6d a = create_state_transition_matrix(x, dt); + const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - EXPECT_LT((df - A * dx).norm(), 2e-3); + EXPECT_LT((df - a * dx).norm(), 2e-3); } { @@ -76,20 +77,20 @@ TEST(CreateStateTransitionMatrix, NumericalApproximation) const Vector6d dx = 0.1 * Vector6d::Ones(); const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished(); - const Matrix6d A = createStateTransitionMatrix(x, dt); - const Vector6d df = predictNextState(x + dx, dt) - predictNextState(x, dt); + const Matrix6d a = create_state_transition_matrix(x, dt); + const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt); - EXPECT_LT((df - A * dx).norm(), 5e-3); + EXPECT_LT((df - a * dx).norm(), 5e-3); } } -TEST(ProcessNoiseCovariance, ProcessNoiseCovariance) +TEST(process_noise_covariance, process_noise_covariance) { - const Matrix6d Q = processNoiseCovariance(1., 2., 3.); - EXPECT_EQ(Q(2, 2), 1.); // for yaw - EXPECT_EQ(Q(4, 4), 2.); // for vx - EXPECT_EQ(Q(5, 5), 3.); // for wz + const Matrix6d q = process_noise_covariance(1., 2., 3.); + EXPECT_EQ(q(2, 2), 1.); // for yaw + EXPECT_EQ(q(4, 4), 2.); // for vx + EXPECT_EQ(q(5, 5), 3.); // for wz // Make sure other elements are zero - EXPECT_EQ(processNoiseCovariance(0, 0, 0).norm(), 0.); + EXPECT_EQ(process_noise_covariance(0, 0, 0).norm(), 0.); } diff --git a/localization/ekf_localizer/test/test_string.cpp b/localization/ekf_localizer/test/test_string.cpp index 7b35a56d889e9..013072e5145c7 100644 --- a/localization/ekf_localizer/test/test_string.cpp +++ b/localization/ekf_localizer/test/test_string.cpp @@ -16,11 +16,11 @@ #include -TEST(EraseLeadingSlash, SmokeTest) +TEST(erase_leading_slash, SmokeTest) { - EXPECT_EQ(eraseLeadingSlash("/topic"), "topic"); - EXPECT_EQ(eraseLeadingSlash("topic"), "topic"); // do nothing + EXPECT_EQ(erase_leading_slash("/topic"), "topic"); + EXPECT_EQ(erase_leading_slash("topic"), "topic"); // do nothing - EXPECT_EQ(eraseLeadingSlash(""), ""); - EXPECT_EQ(eraseLeadingSlash("/"), ""); + EXPECT_EQ(erase_leading_slash(""), ""); + EXPECT_EQ(erase_leading_slash("/"), ""); } diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index 2069969e0ae5e..ec7d3420d7f79 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -18,45 +18,45 @@ #include -TEST(PoseDelayStepWarningMessage, SmokeTest) +TEST(pose_delay_step_warning_message, SmokeTest) { EXPECT_STREQ( - poseDelayStepWarningMessage(6.0, 4.0).c_str(), + pose_delay_step_warning_message(6.0, 4.0).c_str(), "Pose delay exceeds the compensation limit, ignored. " "delay: 6.000[s], limit: 4.000[s]"); } -TEST(TwistDelayStepWarningMessage, SmokeTest) +TEST(twist_delay_step_warning_message, SmokeTest) { EXPECT_STREQ( - twistDelayStepWarningMessage(10.0, 6.0).c_str(), + twist_delay_step_warning_message(10.0, 6.0).c_str(), "Twist delay exceeds the compensation limit, ignored. " "delay: 10.000[s], limit: 6.000[s]"); } -TEST(PoseDelayTimeWarningMessage, SmokeTest) +TEST(pose_delay_time_warning_message, SmokeTest) { EXPECT_STREQ( - poseDelayTimeWarningMessage(-1.0).c_str(), + pose_delay_time_warning_message(-1.0).c_str(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); EXPECT_STREQ( - poseDelayTimeWarningMessage(-0.4).c_str(), + pose_delay_time_warning_message(-0.4).c_str(), "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); } -TEST(TwistDelayTimeWarningMessage, SmokeTest) +TEST(twist_delay_time_warning_message, SmokeTest) { EXPECT_STREQ( - twistDelayTimeWarningMessage(-1.0).c_str(), + twist_delay_time_warning_message(-1.0).c_str(), "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000"); EXPECT_STREQ( - twistDelayTimeWarningMessage(-0.4).c_str(), + twist_delay_time_warning_message(-0.4).c_str(), "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400"); } -TEST(MahalanobisWarningMessage, SmokeTest) +TEST(mahalanobis_warning_message, SmokeTest) { EXPECT_STREQ( - mahalanobisWarningMessage(1.0, 0.5).c_str(), + mahalanobis_warning_message(1.0, 0.5).c_str(), "The Mahalanobis distance 1.0000 is over the limit 0.5000."); } diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp index 49b881900b997..c5b7b7a592013 100644 --- a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -30,14 +30,14 @@ class DiagnosticsModule public: DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); - void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template - void addKeyValue(const std::string & key, const T & value); - void updateLevelAndMessage(const int8_t level, const std::string & message); + void add_key_value(const std::string & key, const T & value); + void update_level_and_message(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; @@ -47,18 +47,18 @@ class DiagnosticsModule }; template -void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +void DiagnosticsModule::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = std::to_string(value); - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); } // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 3b2da504f938a..f85640ab2c523 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -47,14 +47,13 @@ class GyroOdometerNode : public rclcpp::Node public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); - ~GyroOdometerNode(); private: - void callbackVehicleTwist( + void callback_vehicle_twist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); - void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); - void concatGyroAndOdometer(); - void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); + void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concat_gyro_and_odometer(); + void publish_data(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr vehicle_twist_sub_; diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp index 9d88d8e6833ed..5d687943cef53 100644 --- a/localization/gyro_odometer/src/diagnostics_module.cpp +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -44,7 +44,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -58,24 +58,24 @@ void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_v } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value; - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value ? "True" : "False"; - addKeyValue(key_value); + add_key_value(key_value); } -void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -90,10 +90,10 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 1852d70bced71..0ec479770740f 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -31,13 +31,13 @@ namespace autoware::gyro_odometer { -std::array transformCovariance(const std::array & cov) +std::array transform_covariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); - std::array cov_transformed; + std::array cov_transformed = {}; cov_transformed.fill(0.); cov_transformed[COV_IDX::X_X] = max_cov; cov_transformed[COV_IDX::Y_Y] = max_cov; @@ -57,11 +57,11 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_vehicle_twist, this, std::placeholders::_1)); imu_sub_ = create_subscription( "imu", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_imu, this, std::placeholders::_1)); twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); twist_with_covariance_raw_pub_ = create_publisher( @@ -76,49 +76,46 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) // TODO(YamatoAndo) createTimer } -GyroOdometerNode::~GyroOdometerNode() -{ -} - -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) +void GyroOdometerNode::callback_vehicle_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue( - "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); + diagnostics_->add_key_value( + "topic_time_stamp", + static_cast(vehicle_twist_msg_ptr->header.stamp).nanoseconds()); vehicle_twist_arrived_ = true; - latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp; - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); - concatGyroAndOdometer(); + latest_vehicle_twist_ros_time_ = vehicle_twist_msg_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_msg_ptr); + concat_gyro_and_odometer(); - diagnostics_->publish(vehicle_twist_ptr->header.stamp); + diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp); } -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue( + diagnostics_->add_key_value( "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); imu_arrived_ = true; latest_imu_ros_time_ = imu_msg_ptr->header.stamp; gyro_queue_.push_back(*imu_msg_ptr); - concatGyroAndOdometer(); + concat_gyro_and_odometer(); diagnostics_->publish(imu_msg_ptr->header.stamp); } -void GyroOdometerNode::concatGyroAndOdometer() +void GyroOdometerNode::concat_gyro_and_odometer() { // check arrive first topic - diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); - diagnostics_->addKeyValue("is_arrived_first_imu", imu_arrived_); + diagnostics_->add_key_value("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->add_key_value("is_arrived_first_imu", imu_arrived_); if (!vehicle_twist_arrived_) { std::stringstream message; message << "Twist msg is not subscribed"; RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); vehicle_twist_queue_.clear(); @@ -129,7 +126,7 @@ void GyroOdometerNode::concatGyroAndOdometer() std::stringstream message; message << "Imu msg is not subscribed"; RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); vehicle_twist_queue_.clear(); @@ -141,14 +138,14 @@ void GyroOdometerNode::concatGyroAndOdometer() const double vehicle_twist_dt = std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); - diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt); - diagnostics_->addKeyValue("imu_time_stamp_dt", imu_dt); + diagnostics_->add_key_value("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->add_key_value("imu_time_stamp_dt", imu_dt); if (vehicle_twist_dt > message_timeout_sec_) { const std::string message = fmt::format( "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", vehicle_twist_dt, message_timeout_sec_); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); - diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); vehicle_twist_queue_.clear(); gyro_queue_.clear(); @@ -158,7 +155,7 @@ void GyroOdometerNode::concatGyroAndOdometer() const std::string message = fmt::format( "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); - diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); vehicle_twist_queue_.clear(); gyro_queue_.clear(); @@ -166,8 +163,8 @@ void GyroOdometerNode::concatGyroAndOdometer() } // check queue size - diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); - diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + diagnostics_->add_key_value("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->add_key_value("imu_queue_size", gyro_queue_.size()); if (vehicle_twist_queue_.empty()) { // not output error and clear queue return; @@ -182,13 +179,13 @@ void GyroOdometerNode::concatGyroAndOdometer() transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); - diagnostics_->addKeyValue("is_succeed_transform_imu", is_succeed_transform_imu); + diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu); if (!is_succeed_transform_imu) { std::stringstream message; message << "Please publish TF " << output_frame_ << " to " << gyro_queue_.front().header.frame_id; RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); vehicle_twist_queue_.clear(); @@ -208,7 +205,7 @@ void GyroOdometerNode::concatGyroAndOdometer() gyro.header.frame_id = output_frame_; gyro.angular_velocity = transformed_angular_velocity.vector; - gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance); + gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); } using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -223,8 +220,8 @@ void GyroOdometerNode::concatGyroAndOdometer() vx_mean += vehicle_twist.twist.twist.linear.x; vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } - vx_mean /= vehicle_twist_queue_.size(); - vx_covariance_original /= vehicle_twist_queue_.size(); + vx_mean /= static_cast(vehicle_twist_queue_.size()); + vx_covariance_original /= static_cast(vehicle_twist_queue_.size()); for (const auto & gyro : gyro_queue_) { gyro_mean.x += gyro.angular_velocity.x; @@ -234,12 +231,12 @@ void GyroOdometerNode::concatGyroAndOdometer() gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; } - gyro_mean.x /= gyro_queue_.size(); - gyro_mean.y /= gyro_queue_.size(); - gyro_mean.z /= gyro_queue_.size(); - gyro_covariance_original.x /= gyro_queue_.size(); - gyro_covariance_original.y /= gyro_queue_.size(); - gyro_covariance_original.z /= gyro_queue_.size(); + gyro_mean.x /= static_cast(gyro_queue_.size()); + gyro_mean.y /= static_cast(gyro_queue_.size()); + gyro_mean.z /= static_cast(gyro_queue_.size()); + gyro_covariance_original.x /= static_cast(gyro_queue_.size()); + gyro_covariance_original.y /= static_cast(gyro_queue_.size()); + gyro_covariance_original.z /= static_cast(gyro_queue_.size()); // concat geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; @@ -257,23 +254,23 @@ void GyroOdometerNode::concatGyroAndOdometer() // From a statistical point of view, here we reduce the covariances according to the number of // observed data twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue_.size(); + vx_covariance_original / static_cast(vehicle_twist_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue_.size(); + gyro_covariance_original.x / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue_.size(); + gyro_covariance_original.y / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue_.size(); + gyro_covariance_original.z / static_cast(gyro_queue_.size()); - publishData(twist_with_cov); + publish_data(twist_with_cov); vehicle_twist_queue_.clear(); gyro_queue_.clear(); } -void GyroOdometerNode::publishData( +void GyroOdometerNode::publish_data( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp index 54e1d320319d3..f55278998f5f0 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp @@ -17,7 +17,7 @@ using geometry_msgs::msg::TwistWithCovarianceStamped; using sensor_msgs::msg::Imu; -Imu generateSampleImu() +Imu generate_sample_imu() { Imu imu; imu.header.frame_id = "base_link"; @@ -27,7 +27,7 @@ Imu generateSampleImu() return imu; } -TwistWithCovarianceStamped generateSampleVelocity() +TwistWithCovarianceStamped generate_sample_velocity() { TwistWithCovarianceStamped twist; twist.header.frame_id = "base_link"; @@ -35,7 +35,7 @@ TwistWithCovarianceStamped generateSampleVelocity() return twist; } -rclcpp::NodeOptions getNodeOptionsWithDefaultParams() +rclcpp::NodeOptions get_node_options_with_default_params() { rclcpp::NodeOptions node_options; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp index 6e3aff0b841a9..9da344e410e61 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp @@ -20,11 +20,8 @@ #include #include -using geometry_msgs::msg::TwistWithCovarianceStamped; -using sensor_msgs::msg::Imu; - -Imu generateSampleImu(); -TwistWithCovarianceStamped generateSampleVelocity(); -rclcpp::NodeOptions getNodeOptionsWithDefaultParams(); +sensor_msgs::msg::Imu generate_sample_imu(); +geometry_msgs::msg::TwistWithCovarianceStamped generate_sample_velocity(); +rclcpp::NodeOptions get_node_options_with_default_params(); #endif // TEST_GYRO_ODOMETER_HELPER_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index b7849ef03bfc5..fc331a638a1dd 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -50,19 +50,22 @@ class VelocityGenerator : public rclcpp::Node class GyroOdometerValidator : public rclcpp::Node { public: - GyroOdometerValidator() : Node("gyro_odometer_validator"), received_latest_twist_ptr(nullptr) - { - twist_sub = create_subscription( - "/twist_with_covariance", 1, [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { + GyroOdometerValidator() + : Node("gyro_odometer_validator"), + twist_sub(create_subscription( + "/twist_with_covariance", 1, + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { received_latest_twist_ptr = msg; - }); + })), + received_latest_twist_ptr(nullptr) + { } rclcpp::Subscription::SharedPtr twist_sub; TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr; }; -void spinSome(rclcpp::Node::SharedPtr node_ptr) +void wait_spin_some(rclcpp::Node::SharedPtr node_ptr) { for (int i = 0; i < 50; ++i) { rclcpp::spin_some(node_ptr); @@ -70,7 +73,7 @@ void spinSome(rclcpp::Node::SharedPtr node_ptr) } } -bool isTwistValid( +bool is_twist_valid( const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth) { if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) { @@ -99,8 +102,8 @@ bool isTwistValid( // velocity data are provided TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) { - Imu input_imu = generateSampleImu(); - TwistWithCovarianceStamped input_velocity = generateSampleVelocity(); + Imu input_imu = generate_sample_imu(); + TwistWithCovarianceStamped input_velocity = generate_sample_velocity(); TwistWithCovarianceStamped expected_output_twist; expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x; @@ -108,8 +111,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -120,13 +123,13 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) velocity_generator->vehicle_velocity_pub->publish(input_velocity); // gyro_odometer receives IMU and velocity, and publishes the fused twist data. - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node receives the fused twist data and store in "received_latest_twist_ptr". - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); - EXPECT_TRUE(isTwistValid( + EXPECT_TRUE(is_twist_valid( *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist)); } @@ -134,20 +137,20 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) // Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided TEST(GyroOdometer, TestGyroOdometerImuOnly) { - Imu input_imu = generateSampleImu(); + Imu input_imu = generate_sample_imu(); - auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); imu_generator->imu_pub->publish(input_imu); // gyro_odometer receives IMU - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node waits for the output fused twist from gyro_odometer - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); } diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index ae6f0b61f063c..133442df68dcd 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -28,11 +28,11 @@ std_msgs::msg::ColorRGBA exchange_color_crc(double x) color.b = 1.0; color.g = static_cast(std::sin(x * 2.0 * M_PI)); color.r = 0; - } else if (x > 0.25 && x <= 0.5) { + } else if (x <= 0.5) { color.b = static_cast(std::sin(x * 2 * M_PI)); color.g = 1.0; color.r = 0; - } else if (x > 0.5 && x <= 0.75) { + } else if (x <= 0.75) { color.b = 0; color.g = 1.0; color.r = static_cast(-std::sin(x * 2.0 * M_PI)); diff --git a/localization/localization_util/test/test_smart_pose_buffer.cpp b/localization/localization_util/test/test_smart_pose_buffer.cpp index 2520e458f2d33..a8ed6d98b6064 100644 --- a/localization/localization_util/test/test_smart_pose_buffer.cpp +++ b/localization/localization_util/test/test_smart_pose_buffer.cpp @@ -42,15 +42,7 @@ bool compare_pose( pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; } -class TestSmartPoseBuffer : public ::testing::Test -{ -protected: - void SetUp() override {} - - void TearDown() override {} -}; - -TEST_F(TestSmartPoseBuffer, interpolate_pose) // NOLINT +TEST(TestSmartPoseBuffer, interpolate_pose) // NOLINT { rclcpp::Logger logger = rclcpp::get_logger("test_logger"); const double pose_timeout_sec = 10.0; diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index 2a586aa9cd049..ee63d9f43559a 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -14,6 +14,13 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTOR SingleThreadedExecutor ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_angular_velocity + test/test_angular_velocity.cpp + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/README.md b/localization/pose2twist/README.md index 07d9c37b710fc..f1f7d6408fafb 100644 --- a/localization/pose2twist/README.md +++ b/localization/pose2twist/README.md @@ -5,7 +5,7 @@ This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging. The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero. -The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field. +The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field. ## Inputs / Outputs diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index 274fec47b3c32..d1ff6ee5ff8b6 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -21,6 +21,16 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +// Compute the relative rotation of q2 from q1 as a rotation vector +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2); + class Pose2Twist : public rclcpp::Node { public: diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index f1106c15ebc0d..cdde78ed7e357 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -14,12 +14,6 @@ #include "pose2twist/pose2twist_core.hpp" -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include @@ -41,29 +35,21 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1)); } -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; -} - -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose) +tf2::Quaternion get_quaternion(const geometry_msgs::msg::PoseStamped::SharedPtr & pose_stamped_ptr) { - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; + const auto & orientation = pose_stamped_ptr->pose.orientation; + return tf2::Quaternion{orientation.x, orientation.y, orientation.z, orientation.w}; } -geometry_msgs::msg::Vector3 get_rpy(geometry_msgs::msg::PoseStamped::SharedPtr pose) +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2) { - return get_rpy(pose->pose); + // If we define q2 as the rotation obtained by applying dq after applying q1, + // then q2 = q1 * dq . + // Therefore, dq = q1.inverse() * q2 . + const tf2::Quaternion diff_quaternion = q1.inverse() * q2; + const tf2::Vector3 axis = diff_quaternion.getAxis() * diff_quaternion.getAngle(); + return geometry_msgs::msg::Vector3{}.set__x(axis.x()).set__y(axis.y()).set__z(axis.z()); } geometry_msgs::msg::TwistStamped calc_twist( @@ -79,18 +65,16 @@ geometry_msgs::msg::TwistStamped calc_twist( return twist; } - const auto pose_a_rpy = get_rpy(pose_a); - const auto pose_b_rpy = get_rpy(pose_b); + const auto pose_a_quaternion = get_quaternion(pose_a); + const auto pose_b_quaternion = get_quaternion(pose_b); geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; + const geometry_msgs::msg::Vector3 relative_rotation_vector = + compute_relative_rotation_vector(pose_a_quaternion, pose_b_quaternion); diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x; diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y; diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z; - diff_rpy.x = 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::TwistStamped twist; twist.header = pose_b->header; @@ -99,9 +83,9 @@ geometry_msgs::msg::TwistStamped calc_twist( dt; twist.twist.linear.y = 0; twist.twist.linear.z = 0; - twist.twist.angular.x = diff_rpy.x / dt; - twist.twist.angular.y = diff_rpy.y / dt; - twist.twist.angular.z = diff_rpy.z / dt; + twist.twist.angular.x = relative_rotation_vector.x / dt; + twist.twist.angular.y = relative_rotation_vector.y / dt; + twist.twist.angular.z = relative_rotation_vector.z / dt; return twist; } diff --git a/localization/pose2twist/test/test_angular_velocity.cpp b/localization/pose2twist/test/test_angular_velocity.cpp new file mode 100644 index 0000000000000..bf2ca0a3ba5c2 --- /dev/null +++ b/localization/pose2twist/test/test_angular_velocity.cpp @@ -0,0 +1,115 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose2twist/pose2twist_core.hpp" + +#include + +// 1e-3 radian = 0.057 degrees +constexpr double acceptable_error = 1e-3; + +TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder) +{ + // If we define q2 as the rotation obtained by applying dq after applying q1, then q2 = q1 * dq . + // + // IT IS NOT q2 = dq * q1 . + // + // This test checks that the multiplication order is correct. + + const tf2::Vector3 target_vector(1, 2, 3); + // initial state + // Now, car is facing to the +x direction + // z + // ^ y + // | ^ + // | / + // | / + // car -> x + // + // + // + + tf2::Quaternion q1; + q1.setRPY(0., 0., M_PI / 2.); // yaw = 90 degrees + const tf2::Vector3 initially_rotated_vector = tf2::quatRotate(q1, target_vector); + // after applying q1 + // Now, car is facing to the +y direction + // z + // ^ + // | y + // | ^ + // | / + // <--car x + // + // + // + EXPECT_NEAR(initially_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.y(), 1., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.z(), 3., acceptable_error); + + tf2::Quaternion dq; + dq.setRPY(0., M_PI / 2., 0.); // pitch = 90 degrees + const tf2::Vector3 finally_rotated_vector = tf2::quatRotate(q1 * dq, target_vector); + // after applying dq + // Now, car is facing to the -z direction + // z y + // ^ + // / + // / + // / + // <--car x + // | + // v + // + EXPECT_NEAR(finally_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.y(), 3., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.z(), -1., acceptable_error); + + // Failure case + { + const tf2::Vector3 false_rotated_vector = tf2::quatRotate(dq * q1, target_vector); + + EXPECT_FALSE(std::abs(false_rotated_vector.x() - (-2)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.y() - (3)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.z() - (-1)) < acceptable_error); + } +} + +TEST(AngularVelocityFromQuaternion, CheckNumericalValidity) +{ + auto test = [](const tf2::Vector3 & expected_axis, const double expected_angle) -> void { + tf2::Quaternion expected_q; + expected_q.setRotation(expected_axis, expected_angle); + + // Create a random initial quaternion + tf2::Quaternion initial_q; + initial_q.setRPY(0.2, 0.3, 0.4); + + // Calculate the final quaternion by rotating the initial quaternion by the expected + // quaternion + const tf2::Quaternion final_q = initial_q * expected_q; + + // Calculate the relative rotation between the initial and final quaternion + const geometry_msgs::msg::Vector3 rotation_vector = + compute_relative_rotation_vector(initial_q, final_q); + + EXPECT_NEAR(rotation_vector.x, expected_axis.x() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.y, expected_axis.y() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.z, expected_axis.z() * expected_angle, acceptable_error); + }; + + test(tf2::Vector3(1.0, 0.0, 0.0).normalized(), 0.1); // 0.1 radian = 5.7 degrees + test(tf2::Vector3(1.0, 1.0, 0.0).normalized(), -0.2); // 0.2 radian = 11.4 degrees + test(tf2::Vector3(1.0, 2.0, 3.0).normalized(), 0.3); // 0.3 radian = 17.2 degrees +} diff --git a/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp index 1274092ae4993..33d95c58a726b 100644 --- a/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp +++ b/localization/pose_initializer/src/pose_initializer/copy_vector_to_array.hpp @@ -40,7 +40,7 @@ template std::array get_covariance_parameter(NodeT * node, const std::string & name) { const auto parameter = node->template declare_parameter>(name); - std::array covariance; + std::array covariance{}; copy_vector_to_array(parameter, covariance); return covariance; } diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.cpp b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp index e8762c8c6f493..b746828e33b1c 100644 --- a/localization/pose_initializer/src/pose_initializer/gnss_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/gnss_module.cpp @@ -19,13 +19,18 @@ #include -GnssModule::GnssModule(rclcpp::Node * node) : fitter_(node) +GnssModule::GnssModule(rclcpp::Node * node) +: fitter_(node), + clock_(node->get_clock()), + timeout_(node->declare_parameter("gnss_pose_timeout")) { sub_gnss_pose_ = node->create_subscription( - "gnss_pose_cov", 1, [this](PoseWithCovarianceStamped::ConstSharedPtr msg) { pose_ = msg; }); + "gnss_pose_cov", 1, std::bind(&GnssModule::on_pose, this, std::placeholders::_1)); +} - clock_ = node->get_clock(); - timeout_ = node->declare_parameter("gnss_pose_timeout"); +void GnssModule::on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + pose_ = msg; } geometry_msgs::msg::PoseWithCovarianceStamped GnssModule::get_pose() diff --git a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp index 8c3bc658e7e7a..fd490b00d0f70 100644 --- a/localization/pose_initializer/src/pose_initializer/gnss_module.hpp +++ b/localization/pose_initializer/src/pose_initializer/gnss_module.hpp @@ -30,6 +30,8 @@ class GnssModule PoseWithCovarianceStamped get_pose(); private: + void on_pose(PoseWithCovarianceStamped::ConstSharedPtr msg); + map_height_fitter::MapHeightFitter fitter_; rclcpp::Clock::SharedPtr clock_; rclcpp::Subscription::SharedPtr sub_gnss_pose_; diff --git a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp index 6e11f8fe74212..1f657545e687c 100644 --- a/localization/pose_initializer/src/pose_initializer/ndt_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/ndt_module.cpp @@ -23,9 +23,9 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -NdtModule::NdtModule(rclcpp::Node * node) : logger_(node->get_logger()) +NdtModule::NdtModule(rclcpp::Node * node) +: logger_(node->get_logger()), cli_align_(node->create_client("ndt_align")) { - cli_align_ = node->create_client("ndt_align"); } PoseWithCovarianceStamped NdtModule::align_pose(const PoseWithCovarianceStamped & pose) diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp index 2fc7d61890837..1111547b393e7 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.cpp @@ -67,28 +67,24 @@ PoseInitializer::PoseInitializer(const rclcpp::NodeOptions & options) throw std::invalid_argument( "Could not set user defined initial pose. The size of initial_pose is " + std::to_string(initial_pose_array.size()) + ". It must be 7."); - } else if ( + } + if ( std::abs(initial_pose_array[3]) < 1e-6 && std::abs(initial_pose_array[4]) < 1e-6 && std::abs(initial_pose_array[5]) < 1e-6 && std::abs(initial_pose_array[6]) < 1e-6) { throw std::invalid_argument("Input quaternion is invalid. All elements are close to zero."); - } else { - geometry_msgs::msg::Pose initial_pose; - initial_pose.position.x = initial_pose_array[0]; - initial_pose.position.y = initial_pose_array[1]; - initial_pose.position.z = initial_pose_array[2]; - initial_pose.orientation.x = initial_pose_array[3]; - initial_pose.orientation.y = initial_pose_array[4]; - initial_pose.orientation.z = initial_pose_array[5]; - initial_pose.orientation.w = initial_pose_array[6]; - - set_user_defined_initial_pose(initial_pose, true); } - } -} -PoseInitializer::~PoseInitializer() -{ - // to delete gnss module + geometry_msgs::msg::Pose initial_pose; + initial_pose.position.x = initial_pose_array[0]; + initial_pose.position.y = initial_pose_array[1]; + initial_pose.position.z = initial_pose_array[2]; + initial_pose.orientation.x = initial_pose_array[3]; + initial_pose.orientation.y = initial_pose_array[4]; + initial_pose.orientation.z = initial_pose_array[5]; + initial_pose.orientation.w = initial_pose_array[6]; + + set_user_defined_initial_pose(initial_pose, true); + } } void PoseInitializer::change_state(State::Message::_state_type state) @@ -175,7 +171,7 @@ void PoseInitializer::on_initialize( std::stringstream message; message << "No input pose_with_covariance. If you want to use DIRECT method, please input " "pose_with_covariance."; - RCLCPP_ERROR(get_logger(), message.str().c_str()); + RCLCPP_ERROR_STREAM(get_logger(), message.str()); throw ServiceException( autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); } @@ -186,7 +182,7 @@ void PoseInitializer::on_initialize( } else { std::stringstream message; message << "Unknown method type (=" << std::to_string(req->method) << ")"; - RCLCPP_ERROR(get_logger(), message.str().c_str()); + RCLCPP_ERROR_STREAM(get_logger(), message.str()); throw ServiceException( autoware_common_msgs::msg::ResponseStatus::PARAMETER_ERROR, message.str()); } diff --git a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp index eeba71b90129b..d80fb26fdd41f 100644 --- a/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp +++ b/localization/pose_initializer/src/pose_initializer/pose_initializer_core.hpp @@ -35,7 +35,6 @@ class PoseInitializer : public rclcpp::Node { public: explicit PoseInitializer(const rclcpp::NodeOptions & options); - ~PoseInitializer(); private: using ServiceException = component_interface_utils::ServiceException; @@ -48,8 +47,8 @@ class PoseInitializer : public rclcpp::Node component_interface_utils::Publisher::SharedPtr pub_state_; component_interface_utils::Service::SharedPtr srv_initialize_; State::Message state_; - std::array output_pose_covariance_; - std::array gnss_particle_covariance_; + std::array output_pose_covariance_{}; + std::array gnss_particle_covariance_{}; std::unique_ptr gnss_; std::unique_ptr ndt_; std::unique_ptr yabloc_; diff --git a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp index f79f1e58e8d62..204f2289adbf4 100644 --- a/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp +++ b/localization/pose_initializer/src/pose_initializer/yabloc_module.cpp @@ -23,9 +23,9 @@ using ServiceException = component_interface_utils::ServiceException; using Initialize = localization_interface::Initialize; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; -YabLocModule::YabLocModule(rclcpp::Node * node) : logger_(node->get_logger()) +YabLocModule::YabLocModule(rclcpp::Node * node) +: logger_(node->get_logger()), cli_align_(node->create_client("yabloc_align")) { - cli_align_ = node->create_client("yabloc_align"); } PoseWithCovarianceStamped YabLocModule::align_pose(const PoseWithCovarianceStamped & pose) diff --git a/localization/pose_initializer/test/test_copy_vector_to_array.cpp b/localization/pose_initializer/test/test_copy_vector_to_array.cpp index 65b1d7e7d2711..f372a40076ffb 100644 --- a/localization/pose_initializer/test/test_copy_vector_to_array.cpp +++ b/localization/pose_initializer/test/test_copy_vector_to_array.cpp @@ -19,7 +19,7 @@ TEST(CopyVectorToArray, CopyAllElements) { const std::vector vector{0, 1, 2, 3, 4}; - std::array array; + std::array array{}; copy_vector_to_array(vector, array); EXPECT_THAT(array, testing::ElementsAre(0, 1, 2, 3, 4)); } @@ -28,7 +28,7 @@ TEST(CopyVectorToArray, CopyZeroElements) { const std::vector vector{}; // just confirm that this works - std::array array; + std::array array{}; copy_vector_to_array(vector, array); } @@ -36,7 +36,7 @@ TEST(CopyVectorToArray, ThrowsInvalidArgumentIfMoreElementsExpected) { auto f = [] { const std::vector vector{0, 1, 2, 3, 4}; - std::array array; + std::array array{}; copy_vector_to_array(vector, array); }; diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md index 89cf6ca3be684..4ced0fa8eb97b 100644 --- a/localization/pose_instability_detector/README.md +++ b/localization/pose_instability_detector/README.md @@ -1,20 +1,111 @@ # pose_instability_detector -The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). +The `pose_instability_detector` is a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). This node triggers periodic timer callbacks to compare two poses: -- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`. +- The pose calculated by dead reckoning starting from the pose of `/localization/kinematic_state` obtained `timer_period` seconds ago. - The latest pose from `/localization/kinematic_state`. The results of this comparison are then output to the `/diagnostics` topic. +![overview](./media/pose_instability_detector_overview.png) + +![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) + If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values. +In other words, WARN outputs indicate that the vehicle has moved to a place outside the expected range based on the twist values. This discrepancy suggests that there may be an issue with either the estimated pose or the input twist. -The following diagram provides an overview of what the timeline of this process looks like: +The following diagram provides an overview of how the procedure looks like: + +![procedure](./media/pose_instabilty_detector_procedure.svg) + +## Dead reckoning algorithm + +Dead reckoning is a method of estimating the position of a vehicle based on its previous position and velocity. +The procedure for dead reckoning is as follows: + +1. Capture the necessary twist values from the `/input/twist` topic. +2. Integrate the twist values to calculate the pose transition. +3. Apply the pose transition to the previous pose to obtain the current pose. + +### Collecting twist values + +The `pose_instability_detector` node collects the twist values from the `~/input/twist` topic to perform dead reckoning. +Ideally, `pose_instability_detector` needs the twist values between the previous pose and the current pose. +Therefore, `pose_instability_detector` snips the twist buffer and apply interpolations and extrapolations to obtain the twist values at the desired time. + +![how_to_snip_necessary_twist](./media/how_to_snip_twist.png) + +### Linear transition and angular transition + +After the twist values are collected, the node calculates the linear transition and angular transition based on the twist values and add them to the previous pose. + +## Threshold definition + +The `pose_instability_detector` node compares the pose calculated by dead reckoning with the latest pose from the EKF output. +These two pose are ideally the same, but in reality, they are not due to the error in the twist values the pose observation. +If these two poses are significantly different so that the absolute value exceeds the threshold, the node outputs a WARN message to the `/diagnostics` topic. +There are six thresholds (x, y, z, roll, pitch, and yaw) to determine whether the poses are significantly different, and these thresholds are determined by the following subsections. + +### `diff_position_x` + +This threshold examines the difference in the longitudinal axis between the two poses, and check whether the vehicle goes beyond the expected error. +This threshold is a sum of "maximum longitudinal error due to velocity scale factor error" and "pose estimation error tolerance". + +$$ +\tau_x = v_{\rm max}\frac{\beta_v}{100} \Delta t + \epsilon_x\\ +$$ -![timeline](./media/timeline.drawio.svg) +| Symbol | Description | Unit | +| ------------- | -------------------------------------------------------------------------------- | ----- | +| $\tau_x$ | Threshold for the difference in the longitudinal axis | $m$ | +| $v_{\rm max}$ | Maximum velocity | $m/s$ | +| $\beta_v$ | Scale factor tolerance for the maximum velocity | $\%$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_x$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the longitudinal axis | $m$ | + +### `diff_position_y` and `diff_position_z` + +These thresholds examine the difference in the lateral and vertical axes between the two poses, and check whether the vehicle goes beyond the expected error. +The `pose_instability_detector` calculates the possible range where the vehicle goes, and get the maximum difference between the nominal dead reckoning pose and the maximum limit pose. + +![lateral_threshold_calculation](./media/lateral_threshold_calculation.png) + +Addition to this, the `pose_instability_detector` node considers the pose estimation error tolerance to determine the threshold. + +$$ +\tau_y = l + \epsilon_y +$$ + +| Symbol | Description | Unit | +| ------------ | ----------------------------------------------------------------------------------------------- | ---- | +| $\tau_y$ | Threshold for the difference in the lateral axis | $m$ | +| $l$ | Maximum lateral distance described in the image above (See the appendix how this is calculated) | $m$ | +| $\epsilon_y$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the lateral axis | $m$ | + +Note that `pose_instability_detector` sets the threshold for the vertical axis as the same as the lateral axis. Only the pose estimator error tolerance is different. + +### `diff_angle_x`, `diff_angle_y`, and `diff_angle_z` + +These thresholds examine the difference in the roll, pitch, and yaw angles between the two poses. +This threshold is a sum of "maximum angular error due to velocity scale factor error and bias error" and "pose estimation error tolerance". + +$$ +\tau_\phi = \tau_\theta = \tau_\psi = \left(\omega_{\rm max}\frac{\beta_\omega}{100} + b \right) \Delta t + \epsilon_\psi +$$ + +| Symbol | Description | Unit | +| ------------------ | ------------------------------------------------------------------------ | ------------- | +| $\tau_\phi$ | Threshold for the difference in the roll angle | ${\rm rad}$ | +| $\tau_\theta$ | Threshold for the difference in the pitch angle | ${\rm rad}$ | +| $\tau_\psi$ | Threshold for the difference in the yaw angle | ${\rm rad}$ | +| $\omega_{\rm max}$ | Maximum angular velocity | ${\rm rad}/s$ | +| $\beta_\omega$ | Scale factor tolerance for the maximum angular velocity | $\%$ | +| $b$ | Bias tolerance of the angular velocity | ${\rm rad}/s$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_\psi$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the yaw angle | ${\rm rad}$ | ## Parameters @@ -34,4 +125,44 @@ The following diagram provides an overview of what the timeline of this process | `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped | diff_pose | | `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Diagnostics | -![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) +## Appendix + +On calculating the maximum lateral distance $l$, the `pose_instability_detector` node will estimate the following poses. + +| Pose | heading velocity $v$ | angular velocity $\omega$ | +| ------------------------------- | ------------------------------------------------ | -------------------------------------------------------------- | +| Nominal dead reckoning pose | $v_{\rm max}$ | $\omega_{\rm max}$ | +| Dead reckoning pose of corner A | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner B | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner C | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | +| Dead reckoning pose of corner D | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | + +Given a heading velocity $v$ and $\omega$, the 2D theoretical variation seen from the previous pose is calculated as follows: + +$$ +\begin{align*} +\left[ + \begin{matrix} + \Delta x\\ + \Delta y + \end{matrix} +\right] +&= +\left[ + \begin{matrix} + \int_{0}^{\Delta t} v \cos(\omega t) dt\\ + \int_{0}^{\Delta t} v \sin(\omega t) dt + \end{matrix} +\right] +\\ +&= +\left[ + \begin{matrix} + \frac{v}{\omega} \sin(\omega \Delta t)\\ + \frac{v}{\omega} \left(1 - \cos(\omega \Delta t)\right) + \end{matrix} +\right] +\end{align*} +$$ + +We calculate this variation for each corner and get the maximum value of the lateral distance $l$ by comparing the distance between the nominal dead reckoning pose and the corner poses. diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml index d94de020a4a12..d9b11b78885c9 100644 --- a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml +++ b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml @@ -1,9 +1,15 @@ /**: ros__parameters: - interval_sec: 0.5 # [sec] - threshold_diff_position_x: 1.0 # [m] - threshold_diff_position_y: 1.0 # [m] - threshold_diff_position_z: 1.0 # [m] - threshold_diff_angle_x: 1.0 # [rad] - threshold_diff_angle_y: 1.0 # [rad] - threshold_diff_angle_z: 1.0 # [rad] + timer_period: 0.5 # [sec] + + heading_velocity_maximum: 16.667 # [m/s] + heading_velocity_scale_factor_tolerance: 3.0 # [%] + + angular_velocity_maximum: 0.523 # [rad/s] + angular_velocity_scale_factor_tolerance: 0.2 # [%] + angular_velocity_bias_tolerance: 0.00698 # [rad/s] + + pose_estimator_longitudinal_tolerance: 0.11 # [m] + pose_estimator_lateral_tolerance: 0.11 # [m] + pose_estimator_vertical_tolerance: 0.11 # [m] + pose_estimator_angular_tolerance: 0.0175 # [rad] diff --git a/localization/pose_instability_detector/src/pose_instability_detector.hpp b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp similarity index 57% rename from localization/pose_instability_detector/src/pose_instability_detector.hpp rename to localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp index 761a10b7a6bf7..0a55a5005dde1 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.hpp +++ b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_INSTABILITY_DETECTOR_HPP_ -#define POSE_INSTABILITY_DETECTOR_HPP_ +#ifndef AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ +#define AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ #include @@ -22,6 +22,8 @@ #include #include +#include +#include #include class PoseInstabilityDetector : public rclcpp::Node @@ -37,13 +39,31 @@ class PoseInstabilityDetector : public rclcpp::Node using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; public: + struct ThresholdValues + { + double position_x; + double position_y; + double position_z; + double angle_x; + double angle_y; + double angle_z; + }; + explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ThresholdValues calculate_threshold(double interval_sec); + void dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose); private: void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); void callback_timer(); + std::deque clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time); + // subscribers and timer rclcpp::Subscription::SharedPtr odometry_sub_; rclcpp::Subscription::SharedPtr twist_sub_; @@ -54,17 +74,26 @@ class PoseInstabilityDetector : public rclcpp::Node rclcpp::Publisher::SharedPtr diagnostics_pub_; // parameters - const double threshold_diff_position_x_; - const double threshold_diff_position_y_; - const double threshold_diff_position_z_; - const double threshold_diff_angle_x_; - const double threshold_diff_angle_y_; - const double threshold_diff_angle_z_; + const double timer_period_; // [sec] + + ThresholdValues threshold_values_; + + const double heading_velocity_maximum_; // [m/s] + const double heading_velocity_scale_factor_tolerance_; // [%] + + const double angular_velocity_maximum_; // [rad/s] + const double angular_velocity_scale_factor_tolerance_; // [%] + const double angular_velocity_bias_tolerance_; // [rad/s] + + const double pose_estimator_longitudinal_tolerance_; // [m] + const double pose_estimator_lateral_tolerance_; // [m] + const double pose_estimator_vertical_tolerance_; // [m] + const double pose_estimator_angular_tolerance_; // [rad] // variables std::optional latest_odometry_ = std::nullopt; std::optional prev_odometry_ = std::nullopt; - std::vector twist_buffer_; + std::deque twist_buffer_; }; -#endif // POSE_INSTABILITY_DETECTOR_HPP_ +#endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/pose_instability_detector/media/how_to_snip_twist.png new file mode 100644 index 0000000000000..6dc66a480e769 Binary files /dev/null and b/localization/pose_instability_detector/media/how_to_snip_twist.png differ diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/pose_instability_detector/media/lateral_threshold_calculation.png new file mode 100644 index 0000000000000..cd919cdcb0383 Binary files /dev/null and b/localization/pose_instability_detector/media/lateral_threshold_calculation.png differ diff --git a/localization/pose_instability_detector/media/pose_instability_detector_overview.png b/localization/pose_instability_detector/media/pose_instability_detector_overview.png new file mode 100644 index 0000000000000..ea8f7a81700ae Binary files /dev/null and b/localization/pose_instability_detector/media/pose_instability_detector_overview.png differ diff --git a/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg new file mode 100644 index 0000000000000..ba45b1f52600b --- /dev/null +++ b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg @@ -0,0 +1,316 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Get necessary subsequence +
+ from twist buffer +
+
+
+
+ +
+
+
+ + + + + + + + + + + +
+
+
+ Update +
+ + latest pose +
+
+
+
+
+
+ +
+
+
+ + + + + + + + + + + +
+
+
+ Calculate integration of +
+ the twist subsequence +
+
+
+
+ +
+
+
+ + + + + + + + + + + +
+
+
+
+ Calculate relative difference between +
+
+
+ + latest pose +
+
+
+
and
+
+ previous pose + twist integration +
+
+
+
+
+
+ +
+
+
+ + + + + + + + + + + +
+
+
+
+ Compare pose difference with thresholds +
+
+
+ Output results as + /diagnostics +
+
+
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+
+ + previous pose ← latest pose +
+
+
+
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+ Timer Callback +
+
+
+
+ +
+
+
+ + + + + + + +
+
+
+ Odometry Subscription Callback +
+
+
+
+ +
+
+
+
+
diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png index b3ad402e48ba7..9bad665db17e5 100644 Binary files a/localization/pose_instability_detector/media/rqt_runtime_monitor.png and b/localization/pose_instability_detector/media/rqt_runtime_monitor.png differ diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json index 560d39a2d5bff..53380f8e7f252 100644 --- a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -1,62 +1,83 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Pose Instability Detector Nodes", + "title": "Parameters for Pose Instability Detector Node", "type": "object", "definitions": { "pose_instability_detector_node": { "type": "object", "properties": { - "interval_sec": { + "timer_period": { "type": "number", "default": 0.5, "exclusiveMinimum": 0, - "description": "The interval of timer_callback in seconds." + "description": "The period of timer_callback (sec)." }, - "threshold_diff_position_x": { + "heading_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 16.667, "minimum": 0.0, - "description": "The threshold of diff_position x (m)." + "description": "The maximum of heading velocity (m/s)." }, - "threshold_diff_position_y": { + "heading_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 3.0, "minimum": 0.0, - "description": "The threshold of diff_position y (m)." + "description": "The tolerance of heading velocity scale factor (%)." }, - "threshold_diff_position_z": { + "angular_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 0.523, "minimum": 0.0, - "description": "The threshold of diff_position z (m)." + "description": "The maximum of angular velocity (rad/s)." }, - "threshold_diff_angle_x": { + "angular_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 0.2, "minimum": 0.0, - "description": "The threshold of diff_angle x (rad)." + "description": "The tolerance of angular velocity scale factor (%)." }, - "threshold_diff_angle_y": { + "angular_velocity_bias_tolerance": { "type": "number", - "default": 1.0, + "default": 0.00698, "minimum": 0.0, - "description": "The threshold of diff_angle y (rad)." + "description": "The tolerance of angular velocity bias (rad/s)." }, - "threshold_diff_angle_z": { + "pose_estimator_longitudinal_tolerance": { "type": "number", - "default": 1.0, + "default": 0.11, "minimum": 0.0, - "description": "The threshold of diff_angle z (rad)." + "description": "The tolerance of longitudinal position of pose estimator (m)." + }, + "pose_estimator_lateral_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of lateral position of pose estimator (m)." + }, + "pose_estimator_vertical_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of vertical position of pose estimator (m)." + }, + "pose_estimator_angular_tolerance": { + "type": "number", + "default": 0.0175, + "minimum": 0.0, + "description": "The tolerance of roll angle of pose estimator (rad)." } }, "required": [ - "interval_sec", - "threshold_diff_position_x", - "threshold_diff_position_y", - "threshold_diff_position_z", - "threshold_diff_angle_x", - "threshold_diff_angle_y", - "threshold_diff_angle_z" + "timer_period", + "heading_velocity_maximum", + "heading_velocity_scale_factor_tolerance", + "angular_velocity_maximum", + "angular_velocity_scale_factor_tolerance", + "angular_velocity_bias_tolerance", + "pose_estimator_longitudinal_tolerance", + "pose_estimator_lateral_tolerance", + "pose_estimator_vertical_tolerance", + "pose_estimator_angular_tolerance" ] } }, diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index c2bce6a3db288..fa7b2ecf16562 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -20,17 +20,32 @@ #include +#include +#include +#include #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_instability_detector", options), - threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), - threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), - threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), - threshold_diff_angle_x_(this->declare_parameter("threshold_diff_angle_x")), - threshold_diff_angle_y_(this->declare_parameter("threshold_diff_angle_y")), - threshold_diff_angle_z_(this->declare_parameter("threshold_diff_angle_z")) + timer_period_(this->declare_parameter("timer_period")), + heading_velocity_maximum_(this->declare_parameter("heading_velocity_maximum")), + heading_velocity_scale_factor_tolerance_( + this->declare_parameter("heading_velocity_scale_factor_tolerance")), + angular_velocity_maximum_(this->declare_parameter("angular_velocity_maximum")), + angular_velocity_scale_factor_tolerance_( + this->declare_parameter("angular_velocity_scale_factor_tolerance")), + angular_velocity_bias_tolerance_( + this->declare_parameter("angular_velocity_bias_tolerance")), + pose_estimator_longitudinal_tolerance_( + this->declare_parameter("pose_estimator_longitudinal_tolerance")), + pose_estimator_lateral_tolerance_( + this->declare_parameter("pose_estimator_lateral_tolerance")), + pose_estimator_vertical_tolerance_( + this->declare_parameter("pose_estimator_vertical_tolerance")), + pose_estimator_angular_tolerance_( + this->declare_parameter("pose_estimator_angular_tolerance")) { + // Define subscribers, publishers and a timer. odometry_sub_ = this->create_subscription( "~/input/odometry", 10, std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1)); @@ -39,9 +54,8 @@ PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & opt "~/input/twist", 10, std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1)); - const double interval_sec = this->declare_parameter("interval_sec"); timer_ = rclcpp::create_timer( - this, this->get_clock(), std::chrono::duration(interval_sec), + this, this->get_clock(), std::chrono::duration(timer_period_), std::bind(&PoseInstabilityDetector::callback_timer, this)); diff_pose_pub_ = this->create_publisher("~/debug/diff_pose", 10); @@ -61,6 +75,7 @@ void PoseInstabilityDetector::callback_twist( void PoseInstabilityDetector::callback_timer() { + // odometry callback and timer callback has to be called at least once if (latest_odometry_ == std::nullopt) { return; } @@ -69,6 +84,16 @@ void PoseInstabilityDetector::callback_timer() return; } + // twist callback has to be called at least once + if (twist_buffer_.empty()) { + return; + } + + // time variables + const rclcpp::Time latest_odometry_time = rclcpp::Time(latest_odometry_->header.stamp); + const rclcpp::Time prev_odometry_time = rclcpp::Time(prev_odometry_->header.stamp); + + // define lambda function to convert quaternion to rpy auto quat_to_rpy = [](const Quaternion & quat) { tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); tf2::Matrix3x3 mat(tf2_quat); @@ -79,70 +104,48 @@ void PoseInstabilityDetector::callback_timer() return std::make_tuple(roll, pitch, yaw); }; - Pose pose = prev_odometry_->pose.pose; - rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp); - for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) { - const Twist twist = twist_with_cov.twist.twist; - - const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp); - if (curr_time > latest_odometry_->header.stamp) { + // delete twist data older than prev_odometry_ (but preserve the one right before prev_odometry_) + while (twist_buffer_.size() > 1) { + if (rclcpp::Time(twist_buffer_[1].header.stamp) < prev_odometry_time) { + twist_buffer_.pop_front(); + } else { break; } + } - const rclcpp::Duration time_diff = curr_time - prev_time; - const double time_diff_sec = time_diff.seconds(); - if (time_diff_sec < 0.0) { - continue; - } - - // quat to rpy - auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation); - - // rpy update - ang_x += twist.angular.x * time_diff_sec; - ang_y += twist.angular.y * time_diff_sec; - ang_z += twist.angular.z * time_diff_sec; - tf2::Quaternion quat; - quat.setRPY(ang_x, ang_y, ang_z); + // dead reckoning from prev_odometry_ to latest_odometry_ + PoseStamped::SharedPtr prev_pose = std::make_shared(); + prev_pose->header = prev_odometry_->header; + prev_pose->pose = prev_odometry_->pose.pose; - // Convert twist to world frame - tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); - linear_velocity = tf2::quatRotate(quat, linear_velocity); - - // update - pose.position.x += linear_velocity.x() * time_diff_sec; - pose.position.y += linear_velocity.y() * time_diff_sec; - pose.position.z += linear_velocity.z() * time_diff_sec; - pose.orientation.x = quat.x(); - pose.orientation.y = quat.y(); - pose.orientation.z = quat.z(); - pose.orientation.w = quat.w(); - prev_time = curr_time; - } + Pose::SharedPtr DR_pose = std::make_shared(); + dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, DR_pose); - // compare pose and latest_odometry_ + // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose); - const geometry_msgs::msg::Point pos = ekf_to_odom.position; - const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation); + const Pose ekf_to_DR = tier4_autoware_utils::inverseTransformPose(*DR_pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_DR.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_DR.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; - const rclcpp::Time stamp = latest_odometry_->header.stamp; - // publish diff_pose for debug PoseStamped diff_pose; - diff_pose.header = latest_odometry_->header; - diff_pose.pose = ekf_to_odom; + diff_pose.header.stamp = latest_odometry_time; + diff_pose.header.frame_id = "base_link"; + diff_pose.pose = ekf_to_DR; diff_pose_pub_->publish(diff_pose); - const std::vector thresholds = {threshold_diff_position_x_, threshold_diff_position_y_, - threshold_diff_position_z_, threshold_diff_angle_x_, - threshold_diff_angle_y_, threshold_diff_angle_z_}; + // publish diagnostics + ThresholdValues threshold_values = + calculate_threshold((latest_odometry_time - prev_odometry_time).seconds()); + + const std::vector thresholds = {threshold_values.position_x, threshold_values.position_y, + threshold_values.position_z, threshold_values.angle_x, + threshold_values.angle_y, threshold_values.angle_z}; const std::vector labels = {"diff_position_x", "diff_position_y", "diff_position_z", "diff_angle_x", "diff_angle_y", "diff_angle_z"}; - // publish diagnostics DiagnosticStatus status; status.name = "localization: pose_instability_detector"; status.hardware_id = this->get_name(); @@ -166,13 +169,239 @@ void PoseInstabilityDetector::callback_timer() status.message = (all_ok ? "OK" : "WARN"); DiagnosticArray diagnostics; - diagnostics.header.stamp = stamp; + diagnostics.header.stamp = latest_odometry_time; diagnostics.status.emplace_back(status); diagnostics_pub_->publish(diagnostics); // prepare for next loop prev_odometry_ = latest_odometry_; - twist_buffer_.clear(); +} + +PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold( + double interval_sec) +{ + // Calculate maximum longitudinal difference + const double longitudinal_difference = + heading_velocity_maximum_ * heading_velocity_scale_factor_tolerance_ * 0.01 * interval_sec; + + // Calculate maximum lateral and vertical difference + double lateral_difference = 0.0; + + const std::vector heading_velocity_signs = {1.0, -1.0, -1.0, 1.0}; + const std::vector angular_velocity_signs = {1.0, 1.0, -1.0, -1.0}; + + const double nominal_variation_x = heading_velocity_maximum_ / angular_velocity_maximum_ * + sin(angular_velocity_maximum_ * interval_sec); + const double nominal_variation_y = heading_velocity_maximum_ / angular_velocity_maximum_ * + (1 - cos(angular_velocity_maximum_ * interval_sec)); + + for (int i = 0; i < 4; i++) { + const double edge_heading_velocity = + heading_velocity_maximum_ * + (1 + heading_velocity_signs[i] * heading_velocity_scale_factor_tolerance_ * 0.01); + const double edge_angular_velocity = + angular_velocity_maximum_ * + (1 + angular_velocity_signs[i] * angular_velocity_scale_factor_tolerance_ * 0.01) + + angular_velocity_signs[i] * angular_velocity_bias_tolerance_; + + const double edge_variation_x = + edge_heading_velocity / edge_angular_velocity * sin(edge_angular_velocity * interval_sec); + const double edge_variation_y = edge_heading_velocity / edge_angular_velocity * + (1 - cos(edge_angular_velocity * interval_sec)); + + const double diff_variation_x = edge_variation_x - nominal_variation_x; + const double diff_variation_y = edge_variation_y - nominal_variation_y; + + const double lateral_difference_candidate = abs( + diff_variation_x * sin(angular_velocity_maximum_ * interval_sec) - + diff_variation_y * cos(angular_velocity_maximum_ * interval_sec)); + lateral_difference = std::max(lateral_difference, lateral_difference_candidate); + } + + const double vertical_difference = lateral_difference; + + // Calculate maximum angular difference + const double roll_difference = + (angular_velocity_maximum_ * angular_velocity_scale_factor_tolerance_ * 0.01 + + angular_velocity_bias_tolerance_) * + interval_sec; + const double pitch_difference = roll_difference; + const double yaw_difference = roll_difference; + + // Set thresholds + ThresholdValues result_values; + result_values.position_x = longitudinal_difference + pose_estimator_longitudinal_tolerance_; + result_values.position_y = lateral_difference + pose_estimator_lateral_tolerance_; + result_values.position_z = vertical_difference + pose_estimator_vertical_tolerance_; + result_values.angle_x = roll_difference + pose_estimator_angular_tolerance_; + result_values.angle_y = pitch_difference + pose_estimator_angular_tolerance_; + result_values.angle_z = yaw_difference + pose_estimator_angular_tolerance_; + + return result_values; +} + +void PoseInstabilityDetector::dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose) +{ + // get start time + rclcpp::Time start_time = rclcpp::Time(initial_pose->header.stamp); + + // initialize estimated_pose + estimated_pose->position = initial_pose->pose.position; + estimated_pose->orientation = initial_pose->pose.orientation; + + // cut out necessary twist data + std::deque sliced_twist_deque = + clip_out_necessary_twist(twist_deque, start_time, end_time); + + // dead reckoning + rclcpp::Time prev_odometry_time = rclcpp::Time(sliced_twist_deque.front().header.stamp); + tf2::Quaternion prev_orientation; + tf2::fromMsg(estimated_pose->orientation, prev_orientation); + + for (size_t i = 1; i < sliced_twist_deque.size(); ++i) { + const rclcpp::Time curr_time = rclcpp::Time(sliced_twist_deque[i].header.stamp); + const double time_diff_sec = (curr_time - prev_odometry_time).seconds(); + + const Twist twist = sliced_twist_deque[i - 1].twist.twist; + + // variation of orientation (rpy update) + tf2::Quaternion delta_orientation; + tf2::Vector3 rotation_axis(twist.angular.x, twist.angular.y, twist.angular.z); + if (rotation_axis.length() > 0.0) { + delta_orientation.setRotation( + rotation_axis.normalized(), rotation_axis.length() * time_diff_sec); + } else { + delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); + } + + tf2::Quaternion curr_orientation; + curr_orientation = prev_orientation * delta_orientation; + curr_orientation.normalize(); + + // average quaternion of two frames + tf2::Quaternion average_quat = prev_orientation.slerp(curr_orientation, 0.5); + + // Convert twist to world frame (take average of two frames) + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(average_quat, linear_velocity); + + // xyz update + estimated_pose->position.x += linear_velocity.x() * time_diff_sec; + estimated_pose->position.y += linear_velocity.y() * time_diff_sec; + estimated_pose->position.z += linear_velocity.z() * time_diff_sec; + + // update previous variables + prev_odometry_time = curr_time; + prev_orientation = curr_orientation; + } + estimated_pose->orientation.x = prev_orientation.x(); + estimated_pose->orientation.y = prev_orientation.y(); + estimated_pose->orientation.z = prev_orientation.z(); + estimated_pose->orientation.w = prev_orientation.w(); +} + +std::deque +PoseInstabilityDetector::clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time) +{ + // If there is only one element in the twist_buffer, return a deque that has the same twist + // from the start till the end + if (twist_buffer.size() == 1) { + TwistWithCovarianceStamped twist = twist_buffer.front(); + std::deque simple_twist_deque; + + twist.header.stamp = start_time; + simple_twist_deque.push_back(twist); + + twist.header.stamp = end_time; + simple_twist_deque.push_back(twist); + + return simple_twist_deque; + } + + // get iterator to the element that is right before start_time (if it does not exist, start_it = + // twist_buffer.begin()) + auto start_it = twist_buffer.begin(); + + for (auto it = twist_buffer.begin(); it != twist_buffer.end(); ++it) { + if (rclcpp::Time(it->header.stamp) > start_time) { + break; + } + start_it = it; + } + + // get iterator to the element that is right after end_time (if it does not exist, end_it = + // twist_buffer.end()) + auto end_it = twist_buffer.end(); + end_it--; + for (auto it = end_it; it != twist_buffer.begin(); --it) { + if (rclcpp::Time(it->header.stamp) < end_time) { + break; + } + end_it = it; + } + + // Create result deque + std::deque result_deque(start_it, end_it); + + // If the first element is later than start_time, add the first element to the front of the + // result_deque + if (rclcpp::Time(result_deque.front().header.stamp) > start_time) { + TwistWithCovarianceStamped start_twist = *start_it; + start_twist.header.stamp = start_time; + result_deque.push_front(start_twist); + } else { + // If the first element is earlier than start_time, interpolate the first element + rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp); + double ratio = (start_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[0].twist.twist; + Twist twist1 = result_deque[1].twist.twist; + result_deque[0].twist.twist.linear.x = twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[0].twist.twist.linear.y = twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[0].twist.twist.linear.z = twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[0].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[0].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[0].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[0].header.stamp = start_time; + } + + // If the last element is earlier than end_time, add the last element to the back of the + // result_deque + if (rclcpp::Time(result_deque.back().header.stamp) < end_time) { + TwistWithCovarianceStamped end_twist = *end_it; + end_twist.header.stamp = end_time; + result_deque.push_back(end_twist); + } else { + // If the last element is later than end_time, interpolate the last element + rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp); + double ratio = (end_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[result_deque.size() - 2].twist.twist; + Twist twist1 = result_deque[result_deque.size() - 1].twist.twist; + result_deque[result_deque.size() - 1].twist.twist.linear.x = + twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.y = + twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.z = + twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[result_deque.size() - 1].header.stamp = end_time; + } + return result_deque; } #include diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index 5ea03859d7731..9300984967d4b 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/pose_instability_detector/test/test.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "../src/pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "test_message_helper_node.hpp" #include @@ -81,6 +81,11 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -88,11 +93,6 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); - // send the twist message2 (move 1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -101,7 +101,9 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; @@ -124,6 +126,11 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -131,11 +138,6 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 0.1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); - // send the twist message2 (move 0.1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -144,7 +146,9 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index de675da5cc2d4..d4c8b71fa6b66 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -29,6 +29,7 @@ sophus std_msgs tf2_ros + tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index d0dfc87067f7f..1126d6260b911 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -15,7 +15,7 @@ #include "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" #include -#include +#include #include #include @@ -198,7 +198,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0.6f, 0.6f, 0.6f, 0.999f); + marker.color = tier4_autoware_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); marker.scale.x = 0.1; marker.ns = ns; marker.id = id++; @@ -228,7 +228,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0.4f, 0.4f, 0.8f, 0.999f); + marker.color = tier4_autoware_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); marker.scale.x = 0.2; marker.ns = ns; marker.id = id++; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index 90cd62883e339..e6e59a836f6db 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -93,7 +93,8 @@ class ParticleVisualize : public rclcpp::Node marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(bound_weight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(bound_weight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp index 955ed84fdea2d..888baf0bea0ac 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -45,7 +45,8 @@ void ParticleVisualizer::publish(const ParticleArray & msg) marker.scale.x = 0.3; marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(boundWeight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(boundWeight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp index b321986240b69..e8c8890561a7f 100644 --- a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp @@ -136,7 +136,7 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo marker.pose.position.z = latest_height_.data; float prob = i / 4.0f; - marker.color = common::color_scale::rainbow(prob); + marker.color = static_cast(common::color_scale::rainbow(prob)); marker.color.a = 0.5f; marker.scale.x = 0.1; drawCircle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index ebd9c424edf5e..62d8ed826b373 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -169,7 +169,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0, 0, 1.0f, 1.0f); + marker.color = tier4_autoware_utils::createMarkerColor(0, 0, 1.0f, 1.0f); marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp index c6c34f0e28a42..8a5fc65e2fac3 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -42,7 +42,8 @@ void MarkerModule::publish_marker( marker.type = Marker::ARROW; marker.id = i; marker.ns = "arrow"; - marker.color = common::color_scale::rainbow(normalize(scores.at(i))); + marker.color = + static_cast(common::color_scale::rainbow(normalize(scores.at(i)))); marker.color.a = 0.5; marker.pose.position.x = position.x(); diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/autoware_map_based_prediction/CMakeLists.txt similarity index 90% rename from perception/map_based_prediction/CMakeLists.txt rename to perception/autoware_map_based_prediction/CMakeLists.txt index 786446457143f..9a1d9a1947c7c 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/autoware_map_based_prediction/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(map_based_prediction) +project(autoware_map_based_prediction) find_package(autoware_cmake REQUIRED) autoware_package() @@ -22,7 +22,7 @@ ament_auto_add_library(map_based_prediction_node SHARED target_link_libraries(map_based_prediction_node glog::glog) rclcpp_components_register_node(map_based_prediction_node - PLUGIN "map_based_prediction::MapBasedPredictionNode" + PLUGIN "autoware::map_based_prediction::MapBasedPredictionNode" EXECUTABLE map_based_prediction ) diff --git a/perception/map_based_prediction/README.md b/perception/autoware_map_based_prediction/README.md similarity index 100% rename from perception/map_based_prediction/README.md rename to perception/autoware_map_based_prediction/README.md diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml similarity index 100% rename from perception/map_based_prediction/config/map_based_prediction.param.yaml rename to perception/autoware_map_based_prediction/config/map_based_prediction.param.yaml diff --git a/perception/map_based_prediction/images/exception.svg b/perception/autoware_map_based_prediction/images/exception.svg similarity index 100% rename from perception/map_based_prediction/images/exception.svg rename to perception/autoware_map_based_prediction/images/exception.svg diff --git a/perception/map_based_prediction/images/inside_road.svg b/perception/autoware_map_based_prediction/images/inside_road.svg similarity index 100% rename from perception/map_based_prediction/images/inside_road.svg rename to perception/autoware_map_based_prediction/images/inside_road.svg diff --git a/perception/map_based_prediction/images/outside_road.svg b/perception/autoware_map_based_prediction/images/outside_road.svg similarity index 100% rename from perception/map_based_prediction/images/outside_road.svg rename to perception/autoware_map_based_prediction/images/outside_road.svg diff --git a/perception/map_based_prediction/images/target_objects.svg b/perception/autoware_map_based_prediction/images/target_objects.svg similarity index 100% rename from perception/map_based_prediction/images/target_objects.svg rename to perception/autoware_map_based_prediction/images/target_objects.svg diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp similarity index 99% rename from perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp rename to perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 7a0010637c2f4..22b28b00d645e 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -51,7 +51,7 @@ #include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { struct LateralKinematicsToLanelet { @@ -416,6 +416,6 @@ class MapBasedPredictionNode : public rclcpp::Node return true; }; }; -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction #endif // MAP_BASED_PREDICTION__MAP_BASED_PREDICTION_NODE_HPP_ diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp similarity index 98% rename from perception/map_based_prediction/include/map_based_prediction/path_generator.hpp rename to perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp index 0c1642600f4b1..bd098894f8a88 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/autoware_map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -28,7 +28,7 @@ #include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -145,6 +145,6 @@ class PathGenerator const TrackedObject & object, const PosePath & ref_path, const double duration, const double speed_limit = 0.0) const; }; -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction #endif // MAP_BASED_PREDICTION__PATH_GENERATOR_HPP_ diff --git a/perception/map_based_prediction/launch/map_based_prediction.launch.xml b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml similarity index 72% rename from perception/map_based_prediction/launch/map_based_prediction.launch.xml rename to perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml index 7f81971b2f8d7..b65f0de892dcc 100644 --- a/perception/map_based_prediction/launch/map_based_prediction.launch.xml +++ b/perception/autoware_map_based_prediction/launch/map_based_prediction.launch.xml @@ -1,13 +1,13 @@ - + - + diff --git a/perception/map_based_prediction/media/lane_change_detection.drawio.svg b/perception/autoware_map_based_prediction/media/lane_change_detection.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/lane_change_detection.drawio.svg rename to perception/autoware_map_based_prediction/media/lane_change_detection.drawio.svg diff --git a/perception/map_based_prediction/media/lanechange_detection_right_to_left.png b/perception/autoware_map_based_prediction/media/lanechange_detection_right_to_left.png similarity index 100% rename from perception/map_based_prediction/media/lanechange_detection_right_to_left.png rename to perception/autoware_map_based_prediction/media/lanechange_detection_right_to_left.png diff --git a/perception/map_based_prediction/media/map_based_prediction_flow.drawio.svg b/perception/autoware_map_based_prediction/media/map_based_prediction_flow.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/map_based_prediction_flow.drawio.svg rename to perception/autoware_map_based_prediction/media/map_based_prediction_flow.drawio.svg diff --git a/perception/map_based_prediction/media/object_history.drawio.svg b/perception/autoware_map_based_prediction/media/object_history.drawio.svg similarity index 100% rename from perception/map_based_prediction/media/object_history.drawio.svg rename to perception/autoware_map_based_prediction/media/object_history.drawio.svg diff --git a/perception/map_based_prediction/package.xml b/perception/autoware_map_based_prediction/package.xml similarity index 96% rename from perception/map_based_prediction/package.xml rename to perception/autoware_map_based_prediction/package.xml index 70ad1ec8519d9..3671c1a01b0a2 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/autoware_map_based_prediction/package.xml @@ -1,7 +1,7 @@ - map_based_prediction + autoware_map_based_prediction 0.1.0 This package implements a map_based_prediction Tomoya Kimura diff --git a/perception/map_based_prediction/schema/map_based_prediction.schema.json b/perception/autoware_map_based_prediction/schema/map_based_prediction.schema.json similarity index 100% rename from perception/map_based_prediction/schema/map_based_prediction.schema.json rename to perception/autoware_map_based_prediction/schema/map_based_prediction.schema.json diff --git a/perception/map_based_prediction/src/debug.cpp b/perception/autoware_map_based_prediction/src/debug.cpp similarity index 94% rename from perception/map_based_prediction/src/debug.cpp rename to perception/autoware_map_based_prediction/src/debug.cpp index f78879ba209a1..0c58d4aae1638 100644 --- a/perception/map_based_prediction/src/debug.cpp +++ b/perception/autoware_map_based_prediction/src/debug.cpp @@ -15,7 +15,7 @@ #include "map_based_prediction/map_based_prediction_node.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" -namespace map_based_prediction +namespace autoware::map_based_prediction { visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( const TrackedObject & object, const Maneuver & maneuver, const size_t obj_num) @@ -46,4 +46,4 @@ visualization_msgs::msg::Marker MapBasedPredictionNode::getDebugMarker( return marker; } -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp similarity index 99% rename from perception/map_based_prediction/src/map_based_prediction_node.cpp rename to perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp index a00a43545cf23..c23e78d243f9c 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp @@ -50,7 +50,7 @@ #include #include -namespace map_based_prediction +namespace autoware::map_based_prediction { namespace @@ -2495,7 +2495,7 @@ bool MapBasedPredictionNode::calcIntentionToCrossWithTrafficSignal( return true; } -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction #include -RCLCPP_COMPONENTS_REGISTER_NODE(map_based_prediction::MapBasedPredictionNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::map_based_prediction::MapBasedPredictionNode) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/autoware_map_based_prediction/src/path_generator.cpp similarity index 99% rename from perception/map_based_prediction/src/path_generator.cpp rename to perception/autoware_map_based_prediction/src/path_generator.cpp index 52e104f572f76..873219ffcf130 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/autoware_map_based_prediction/src/path_generator.cpp @@ -20,7 +20,7 @@ #include -namespace map_based_prediction +namespace autoware::map_based_prediction { PathGenerator::PathGenerator( const double sampling_time_interval, const double min_crosswalk_user_velocity) @@ -487,4 +487,4 @@ FrenetPoint PathGenerator::getFrenetPoint( return frenet_point; } -} // namespace map_based_prediction +} // namespace autoware::map_based_prediction diff --git a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp similarity index 84% rename from perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp rename to perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp index 518b28a555849..08e5b4977e583 100644 --- a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp +++ b/perception/autoware_map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -54,8 +54,9 @@ TEST(PathGenerator, test_generatePathForNonVehicleObject) const double prediction_time_horizon = 10.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate pedestrian object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); @@ -77,8 +78,9 @@ TEST(PathGenerator, test_generatePathForLowSpeedVehicle) const double prediction_time_horizon = 10.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); @@ -100,8 +102,9 @@ TEST(PathGenerator, test_generatePathForOffLaneVehicle) const double prediction_time_horizon = 10.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); @@ -123,14 +126,15 @@ TEST(PathGenerator, test_generatePathForOnLaneVehicle) const double lateral_control_time_horizon = 5.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); // Generate reference path - map_based_prediction::PosePath ref_paths; + autoware::map_based_prediction::PosePath ref_paths; geometry_msgs::msg::Pose pose; pose.position.x = 0.0; pose.position.y = 0.0; @@ -154,14 +158,15 @@ TEST(PathGenerator, test_generatePathForCrosswalkUser) const double prediction_time_horizon = 10.0; const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); // Generate dummy crosswalk - map_based_prediction::CrosswalkEdgePoints reachable_crosswalk; + autoware::map_based_prediction::CrosswalkEdgePoints reachable_crosswalk; reachable_crosswalk.front_center_point << 0.0, 0.0; reachable_crosswalk.front_right_point << 1.0, 0.0; reachable_crosswalk.front_left_point << -1.0, 0.0; @@ -185,8 +190,9 @@ TEST(PathGenerator, test_generatePathToTargetPoint) // Generate Path generator const double prediction_sampling_time_interval = 0.5; const double min_crosswalk_user_velocity = 0.1; - const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( - prediction_sampling_time_interval, min_crosswalk_user_velocity); + const autoware::map_based_prediction::PathGenerator path_generator = + autoware::map_based_prediction::PathGenerator( + prediction_sampling_time_interval, min_crosswalk_user_velocity); // Generate dummy object TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); diff --git a/perception/map_based_prediction/test/test_map_based_prediction.cpp b/perception/autoware_map_based_prediction/test/test_map_based_prediction.cpp similarity index 100% rename from perception/map_based_prediction/test/test_map_based_prediction.cpp rename to perception/autoware_map_based_prediction/test/test_map_based_prediction.cpp diff --git a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp index d97bbaa2118e5..314c9cfb97a75 100644 --- a/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp +++ b/perception/ground_segmentation/include/ground_segmentation/scan_ground_filter_nodelet.hpp @@ -18,7 +18,7 @@ #include "pointcloud_preprocessor/filter.hpp" #include "pointcloud_preprocessor/transform_info.hpp" -#include +#include #include @@ -43,7 +43,7 @@ class ScanGroundFilterTest; namespace ground_segmentation { -using vehicle_info_util::VehicleInfo; +using autoware::vehicle_info_utils::VehicleInfo; class ScanGroundFilterComponent : public pointcloud_preprocessor::Filter { diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.py b/perception/ground_segmentation/launch/scan_ground_filter.launch.py index d3d856925b9db..df8290997315e 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.py +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.py @@ -85,7 +85,7 @@ def add_launch_arg(name: str, default_value=None): return DeclareLaunchArgument(name, default_value=default_value) default_vehicle_info_param = os.path.join( - get_package_share_directory("vehicle_info_util"), "config/vehicle_info.param.yaml" + get_package_share_directory("autoware_vehicle_info_utils"), "config/vehicle_info.param.yaml" ) vehicle_info_param = DeclareLaunchArgument( diff --git a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml index 2a9e4983ecb56..1146138211211 100644 --- a/perception/ground_segmentation/launch/scan_ground_filter.launch.xml +++ b/perception/ground_segmentation/launch/scan_ground_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/ground_segmentation/package.xml b/perception/ground_segmentation/package.xml index 05d5d18baf29e..1b807b393b739 100644 --- a/perception/ground_segmentation/package.xml +++ b/perception/ground_segmentation/package.xml @@ -19,6 +19,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils libopencv-dev pcl_conversions pcl_ros @@ -31,7 +32,6 @@ tf2_eigen tf2_ros tf2_sensor_msgs - vehicle_info_util yaml-cpp ament_lint_auto diff --git a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp index a63d218df62de..e638baba12521 100644 --- a/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp +++ b/perception/ground_segmentation/src/scan_ground_filter_nodelet.cpp @@ -14,10 +14,10 @@ #include "ground_segmentation/scan_ground_filter_nodelet.hpp" +#include #include #include #include -#include #include #include @@ -25,12 +25,12 @@ namespace ground_segmentation { +using autoware::vehicle_info_utils::VehicleInfoUtils; using pointcloud_preprocessor::get_param; using tier4_autoware_utils::calcDistance3d; using tier4_autoware_utils::deg2rad; using tier4_autoware_utils::normalizeDegree; using tier4_autoware_utils::normalizeRadian; -using vehicle_info_util::VehicleInfoUtil; ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & options) : Filter("ScanGroundFilter", options) @@ -59,7 +59,7 @@ ScanGroundFilterComponent::ScanGroundFilterComponent(const rclcpp::NodeOptions & use_recheck_ground_cluster_ = declare_parameter("use_recheck_ground_cluster"); use_lowest_point_ = declare_parameter("use_lowest_point"); radial_dividers_num_ = std::ceil(2.0 * M_PI / radial_divider_angle_rad_); - vehicle_info_ = VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = VehicleInfoUtils(*this).getVehicleInfo(); grid_mode_switch_grid_id_ = grid_mode_switch_radius_ / grid_size_m_; // changing the mode of grid division diff --git a/perception/lidar_transfusion/CMakeLists.txt b/perception/lidar_transfusion/CMakeLists.txt new file mode 100644 index 0000000000000..c0e2d85f27e62 --- /dev/null +++ b/perception/lidar_transfusion/CMakeLists.txt @@ -0,0 +1,156 @@ +cmake_minimum_required(VERSION 3.14) +project(lidar_transfusion) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_compile_options(-Wno-deprecated-declarations) + +option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) + +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if(CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if(CUDA_VERBOSE) + message("CUDA is available!") + message("CUDA Libs: ${CUDA_LIBRARIES}") + message("CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif() + # Note: cublas_device was depreciated in CUDA version 9.2 + # https://forums.developer.nvidia.com/t/where-can-i-find-libcublas-device-so-or-libcublas-device-a/67251/4 + # In LibTorch, CUDA_cublas_device_LIBRARY is used. + unset(CUDA_cublas_device_LIBRARY CACHE) + set(CUDA_AVAIL ON) +else() + message("CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif() + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER nvinfer) +find_library(NVONNXPARSER nvonnxparser) +if(NVINFER AND NVONNXPARSER) + if(CUDA_VERBOSE) + message("TensorRT is available!") + message("NVINFER: ${NVINFER}") + message("NVONNXPARSER: ${NVONNXPARSER}") + endif() + set(TRT_AVAIL ON) +else() + message("TensorRT is NOT Available") + set(TRT_AVAIL OFF) +endif() + +# set flags for CUDNN availability +option(CUDNN_AVAIL "CUDNN available" OFF) +# try to find the CUDNN module +find_library(CUDNN_LIBRARY +NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} +PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} +PATH_SUFFIXES lib lib64 bin +DOC "CUDNN library." +) +if(CUDNN_LIBRARY) + if(CUDA_VERBOSE) + message(STATUS "CUDNN is available!") + message(STATUS "CUDNN_LIBRARY: ${CUDNN_LIBRARY}") + endif() + set(CUDNN_AVAIL ON) +else() + message("CUDNN is NOT Available") + set(CUDNN_AVAIL OFF) +endif() + +if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + include_directories( + include + ${CUDA_INCLUDE_DIRS} + ) + + if(CMAKE_BUILD_TYPE STREQUAL "Debug") + set(CMAKE_CUDA_FLAGS ${CMAKE_CUDA_FLAGS} "-g -G") + set(CUDA_NVCC_FLAGS "-g -G") + endif() + + ament_auto_add_library(transfusion_lib SHARED + lib/detection_class_remapper.cpp + lib/network/network_trt.cpp + lib/postprocess/non_maximum_suppression.cpp + lib/preprocess/voxel_generator.cpp + lib/preprocess/pointcloud_densification.cpp + lib/ros_utils.cpp + lib/transfusion_trt.cpp + ) + + cuda_add_library(transfusion_cuda_lib SHARED + lib/postprocess/circle_nms_kernel.cu + lib/postprocess/postprocess_kernel.cu + lib/preprocess/preprocess_kernel.cu + ) + + target_link_libraries(transfusion_lib + ${NVINFER} + ${NVONNXPARSER} + ${CUDA_LIBRARIES} + ${CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${CUDNN_LIBRARY} + transfusion_cuda_lib + ) + + target_include_directories(transfusion_lib + PUBLIC + $ + $ + ) + + # To suppress unknown-pragmas error. The root-cause is CUB library in CUDA 11.6. + # This issue was fixed by https://github.com/NVIDIA/cub/commit/7d608bf1dc14553e2fb219eabeed80b76621b6fe + target_include_directories(transfusion_lib + SYSTEM PUBLIC + ${CUDA_INCLUDE_DIRS} + ) + + ament_auto_add_library(lidar_transfusion_component SHARED + src/lidar_transfusion_node.cpp + ) + + target_link_libraries(lidar_transfusion_component + transfusion_lib + ) + + rclcpp_components_register_node(lidar_transfusion_component + PLUGIN "lidar_transfusion::LidarTransfusionNode" + EXECUTABLE lidar_transfusion_node + ) + + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + endif() + + ament_auto_package( + INSTALL_TO_SHARE + launch + config + ) + +else() + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + ament_auto_package( + INSTALL_TO_SHARE + launch + ) +endif() diff --git a/perception/lidar_transfusion/README.md b/perception/lidar_transfusion/README.md new file mode 100644 index 0000000000000..7974714720c32 --- /dev/null +++ b/perception/lidar_transfusion/README.md @@ -0,0 +1,113 @@ +# lidar_transfusion + +## Purpose + +The `lidar_transfusion` package is used for 3D object detection based on lidar data (x, y, z, intensity). + +## Inner-workings / Algorithms + +The implementation bases on TransFusion [1] work. It uses TensorRT library for data process and network inference. + +We trained the models using . + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------- | ------------------------------- | ----------------- | +| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | Input pointcloud. | + +### Output + +| Name | Type | Description | +| -------------------------------------- | ------------------------------------------------ | --------------------------- | +| `~/output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | Detected objects. | +| `debug/cyclic_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Cyclic time (ms). | +| `debug/pipeline_latency_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Pipeline latency time (ms). | +| `debug/processing_time/preprocess_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Preprocess (ms). | +| `debug/processing_time/inference_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Inference time (ms). | +| `debug/processing_time/postprocess_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Postprocess time (ms). | +| `debug/processing_time/total_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Total processing time (ms). | + +## Parameters + +### TransFusion + +{{ json_to_markdown("perception/lidar_transfusion/schema/transfusion.schema.json") }} + +### Detection class remapper + +{{ json_to_markdown("perception/lidar_transfusion/schema/detection_class_remapper.schema.json") }} + +### The `build_only` option + +The `lidar_transfusion` node has `build_only` option to build the TensorRT engine file from the ONNX file. +Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: + +```bash +ros2 launch lidar_transfusion lidar_transfusion.launch.xml build_only:=true +``` + +### The `log_level` option + +The default logging severity level for `lidar_transfusion` is `info`. For debugging purposes, the developer may decrease severity level using `log_level` parameter: + +```bash +ros2 launch lidar_transfusion lidar_transfusion.launch.xml log_level:=debug +``` + +## Assumptions / Known limits + +## Trained Models + +You can download the onnx format of trained models by clicking on the links below. + +- TransFusion: [transfusion.onnx](https://awf.ml.dev.web.auto/perception/models/transfusion/v1/transfusion.onnx) + +The model was trained in TIER IV's internal database (~11k lidar frames) for 20 epochs. + +### Changelog + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## References/External links + +[1] Xuyang Bai, Zeyu Hu, Xinge Zhu, Qingqiu Huang, Yilun Chen, Hongbo Fu and Chiew-Lan Tai. "TransFusion: Robust LiDAR-Camera Fusion for 3D Object Detection with Transformers." arXiv preprint arXiv:2203.11496 (2022). + +[2] + +[3] + +[4] + +[5] + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/lidar_transfusion/config/detection_class_remapper.param.yaml b/perception/lidar_transfusion/config/detection_class_remapper.param.yaml new file mode 100644 index 0000000000000..baea087c96bca --- /dev/null +++ b/perception/lidar_transfusion/config/detection_class_remapper.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + allow_remapping_by_area_matrix: + # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. + # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] + # row: original class. column: class to remap to + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 0, 1, 0, 1, 0, 0, 0, #CAR + 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK + 0, 0, 0, 0, 1, 0, 0, 0, #BUS + 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE + 0, 0, 0, 0, 0, 0, 0, 0] #PEDESTRIAN + + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN + + + max_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 36.000, 0.000, 999.999, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN diff --git a/perception/lidar_transfusion/config/transfusion.param.yaml b/perception/lidar_transfusion/config/transfusion.param.yaml new file mode 100644 index 0000000000000..feabe359caf1f --- /dev/null +++ b/perception/lidar_transfusion/config/transfusion.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # network + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + trt_precision: fp16 + voxels_num: [5000, 30000, 60000] # [min, opt, max] + point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] # [x_min, y_min, z_min, x_max, y_max, z_max] + voxel_size: [0.3, 0.3, 8.0] # [x, y, z] + onnx_path: "$(var model_path)/transfusion.onnx" + engine_path: "$(var model_path)/transfusion.engine" + # pre-process params + densification_num_past_frames: 1 + densification_world_frame_id: map + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] # refers to the class_names + score_threshold: 0.2 diff --git a/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp new file mode 100644 index 0000000000000..5c25a936d5392 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp @@ -0,0 +1,126 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +/* + * This code is licensed under CC0 1.0 Universal (Public Domain). + * You can use this without any limitation. + * https://creativecommons.org/publicdomain/zero/1.0/deed.en + */ + +#ifndef LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#define LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ + +#include + +#include +#include +#include +#include + +#define CHECK_CUDA_ERROR(e) (cuda::check_error(e, __FILE__, __LINE__)) + +namespace cuda +{ +inline void check_error(const ::cudaError_t e, const char * f, int n) +{ + if (e != ::cudaSuccess) { + ::std::stringstream s; + s << ::cudaGetErrorName(e) << " (" << e << ")@" << f << "#L" << n << ": " + << ::cudaGetErrorString(e); + throw ::std::runtime_error{s.str()}; + } +} + +struct deleter +{ + void operator()(void * p) const { CHECK_CUDA_ERROR(::cudaFree(p)); } +}; + +template +using unique_ptr = ::std::unique_ptr; + +template +typename ::std::enable_if<::std::is_array::value, cuda::unique_ptr>::type make_unique( + const ::std::size_t n) +{ + using U = typename ::std::remove_extent::type; + U * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(U) * n)); + return cuda::unique_ptr{p}; +} + +template +cuda::unique_ptr make_unique() +{ + T * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(T))); + return cuda::unique_ptr{p}; +} + +constexpr size_t CUDA_ALIGN = 256; + +template +inline size_t get_size_aligned(size_t num_elem) +{ + size_t size = num_elem * sizeof(T); + size_t extra_align = 0; + if (size % CUDA_ALIGN != 0) { + extra_align = CUDA_ALIGN - size % CUDA_ALIGN; + } + return size + extra_align; +} + +template +inline T * get_next_ptr(size_t num_elem, void *& workspace, size_t & workspace_size) +{ + size_t size = get_size_aligned(num_elem); + if (size > workspace_size) { + throw ::std::runtime_error("Workspace is too small!"); + } + workspace_size -= size; + T * ptr = reinterpret_cast(workspace); + workspace = reinterpret_cast(reinterpret_cast(workspace) + size); + return ptr; +} + +template +void clear_async(T * ptr, size_t num_elem, cudaStream_t stream) +{ + CHECK_CUDA_ERROR(::cudaMemsetAsync(ptr, 0, sizeof(T) * num_elem, stream)); +} + +} // namespace cuda + +#endif // LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp b/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp new file mode 100644 index 0000000000000..ace7dc5ff3405 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp @@ -0,0 +1,47 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#define LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ + +#include + +#include +#include +#include +#include + +#include + +namespace lidar_transfusion +{ + +class DetectionClassRemapper +{ +public: + void setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix); + void mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg); + +protected: + Eigen::Matrix allow_remapping_by_area_matrix_; + Eigen::MatrixXd min_area_matrix_; + Eigen::MatrixXd max_area_matrix_; + int num_labels_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp new file mode 100644 index 0000000000000..86303618d7877 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#define LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ + +#include "lidar_transfusion/detection_class_remapper.hpp" +#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/transfusion_trt.hpp" +#include "lidar_transfusion/visibility_control.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace lidar_transfusion +{ + +class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node +{ +public: + explicit LidarTransfusionNode(const rclcpp::NodeOptions & options); + +private: + void cloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + + rclcpp::Subscription::ConstSharedPtr cloud_sub_{nullptr}; + rclcpp::Publisher::SharedPtr objects_pub_{ + nullptr}; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + DetectionClassRemapper detection_class_remapper_; + float score_threshold_{0.0}; + std::vector class_names_; + + NonMaximumSuppression iou_bev_nms_; + + std::unique_ptr detector_ptr_{nullptr}; + + // debugger + std::unique_ptr> stop_watch_ptr_{ + nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr published_time_pub_{nullptr}; +}; +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp new file mode 100644 index 0000000000000..3f8f32e346b0a --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#define LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ + +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +struct ProfileDimension +{ + nvinfer1::Dims min; + nvinfer1::Dims opt; + nvinfer1::Dims max; + + bool operator!=(const ProfileDimension & rhs) const + { + return min.nbDims != rhs.min.nbDims || opt.nbDims != rhs.opt.nbDims || + max.nbDims != rhs.max.nbDims || !std::equal(min.d, min.d + min.nbDims, rhs.min.d) || + !std::equal(opt.d, opt.d + opt.nbDims, rhs.opt.d) || + !std::equal(max.d, max.d + max.nbDims, rhs.max.d); + } +}; + +class NetworkTRT +{ +public: + explicit NetworkTRT(const TransfusionConfig & config); + ~NetworkTRT(); + + bool init( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision); + const char * getTensorName(NetworkIO name); + + tensorrt_common::TrtUniquePtr engine{nullptr}; + tensorrt_common::TrtUniquePtr context{nullptr}; + +private: + bool parseONNX( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision, + size_t workspace_size = (1ULL << 30)); + bool saveEngine(const std::string & engine_path); + bool loadEngine(const std::string & engine_path); + bool createContext(); + bool setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config); + bool validateNetworkIO(); + nvinfer1::Dims validateTensorShape(NetworkIO name, const std::vector shape); + + tensorrt_common::TrtUniquePtr runtime_{nullptr}; + tensorrt_common::TrtUniquePtr plan_{nullptr}; + tensorrt_common::Logger logger_; + TransfusionConfig config_; + std::vector tensors_names_; + + std::array in_profile_dims_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp new file mode 100644 index 0000000000000..e231482652b98 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp @@ -0,0 +1,32 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ + +#include "lidar_transfusion/utils.hpp" + +#include + +namespace lidar_transfusion +{ +// Non-maximum suppression (NMS) uses the distance on the xy plane instead of +// intersection over union (IoU) to suppress overlapped objects. +std::size_t circleNMS( + thrust::device_vector & boxes3d, const float distance_threshold, + thrust::device_vector & keep_mask, cudaStream_t stream); + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp new file mode 100644 index 0000000000000..daf2cc7c1fac1 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ + +#include "lidar_transfusion/ros_utils.hpp" + +#include + +#include + +#include +#include + +namespace lidar_transfusion +{ +using autoware_perception_msgs::msg::DetectedObject; + +enum class NMS_TYPE { + IoU_BEV + // IoU_3D + // Distance_2D + // Distance_3D +}; + +struct NMSParams +{ + NMS_TYPE nms_type_{}; + std::vector target_class_names_{}; + double search_distance_2d_{}; + double iou_threshold_{}; + // double distance_threshold_{}; +}; + +std::vector classNamesToBooleanMask(const std::vector & class_names) +{ + std::vector mask; + constexpr std::size_t num_object_classification = 8; + mask.resize(num_object_classification); + for (const auto & class_name : class_names) { + const auto semantic_type = getSemanticType(class_name); + mask.at(semantic_type) = true; + } + + return mask; +} + +class NonMaximumSuppression +{ +public: + void setParameters(const NMSParams &); + + std::vector apply(const std::vector &); + +private: + bool isTargetLabel(const std::uint8_t); + + bool isTargetPairObject(const DetectedObject &, const DetectedObject &); + + Eigen::MatrixXd generateIoUMatrix(const std::vector &); + + NMSParams params_{}; + std::vector target_class_mask_{}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp new file mode 100644 index 0000000000000..01435424aa248 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ + +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include +#include +#include + +#include + +namespace lidar_transfusion +{ + +class PostprocessCuda +{ +public: + explicit PostprocessCuda(const TransfusionConfig & config, cudaStream_t & stream); + + cudaError_t generateDetectedBoxes3D_launch( + const float * cls_output, const float * box_output, const float * dir_cls_output, + std::vector & det_boxes3d, cudaStream_t stream); + +private: + TransfusionConfig config_; + cudaStream_t stream_; + cudaStream_t stream_event_; + cudaEvent_t start_, stop_; + thrust::device_vector boxes3d_d_; + thrust::device_vector yaw_norm_thresholds_d_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp new file mode 100644 index 0000000000000..25095f4dc9c0b --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp @@ -0,0 +1,101 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +namespace lidar_transfusion +{ + +class DensificationParam +{ +public: + DensificationParam(const std::string & world_frame_id, const unsigned int num_past_frames) + : world_frame_id_(std::move(world_frame_id)), + pointcloud_cache_size_(num_past_frames + /*current frame*/ 1) + { + } + + std::string world_frame_id() const { return world_frame_id_; } + unsigned int pointcloud_cache_size() const { return pointcloud_cache_size_; } + +private: + std::string world_frame_id_; + unsigned int pointcloud_cache_size_{1}; +}; + +struct PointCloudWithTransform +{ + cuda::unique_ptr points_d{nullptr}; + std_msgs::msg::Header header; + size_t num_points{0}; + Eigen::Affine3f affine_past2world; +}; + +class PointCloudDensification +{ +public: + explicit PointCloudDensification(const DensificationParam & param, cudaStream_t & stream); + + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + + double getCurrentTimestamp() const { return current_timestamp_; } + Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } + std::list::iterator getPointCloudCacheIter() + { + return pointcloud_cache_.begin(); + } + bool isCacheEnd(std::list::iterator iter) + { + return iter == pointcloud_cache_.end(); + } + size_t getIdx(std::list::iterator iter) + { + return std::distance(pointcloud_cache_.begin(), iter); + } + size_t getCacheSize() + { + return std::distance(pointcloud_cache_.begin(), pointcloud_cache_.end()); + } + unsigned int pointcloud_cache_size() const { return param_.pointcloud_cache_size(); } + +private: + void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine); + void dequeue(); + + DensificationParam param_; + double current_timestamp_{0.0}; + Eigen::Affine3f affine_world2current_; + std::list pointcloud_cache_; + cudaStream_t stream_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp new file mode 100644 index 0000000000000..c571990d84b51 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +namespace lidar_transfusion +{ + +class PreprocessCuda +{ +public: + PreprocessCuda(const TransfusionConfig & config, cudaStream_t & stream); + + void generateVoxels( + float * points, unsigned int points_size, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs); + + cudaError_t generateVoxels_random_launch( + float * points, unsigned int points_size, unsigned int * mask, float * voxels); + + cudaError_t generateBaseFeatures_launch( + unsigned int * mask, float * voxels, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs); + + // cudaError_t generateVoxelsInput_launch( + // uint8_t * cloud_data, CloudInfo & cloud_info, unsigned int points_agg, unsigned int + // points_size, float time_lag, float * affine_transform, float * points); + + cudaError_t generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform, float * output_points); + +private: + TransfusionConfig config_; + cudaStream_t stream_; + cuda::unique_ptr mask_{nullptr}; + cuda::unique_ptr voxels_{nullptr}; + unsigned int mask_size_; + unsigned int voxels_size_; +}; +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp new file mode 100644 index 0000000000000..3e3660e238473 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp @@ -0,0 +1,70 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include +#include +#include + +namespace lidar_transfusion +{ +constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix +constexpr size_t MAX_CLOUD_STEP_SIZE = 35; // PointXYZIRCADT + +class VoxelGenerator +{ +public: + explicit VoxelGenerator( + const DensificationParam & densification_param, const TransfusionConfig & config, + cudaStream_t & stream); + std::size_t generateSweepPoints( + const sensor_msgs::msg::PointCloud2 & msg, cuda::unique_ptr & points_d); + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + void initCloudInfo(const sensor_msgs::msg::PointCloud2 & msg); + std::tuple getFieldInfo( + const sensor_msgs::msg::PointCloud2 & msg, const std::string & field_name); + +private: + std::unique_ptr pd_ptr_{nullptr}; + std::unique_ptr pre_ptr_{nullptr}; + TransfusionConfig config_; + cuda::unique_ptr cloud_data_d_{nullptr}; + cuda::unique_ptr affine_past2current_d_{nullptr}; + std::vector points_; + cudaStream_t stream_; + CloudInfo cloud_info_; + bool is_initialized_{false}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp new file mode 100644 index 0000000000000..f013a02404a29 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp @@ -0,0 +1,39 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#define LIDAR_TRANSFUSION__ROS_UTILS_HPP_ + +#include "lidar_transfusion/utils.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace lidar_transfusion +{ + +void box3DToDetectedObject( + const Box3D & box3d, const std::vector & class_names, + autoware_perception_msgs::msg::DetectedObject & obj); + +uint8_t getSemanticType(const std::string & class_name); + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__ROS_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp new file mode 100644 index 0000000000000..31976de56a9da --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp @@ -0,0 +1,161 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#define LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ + +#include +#include + +namespace lidar_transfusion +{ + +class TransfusionConfig +{ +public: + TransfusionConfig( + const std::vector & voxels_num, const std::vector & point_cloud_range, + const std::vector & voxel_size, const float circle_nms_dist_threshold, + const std::vector & yaw_norm_thresholds, const float score_threshold) + { + if (voxels_num.size() == 3) { + max_voxels_ = voxels_num[2]; + + voxels_num_[0] = voxels_num[0]; + voxels_num_[1] = voxels_num[1]; + voxels_num_[2] = voxels_num[2]; + + min_voxel_size_ = voxels_num[0]; + opt_voxel_size_ = voxels_num[1]; + max_voxel_size_ = voxels_num[2]; + + min_points_size_ = voxels_num[0]; + opt_points_size_ = voxels_num[1]; + max_points_size_ = voxels_num[2]; + + min_coors_size_ = voxels_num[0]; + opt_coors_size_ = voxels_num[1]; + max_coors_size_ = voxels_num[2]; + } + if (point_cloud_range.size() == 6) { + min_x_range_ = static_cast(point_cloud_range[0]); + min_y_range_ = static_cast(point_cloud_range[1]); + min_z_range_ = static_cast(point_cloud_range[2]); + max_x_range_ = static_cast(point_cloud_range[3]); + max_y_range_ = static_cast(point_cloud_range[4]); + max_z_range_ = static_cast(point_cloud_range[5]); + } + if (voxel_size.size() == 3) { + voxel_x_size_ = static_cast(voxel_size[0]); + voxel_y_size_ = static_cast(voxel_size[1]); + voxel_z_size_ = static_cast(voxel_size[2]); + } + if (score_threshold > 0.0) { + score_threshold_ = score_threshold; + } + if (circle_nms_dist_threshold > 0.0) { + circle_nms_dist_threshold_ = circle_nms_dist_threshold; + } + yaw_norm_thresholds_ = + std::vector(yaw_norm_thresholds.begin(), yaw_norm_thresholds.end()); + for (auto & yaw_norm_threshold : yaw_norm_thresholds_) { + yaw_norm_threshold = + (yaw_norm_threshold >= 0.0 && yaw_norm_threshold < 1.0) ? yaw_norm_threshold : 0.0; + } + grid_x_size_ = static_cast((max_x_range_ - min_x_range_) / voxel_x_size_); + grid_y_size_ = static_cast((max_y_range_ - min_y_range_) / voxel_y_size_); + grid_z_size_ = static_cast((max_z_range_ - min_z_range_) / voxel_z_size_); + } + + ///// INPUT PARAMETERS ///// + const std::size_t cloud_capacity_{1000000}; + ///// KERNEL PARAMETERS ///// + const std::size_t threads_for_voxel_{256}; // threads number for a block + const std::size_t points_per_voxel_{20}; + const std::size_t warp_size_{32}; // one warp(32 threads) for one pillar + const std::size_t pillars_per_block_{64}; // one thread deals with one pillar + // and a block has pillars_per_block threads + const std::size_t pillar_feature_size_{64}; + std::size_t max_voxels_{60000}; + + ///// NETWORK PARAMETERS ///// + const std::size_t batch_size_{1}; + const std::size_t num_classes_{5}; + const std::size_t num_point_feature_size_{5}; // x, y, z, intensity, lag + // the dimension of the input cloud + float min_x_range_{-76.8}; + float max_x_range_{76.8}; + float min_y_range_{-76.8}; + float max_y_range_{76.8}; + float min_z_range_{-3.0}; + float max_z_range_{5.0}; + // the size of a pillar + float voxel_x_size_{0.3}; + float voxel_y_size_{0.3}; + float voxel_z_size_{8.0}; + const std::size_t out_size_factor_{4}; + const std::size_t max_num_points_per_pillar_{points_per_voxel_}; + const std::size_t num_point_values_{4}; + const std::size_t num_proposals_{200}; + // the number of feature maps for pillar scatter + const std::size_t num_feature_scatter_{pillar_feature_size_}; + // the score threshold for classification + float score_threshold_{0.2}; + float circle_nms_dist_threshold_{0.5}; + std::vector yaw_norm_thresholds_{0.3, 0.3, 0.3, 0.3, 0.0}; + std::size_t max_num_pillars_{max_voxels_}; + const std::size_t pillar_points_bev_{max_num_points_per_pillar_ * max_num_pillars_}; + // the detected boxes result decode by (x, y, z, w, l, h, yaw) + const std::size_t num_box_values_{8}; + // the input size of the 2D backbone network + std::size_t grid_x_size_{512}; + std::size_t grid_y_size_{512}; + std::size_t grid_z_size_{1}; + // the output size of the 2D backbone network + std::size_t feature_x_size_{grid_x_size_ / out_size_factor_}; + std::size_t feature_y_size_{grid_y_size_ / out_size_factor_}; + + ///// RUNTIME DIMENSIONS ///// + std::vector voxels_num_{5000, 30000, 60000}; + // voxels + std::size_t min_voxel_size_{voxels_num_[0]}; + std::size_t opt_voxel_size_{voxels_num_[1]}; + std::size_t max_voxel_size_{voxels_num_[2]}; + + std::size_t min_point_in_voxel_size_{points_per_voxel_}; + std::size_t opt_point_in_voxel_size_{points_per_voxel_}; + std::size_t max_point_in_voxel_size_{points_per_voxel_}; + + std::size_t min_network_feature_size_{num_point_feature_size_}; + std::size_t opt_network_feature_size_{num_point_feature_size_}; + std::size_t max_network_feature_size_{num_point_feature_size_}; + + // num_points + std::size_t min_points_size_{voxels_num_[0]}; + std::size_t opt_points_size_{voxels_num_[1]}; + std::size_t max_points_size_{voxels_num_[2]}; + + // coors + std::size_t min_coors_size_{voxels_num_[0]}; + std::size_t opt_coors_size_{voxels_num_[1]}; + std::size_t max_coors_size_{voxels_num_[2]}; + + std::size_t min_coors_dim_size_{num_point_values_}; + std::size_t opt_coors_dim_size_{num_point_values_}; + std::size_t max_coors_dim_size_{num_point_values_}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp new file mode 100644 index 0000000000000..a94c3c1871136 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp @@ -0,0 +1,114 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ +#define LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/network/network_trt.hpp" +#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/preprocess/voxel_generator.hpp" +#include "lidar_transfusion/utils.hpp" +#include "lidar_transfusion/visibility_control.hpp" + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +class NetworkParam +{ +public: + NetworkParam(std::string onnx_path, std::string engine_path, std::string trt_precision) + : onnx_path_(std::move(onnx_path)), + engine_path_(std::move(engine_path)), + trt_precision_(std::move(trt_precision)) + { + } + + std::string onnx_path() const { return onnx_path_; } + std::string engine_path() const { return engine_path_; } + std::string trt_precision() const { return trt_precision_; } + +private: + std::string onnx_path_; + std::string engine_path_; + std::string trt_precision_; +}; + +class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT +{ +public: + explicit TransfusionTRT( + const NetworkParam & network_param, const DensificationParam & densification_param, + const TransfusionConfig & config); + virtual ~TransfusionTRT(); + + bool detect( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, + std::vector & det_boxes3d, std::unordered_map & proc_timing); + +protected: + void initPtr(); + + bool preprocess(const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + + bool inference(); + + bool postprocess(std::vector & det_boxes3d); + + std::unique_ptr network_trt_ptr_{nullptr}; + std::unique_ptr vg_ptr_{nullptr}; + std::unique_ptr> stop_watch_ptr_{ + nullptr}; + std::unique_ptr pre_ptr_{nullptr}; + std::unique_ptr post_ptr_{nullptr}; + cudaStream_t stream_{nullptr}; + + TransfusionConfig config_; + + // input of pre-process + + unsigned int voxel_features_size_{0}; + unsigned int voxel_num_size_{0}; + unsigned int voxel_idxs_size_{0}; + unsigned int cls_size_{0}; + unsigned int box_size_{0}; + unsigned int dir_cls_size_{0}; + cuda::unique_ptr points_d_{nullptr}; + cuda::unique_ptr params_input_d_{nullptr}; + cuda::unique_ptr voxel_features_d_{nullptr}; + cuda::unique_ptr voxel_num_d_{nullptr}; + cuda::unique_ptr voxel_idxs_d_{nullptr}; + cuda::unique_ptr cls_output_d_{nullptr}; + cuda::unique_ptr box_output_d_{nullptr}; + cuda::unique_ptr dir_cls_output_d_{nullptr}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp new file mode 100644 index 0000000000000..e73fe7f055953 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp @@ -0,0 +1,103 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__UTILS_HPP_ +#define LIDAR_TRANSFUSION__UTILS_HPP_ + +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +struct Box3D +{ + int label; + float score; + float x; + float y; + float z; + float width; + float length; + float height; + float yaw; +}; + +struct CloudInfo +{ + uint32_t x_offset; + uint32_t y_offset; + uint32_t z_offset; + uint32_t intensity_offset; + uint8_t x_datatype; + uint8_t y_datatype; + uint8_t z_datatype; + uint8_t intensity_datatype; + uint8_t x_count; + uint8_t y_count; + uint8_t z_count; + uint8_t intensity_count; + uint32_t point_step; + bool is_bigendian; + + CloudInfo() + : x_offset(0), + y_offset(4), + z_offset(8), + intensity_offset(12), + x_datatype(7), + y_datatype(7), + z_datatype(7), + intensity_datatype(7), + x_count(1), + y_count(1), + z_count(1), + intensity_count(1), + point_step(16), + is_bigendian(false) + { + } + + bool operator!=(const CloudInfo & rhs) const + { + return x_offset != rhs.x_offset || y_offset != rhs.y_offset || z_offset != rhs.z_offset || + intensity_offset != rhs.intensity_offset || x_datatype != rhs.x_datatype || + y_datatype != rhs.y_datatype || z_datatype != rhs.z_datatype || + intensity_datatype != rhs.intensity_datatype || x_count != rhs.x_count || + y_count != rhs.y_count || z_count != rhs.z_count || + intensity_count != rhs.intensity_count || is_bigendian != rhs.is_bigendian; + } +}; + +enum NetworkIO { voxels = 0, num_points, coors, cls_score, dir_pred, bbox_pred, ENUM_SIZE }; + +// cspell: ignore divup +template +unsigned int divup(const T1 a, const T2 b) +{ + if (a == 0) { + throw std::runtime_error("A dividend of divup isn't positive."); + } + if (b == 0) { + throw std::runtime_error("A divisor of divup isn't positive."); + } + + return (a + b - 1) / b; +} + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp b/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp new file mode 100644 index 0000000000000..aeab20906628a --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#define LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) +#if defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#define LIDAR_TRANSFUSION_PUBLIC __declspec(dllexport) +#define LIDAR_TRANSFUSION_LOCAL +#else // defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#define LIDAR_TRANSFUSION_PUBLIC __declspec(dllimport) +#define LIDAR_TRANSFUSION_LOCAL +#endif // defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#elif defined(__linux__) +#define LIDAR_TRANSFUSION_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_TRANSFUSION_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define LIDAR_TRANSFUSION_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_TRANSFUSION_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#endif // LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml b/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml new file mode 100644 index 0000000000000..492eb8c4d59b7 --- /dev/null +++ b/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_transfusion/lib/detection_class_remapper.cpp b/perception/lidar_transfusion/lib/detection_class_remapper.cpp new file mode 100644 index 0000000000000..e093637402448 --- /dev/null +++ b/perception/lidar_transfusion/lib/detection_class_remapper.cpp @@ -0,0 +1,71 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace lidar_transfusion +{ + +void DetectionClassRemapper::setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix) +{ + assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); + assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); + assert( + std::pow(static_cast(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size()); + + num_labels_ = static_cast(std::sqrt(min_area_matrix.size())); + + Eigen::Map> + allow_remapping_by_area_matrix_tmp( + allow_remapping_by_area_matrix.data(), num_labels_, num_labels_); + allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose() + .cast(); // Eigen is column major by default + + Eigen::Map min_area_matrix_tmp( + min_area_matrix.data(), num_labels_, num_labels_); + min_area_matrix_ = min_area_matrix_tmp.transpose(); // Eigen is column major by default + + Eigen::Map max_area_matrix_tmp( + max_area_matrix.data(), num_labels_, num_labels_); + max_area_matrix_ = max_area_matrix_tmp.transpose(); // Eigen is column major by default + + min_area_matrix_ = min_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); + max_area_matrix_ = max_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); +} + +void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg) +{ + for (auto & object : msg.objects) { + const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y; + + for (auto & classification : object.classification) { + auto & label = classification.label; + + for (int i = 0; i < num_labels_; ++i) { + if ( + allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) && + bev_area <= max_area_matrix_(label, i)) { + label = i; + break; + } + } + } + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/network/network_trt.cpp b/perception/lidar_transfusion/lib/network/network_trt.cpp new file mode 100644 index 0000000000000..a36890facceec --- /dev/null +++ b/perception/lidar_transfusion/lib/network/network_trt.cpp @@ -0,0 +1,332 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/network/network_trt.hpp" + +#include + +#include +#include +#include + +namespace lidar_transfusion +{ + +std::ostream & operator<<(std::ostream & os, const ProfileDimension & profile) +{ + std::string delim = ""; + os << "min->["; + for (int i = 0; i < profile.min.nbDims; ++i) { + os << delim << profile.min.d[i]; + delim = ", "; + } + os << "], opt->["; + delim = ""; + for (int i = 0; i < profile.opt.nbDims; ++i) { + os << delim << profile.opt.d[i]; + delim = ", "; + } + os << "], max->["; + delim = ""; + for (int i = 0; i < profile.max.nbDims; ++i) { + os << delim << profile.max.d[i]; + delim = ", "; + } + os << "]"; + return os; +} + +NetworkTRT::NetworkTRT(const TransfusionConfig & config) : config_(config) +{ + ProfileDimension voxels_dims = { + nvinfer1::Dims3( + config_.min_voxel_size_, config_.min_point_in_voxel_size_, config_.min_network_feature_size_), + nvinfer1::Dims3( + config_.opt_voxel_size_, config_.opt_point_in_voxel_size_, config_.opt_network_feature_size_), + nvinfer1::Dims3( + config_.max_voxel_size_, config_.max_point_in_voxel_size_, + config_.max_network_feature_size_)}; + ProfileDimension num_points_dims = { + nvinfer1::Dims{1, {static_cast(config_.min_points_size_)}}, + nvinfer1::Dims{1, {static_cast(config_.opt_points_size_)}}, + nvinfer1::Dims{1, {static_cast(config_.max_points_size_)}}}; + ProfileDimension coors_dims = { + nvinfer1::Dims2(config_.min_coors_size_, config_.min_coors_dim_size_), + nvinfer1::Dims2(config_.opt_coors_size_, config_.opt_coors_dim_size_), + nvinfer1::Dims2(config_.max_coors_size_, config_.max_coors_dim_size_)}; + in_profile_dims_ = {voxels_dims, num_points_dims, coors_dims}; +} + +NetworkTRT::~NetworkTRT() +{ + context.reset(); + runtime_.reset(); + plan_.reset(); + engine.reset(); +} + +bool NetworkTRT::init( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision) +{ + runtime_ = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); + if (!runtime_) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; + return false; + } + + bool success; + std::ifstream engine_file(engine_path); + if (engine_file.is_open()) { + success = loadEngine(engine_path); + } else { + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); + success = parseONNX(onnx_path, engine_path, precision); + logger_.stop_throttle(log_thread); + } + success &= createContext(); + + return success; +} + +bool NetworkTRT::setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config) +{ + auto profile = builder.createOptimizationProfile(); + + auto voxels_name = network.getInput(NetworkIO::voxels)->getName(); + auto num_points_name = network.getInput(NetworkIO::num_points)->getName(); + auto coors_name = network.getInput(NetworkIO::coors)->getName(); + + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::voxels].min); + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::voxels].opt); + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::voxels].max); + + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kMIN, + in_profile_dims_[NetworkIO::num_points].min); + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kOPT, + in_profile_dims_[NetworkIO::num_points].opt); + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kMAX, + in_profile_dims_[NetworkIO::num_points].max); + + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::coors].min); + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::coors].opt); + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::coors].max); + + config.addOptimizationProfile(profile); + return true; +} + +bool NetworkTRT::createContext() +{ + if (!engine) { + tensorrt_common::LOG_ERROR(logger_) + << "Failed to create context: Engine was not created" << std::endl; + return false; + } + + context = + tensorrt_common::TrtUniquePtr(engine->createExecutionContext()); + if (!context) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; + return false; + } + + return true; +} + +bool NetworkTRT::parseONNX( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision, + const size_t workspace_size) +{ + auto builder = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); + if (!builder) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; + return false; + } + + auto config = + tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); + if (!config) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; + return false; + } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); +#else + config->setMaxWorkspaceSize(workspace_size); +#endif + if (precision == "fp16") { + if (builder->platformHasFastFp16()) { + tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; + config->setFlag(nvinfer1::BuilderFlag::kFP16); + } else { + tensorrt_common::LOG_INFO(logger_) + << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; + } + } + + const auto flag = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + auto network = + tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); + if (!network) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; + return false; + } + + auto parser = tensorrt_common::TrtUniquePtr( + nvonnxparser::createParser(*network, logger_)); + parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); + + if (!setProfile(*builder, *network, *config)) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; + return false; + } + + plan_ = tensorrt_common::TrtUniquePtr( + builder->buildSerializedNetwork(*network, *config)); + if (!plan_) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create serialized network" << std::endl; + return false; + } + engine = tensorrt_common::TrtUniquePtr( + runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); + if (!engine) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; + return false; + } + + return saveEngine(engine_path); +} + +bool NetworkTRT::saveEngine(const std::string & engine_path) +{ + tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; + std::ofstream file(engine_path, std::ios::out | std::ios::binary); + file.write(reinterpret_cast(plan_->data()), plan_->size()); + return validateNetworkIO(); +} + +bool NetworkTRT::loadEngine(const std::string & engine_path) +{ + std::ifstream engine_file(engine_path); + std::stringstream engine_buffer; + engine_buffer << engine_file.rdbuf(); + std::string engine_str = engine_buffer.str(); + engine = tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( + reinterpret_cast(engine_str.data()), engine_str.size())); + tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; + return validateNetworkIO(); +} + +bool NetworkTRT::validateNetworkIO() +{ + // Whether the number of IO match the expected size + if (engine->getNbIOTensors() != NetworkIO::ENUM_SIZE) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid network IO. Expected size: " << NetworkIO::ENUM_SIZE + << ". Actual size: " << engine->getNbIOTensors() << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + for (int i = 0; i < NetworkIO::ENUM_SIZE; ++i) { + tensors_names_.push_back(engine->getIOTensorName(i)); + } + + // Log the network IO + std::string tensors = std::accumulate( + tensors_names_.begin(), tensors_names_.end(), std::string(), + [](const std::string & a, const std::string & b) -> std::string { return a + b + " "; }); + tensorrt_common::LOG_INFO(logger_) << "Network IO: " << tensors << std::endl; + + // Whether the current engine input profile match the config input profile + for (int i = 0; i <= NetworkIO::coors; ++i) { + ProfileDimension engine_dims{ + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMIN), + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kOPT), + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMAX)}; + + tensorrt_common::LOG_INFO(logger_) + << "Profile for " << tensors_names_[i] << ": " << engine_dims << std::endl; + + if (engine_dims != in_profile_dims_[i]) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid network input dimension. Config: " << in_profile_dims_[i] + << ". Please change the input profile or delete the engine file and build engine again." + << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + } + + // Whether the IO tensor shapes match the network config, -1 for dynamic size + validateTensorShape( + NetworkIO::voxels, {-1, static_cast(config_.points_per_voxel_), + static_cast(config_.num_point_feature_size_)}); + validateTensorShape(NetworkIO::num_points, {-1}); + validateTensorShape(NetworkIO::coors, {-1, static_cast(config_.num_point_values_)}); + auto cls_score = validateTensorShape( + NetworkIO::cls_score, + {static_cast(config_.batch_size_), static_cast(config_.num_classes_), + static_cast(config_.num_proposals_)}); + tensorrt_common::LOG_INFO(logger_) << "Network num classes: " << cls_score.d[1] << std::endl; + validateTensorShape( + NetworkIO::dir_pred, + {static_cast(config_.batch_size_), 2, static_cast(config_.num_proposals_)}); // x, y + validateTensorShape( + NetworkIO::bbox_pred, + {static_cast(config_.batch_size_), static_cast(config_.num_box_values_), + static_cast(config_.num_proposals_)}); + + return true; +} + +const char * NetworkTRT::getTensorName(NetworkIO name) +{ + return tensors_names_.at(name); +} + +nvinfer1::Dims NetworkTRT::validateTensorShape(NetworkIO name, const std::vector shape) +{ + auto tensor_shape = engine->getTensorShape(tensors_names_[name]); + if (tensor_shape.nbDims != static_cast(shape.size())) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid tensor shape for " << tensors_names_[name] << ". Expected size: " << shape.size() + << ". Actual size: " << tensor_shape.nbDims << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + for (int i = 0; i < tensor_shape.nbDims; ++i) { + if (tensor_shape.d[i] != static_cast(shape[i])) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid tensor shape for " << tensors_names_[name] << ". Expected: " << shape[i] + << ". Actual: " << tensor_shape.d[i] << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + } + return tensor_shape; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu b/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu new file mode 100644 index 0000000000000..dcb60831825bb --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu @@ -0,0 +1,144 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Modified from +// https://github.com/open-mmlab/OpenPCDet/blob/master/pcdet/ops/iou3d_nms/src/iou3d_nms_kernel.cu + +/* +3D IoU Calculation and Rotated NMS(modified from 2D NMS written by others) +Written by Shaoshuai Shi +All Rights Reserved 2019-2020. +*/ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +namespace +{ +const std::size_t THREADS_PER_BLOCK_NMS = 16; +} // namespace + +namespace lidar_transfusion +{ + +__device__ inline float dist2dPow(const Box3D * a, const Box3D * b) +{ + return powf(a->x - b->x, 2) + powf(a->y - b->y, 2); +} + +// cspell: ignore divup +__global__ void circleNMS_Kernel( + const Box3D * boxes, const std::size_t num_boxes3d, const std::size_t col_blocks, + const float dist2d_pow_threshold, std::uint64_t * mask) +{ + // params: boxes (N,) + // params: mask (N, divup(N/THREADS_PER_BLOCK_NMS)) + + const auto row_start = blockIdx.y; + const auto col_start = blockIdx.x; + + if (row_start > col_start) return; + + const std::size_t row_size = + fminf(num_boxes3d - row_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS); + const std::size_t col_size = + fminf(num_boxes3d - col_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS); + + __shared__ Box3D block_boxes[THREADS_PER_BLOCK_NMS]; + + if (threadIdx.x < col_size) { + block_boxes[threadIdx.x] = boxes[THREADS_PER_BLOCK_NMS * col_start + threadIdx.x]; + } + __syncthreads(); + + if (threadIdx.x < row_size) { + const std::size_t cur_box_idx = THREADS_PER_BLOCK_NMS * row_start + threadIdx.x; + const Box3D * cur_box = boxes + cur_box_idx; + + std::uint64_t t = 0; + std::size_t start = 0; + if (row_start == col_start) { + start = threadIdx.x + 1; + } + for (std::size_t i = start; i < col_size; i++) { + if (dist2dPow(cur_box, block_boxes + i) < dist2d_pow_threshold) { + t |= 1ULL << i; + } + } + mask[cur_box_idx * col_blocks + col_start] = t; + } +} + +cudaError_t circleNMS_launch( + const thrust::device_vector & boxes3d, const std::size_t num_boxes3d, + std::size_t col_blocks, const float distance_threshold, + thrust::device_vector & mask, cudaStream_t stream) +{ + const float dist2d_pow_thres = powf(distance_threshold, 2); + + dim3 blocks(col_blocks, col_blocks); + dim3 threads(THREADS_PER_BLOCK_NMS); + circleNMS_Kernel<<>>( + thrust::raw_pointer_cast(boxes3d.data()), num_boxes3d, col_blocks, dist2d_pow_thres, + thrust::raw_pointer_cast(mask.data())); + + return cudaGetLastError(); +} + +std::size_t circleNMS( + thrust::device_vector & boxes3d, const float distance_threshold, + thrust::device_vector & keep_mask, cudaStream_t stream) +{ + const auto num_boxes3d = boxes3d.size(); + const auto col_blocks = divup(num_boxes3d, THREADS_PER_BLOCK_NMS); + thrust::device_vector mask_d(num_boxes3d * col_blocks); + + CHECK_CUDA_ERROR( + circleNMS_launch(boxes3d, num_boxes3d, col_blocks, distance_threshold, mask_d, stream)); + + // memcpy device to host + thrust::host_vector mask_h(mask_d.size()); + thrust::copy(mask_d.begin(), mask_d.end(), mask_h.begin()); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream)); + + // generate keep_mask + std::vector remv_h(col_blocks); + thrust::host_vector keep_mask_h(keep_mask.size()); + std::size_t num_to_keep = 0; + for (std::size_t i = 0; i < num_boxes3d; i++) { + auto nblock = i / THREADS_PER_BLOCK_NMS; + auto inblock = i % THREADS_PER_BLOCK_NMS; + + if (!(remv_h[nblock] & (1ULL << inblock))) { + keep_mask_h[i] = true; + num_to_keep++; + std::uint64_t * p = &mask_h[0] + i * col_blocks; + for (std::size_t j = nblock; j < col_blocks; j++) { + remv_h[j] |= p[j]; + } + } else { + keep_mask_h[i] = false; + } + } + + // memcpy host to device + keep_mask = keep_mask_h; + + return num_to_keep; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp new file mode 100644 index 0000000000000..c1179a3f6673d --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -0,0 +1,104 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ + +void NonMaximumSuppression::setParameters(const NMSParams & params) +{ + assert(params.search_distance_2d_ >= 0.0); + assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0); + + params_ = params; + target_class_mask_ = classNamesToBooleanMask(params.target_class_names_); +} + +bool NonMaximumSuppression::isTargetLabel(const uint8_t label) +{ + if (label >= target_class_mask_.size()) { + return false; + } + return target_class_mask_.at(label); +} + +bool NonMaximumSuppression::isTargetPairObject( + const DetectedObject & object1, const DetectedObject & object2) +{ + const auto label1 = object_recognition_utils::getHighestProbLabel(object1.classification); + const auto label2 = object_recognition_utils::getHighestProbLabel(object2.classification); + + if (isTargetLabel(label1) && isTargetLabel(label2)) { + return true; + } + + const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; + const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); + return sqr_dist_2d <= search_sqr_dist_2d; +} + +Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( + const std::vector & input_objects) +{ + // NOTE: row = target objects to be suppressed, col = source objects to be compared + Eigen::MatrixXd triangular_matrix = + Eigen::MatrixXd::Zero(input_objects.size(), input_objects.size()); + for (std::size_t target_i = 0; target_i < input_objects.size(); ++target_i) { + for (std::size_t source_i = 0; source_i < target_i; ++source_i) { + const auto & target_obj = input_objects.at(target_i); + const auto & source_obj = input_objects.at(source_i); + if (!isTargetPairObject(target_obj, source_obj)) { + continue; + } + + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + const double iou = object_recognition_utils::get2dIoU(target_obj, source_obj); + triangular_matrix(target_i, source_i) = iou; + // NOTE: If the target object has any objects with iou > iou_threshold, it + // will be suppressed regardless of later results. + if (iou > params_.iou_threshold_) { + break; + } + } + } + } + + return triangular_matrix; +} + +std::vector NonMaximumSuppression::apply( + const std::vector & input_objects) +{ + Eigen::MatrixXd iou_matrix = generateIoUMatrix(input_objects); + + std::vector output_objects; + output_objects.reserve(input_objects.size()); + for (std::size_t i = 0; i < input_objects.size(); ++i) { + const auto value = iou_matrix.row(i).maxCoeff(); + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + if (value <= params_.iou_threshold_) { + output_objects.emplace_back(input_objects.at(i)); + } + } + } + + return output_objects; +} +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu b/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu new file mode 100644 index 0000000000000..bca23e9961b40 --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu @@ -0,0 +1,145 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ +const size_t THREADS_PER_BLOCK = 256; + +struct is_score_greater +{ + is_score_greater(float t) : t_(t) {} + + __device__ bool operator()(const Box3D & b) { return b.score > t_; } + +private: + float t_{0.0}; +}; + +struct is_kept +{ + __device__ bool operator()(const bool keep) { return keep; } +}; + +struct score_greater +{ + __device__ bool operator()(const Box3D & lb, const Box3D & rb) { return lb.score > rb.score; } +}; + +__device__ inline float sigmoid(float x) +{ + return 1.0f / (1.0f + expf(-x)); +} + +__global__ void generateBoxes3D_kernel( + const float * __restrict__ cls_output, const float * __restrict__ box_output, + const float * __restrict__ dir_cls_output, const float voxel_size_x, const float voxel_size_y, + const float min_x_range, const float min_y_range, const int num_proposals, const int num_classes, + const int num_point_values, const float * __restrict__ yaw_norm_thresholds, + Box3D * __restrict__ det_boxes3d) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= num_proposals) { + return; + } + + int class_id = 0; + float max_score = cls_output[point_idx]; + +#pragma unroll + for (int i = 1; i < num_classes; i++) { + float score = cls_output[i * num_proposals + point_idx]; + if (score > max_score) { + max_score = score; + class_id = i; + } + } + + // yaw validation + const float yaw_sin = dir_cls_output[point_idx]; + const float yaw_cos = dir_cls_output[point_idx + num_proposals]; + const float yaw_norm = sqrtf(yaw_sin * yaw_sin + yaw_cos * yaw_cos); + + det_boxes3d[point_idx].label = class_id; + det_boxes3d[point_idx].score = yaw_norm >= yaw_norm_thresholds[class_id] ? max_score : 0.f; + det_boxes3d[point_idx].x = box_output[point_idx] * num_point_values * voxel_size_x + min_x_range; + det_boxes3d[point_idx].y = + box_output[point_idx + num_proposals] * num_point_values * voxel_size_y + min_y_range; + det_boxes3d[point_idx].z = box_output[point_idx + 2 * num_proposals]; + det_boxes3d[point_idx].width = expf(box_output[point_idx + 3 * num_proposals]); + det_boxes3d[point_idx].length = expf(box_output[point_idx + 4 * num_proposals]); + det_boxes3d[point_idx].height = expf(box_output[point_idx + 5 * num_proposals]); + det_boxes3d[point_idx].yaw = + atan2f(dir_cls_output[point_idx], dir_cls_output[point_idx + num_proposals]); +} + +PostprocessCuda::PostprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) +: config_(config), stream_(stream) +{ + boxes3d_d_ = thrust::device_vector(config_.num_proposals_); + yaw_norm_thresholds_d_ = thrust::device_vector( + config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); +} + +// cspell: ignore divup +cudaError_t PostprocessCuda::generateDetectedBoxes3D_launch( + const float * cls_output, const float * box_output, const float * dir_cls_output, + std::vector & det_boxes3d, cudaStream_t stream) +{ + dim3 threads = {THREADS_PER_BLOCK}; + dim3 blocks = {divup(config_.num_proposals_, threads.x)}; + + generateBoxes3D_kernel<<>>( + cls_output, box_output, dir_cls_output, config_.voxel_x_size_, config_.voxel_y_size_, + config_.min_x_range_, config_.min_y_range_, config_.num_proposals_, config_.num_classes_, + config_.num_point_values_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), + thrust::raw_pointer_cast(boxes3d_d_.data())); + + // suppress by score + const auto num_det_boxes3d = thrust::count_if( + thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), + is_score_greater(config_.score_threshold_)); + if (num_det_boxes3d == 0) { + return cudaGetLastError(); + } + thrust::device_vector det_boxes3d_d(num_det_boxes3d); + thrust::copy_if( + thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), det_boxes3d_d.begin(), + is_score_greater(config_.score_threshold_)); + + // sort by score + thrust::sort(det_boxes3d_d.begin(), det_boxes3d_d.end(), score_greater()); + + // supress by NMS + thrust::device_vector final_keep_mask_d(num_det_boxes3d); + const auto num_final_det_boxes3d = + circleNMS(det_boxes3d_d, config_.circle_nms_dist_threshold_, final_keep_mask_d, stream); + thrust::device_vector final_det_boxes3d_d(num_final_det_boxes3d); + thrust::copy_if( + thrust::device, det_boxes3d_d.begin(), det_boxes3d_d.end(), final_keep_mask_d.begin(), + final_det_boxes3d_d.begin(), is_kept()); + + // memcpy device to host + det_boxes3d.resize(num_final_det_boxes3d); + thrust::copy(final_det_boxes3d_d.begin(), final_det_boxes3d_d.end(), det_boxes3d.begin()); + return cudaGetLastError(); +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp new file mode 100644 index 0000000000000..774b3a05d71c1 --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp @@ -0,0 +1,117 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include + +namespace +{ + +boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, + const std::string & source_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_transfusion"), ex.what()); + return boost::none; + } +} + +Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3f a; + a.matrix() = tf2::transformToEigen(t).matrix().cast(); + return a; +} + +} // namespace + +namespace lidar_transfusion +{ + +PointCloudDensification::PointCloudDensification( + const DensificationParam & param, cudaStream_t & stream) +: param_(param), stream_(stream) +{ +} + +bool PointCloudDensification::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer) +{ + const auto header = pointcloud_msg.header; + + if (param_.pointcloud_cache_size() > 1) { + auto transform_world2current = + getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); + if (!transform_world2current) { + return false; + } + auto affine_world2current = transformToEigen(transform_world2current.get()); + + enqueue(pointcloud_msg, affine_world2current); + } else { + enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); + } + + dequeue(); + + return true; +} + +void PointCloudDensification::enqueue( + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current) +{ + affine_world2current_ = affine_world2current; + current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); + + assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 1); + auto points_d = cuda::make_unique( + sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float)); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, + cudaMemcpyHostToDevice, stream_)); + + PointCloudWithTransform pointcloud = { + std::move(points_d), msg.header, msg.width * msg.height, affine_world2current.inverse()}; + + pointcloud_cache_.push_front(std::move(pointcloud)); +} + +void PointCloudDensification::dequeue() +{ + if (pointcloud_cache_.size() > param_.pointcloud_cache_size()) { + pointcloud_cache_.pop_back(); + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu new file mode 100644 index 0000000000000..b8f9ae998fd4f --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -0,0 +1,208 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" + +namespace lidar_transfusion +{ + +PreprocessCuda::PreprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) +: stream_(stream), config_(config) +{ + mask_size_ = config_.grid_z_size_ * config_.grid_y_size_ * config_.grid_x_size_; + voxels_size_ = config_.grid_z_size_ * config_.grid_y_size_ * config_.grid_x_size_ * + config_.max_num_points_per_pillar_ * config_.num_point_feature_size_ + + 1; + mask_ = cuda::make_unique(mask_size_); + voxels_ = cuda::make_unique(voxels_size_); +} + +void PreprocessCuda::generateVoxels( + float * points, unsigned int points_size, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs) +{ + cuda::clear_async(mask_.get(), mask_size_, stream_); + cuda::clear_async(voxels_.get(), voxels_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(generateVoxels_random_launch(points, points_size, mask_.get(), voxels_.get())); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(generateBaseFeatures_launch( + mask_.get(), voxels_.get(), pillar_num, voxel_features, voxel_num, voxel_idxs)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); +} + +__global__ void generateVoxels_random_kernel( + float * points, unsigned int points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, int points_per_voxel, unsigned int * mask, + float * voxels) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + float x = points[point_idx * 5]; + float y = points[point_idx * 5 + 1]; + float z = points[point_idx * 5 + 2]; + float i = points[point_idx * 5 + 3]; + float t = points[point_idx * 5 + 4]; + + if ( + x <= min_x_range || x >= max_x_range || y <= min_y_range || y >= max_y_range || + z <= min_z_range || z >= max_z_range) + return; + + int voxel_idx = floorf((x - min_x_range) / pillar_x_size); + int voxel_idy = floorf((y - min_y_range) / pillar_y_size); + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + + unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); + + if (point_id >= points_per_voxel) return; + float * address = voxels + (voxel_index * points_per_voxel + point_id) * 5; + atomicExch(address + 0, x); + atomicExch(address + 1, y); + atomicExch(address + 2, z); + atomicExch(address + 3, i); + atomicExch(address + 4, t); +} + +cudaError_t PreprocessCuda::generateVoxels_random_launch( + float * points, unsigned int points_size, unsigned int * mask, float * voxels) +{ + int threadNum = config_.threads_for_voxel_; + dim3 blocks((points_size + threadNum - 1) / threadNum); + dim3 threads(threadNum); + generateVoxels_random_kernel<<>>( + points, points_size, config_.min_x_range_, config_.max_x_range_, config_.min_y_range_, + config_.max_y_range_, config_.min_z_range_, config_.max_z_range_, config_.voxel_x_size_, + config_.voxel_y_size_, config_.voxel_z_size_, config_.grid_y_size_, config_.grid_x_size_, + config_.points_per_voxel_, mask, voxels); + cudaError_t err = cudaGetLastError(); + return err; +} + +__global__ void generateBaseFeatures_kernel( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, float points_per_voxel, + float max_voxels, unsigned int * pillar_num, float * voxel_features, unsigned int * voxel_num, + unsigned int * voxel_idxs) +{ + unsigned int voxel_idx = blockIdx.x * blockDim.x + threadIdx.x; + unsigned int voxel_idy = blockIdx.y * blockDim.y + threadIdx.y; + + if (voxel_idx >= grid_x_size || voxel_idy >= grid_y_size) return; + + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + unsigned int count = mask[voxel_index]; + if (!(count > 0)) return; + count = count < points_per_voxel ? count : points_per_voxel; + + unsigned int current_pillarId = 0; + current_pillarId = atomicAdd(pillar_num, 1); + if (current_pillarId >= max_voxels) return; + + voxel_num[current_pillarId] = count; + + uint4 idx = {0, 0, voxel_idy, voxel_idx}; + ((uint4 *)voxel_idxs)[current_pillarId] = idx; + + for (int i = 0; i < count; i++) { + int inIndex = voxel_index * points_per_voxel + i; + int outIndex = current_pillarId * points_per_voxel + i; + voxel_features[outIndex * 5] = voxels[inIndex * 5]; + voxel_features[outIndex * 5 + 1] = voxels[inIndex * 5 + 1]; + voxel_features[outIndex * 5 + 2] = voxels[inIndex * 5 + 2]; + voxel_features[outIndex * 5 + 3] = voxels[inIndex * 5 + 3]; + voxel_features[outIndex * 5 + 4] = voxels[inIndex * 5 + 4]; + } + + // clear buffer for next infer + atomicExch(mask + voxel_index, 0); +} + +// create 4 channels +cudaError_t PreprocessCuda::generateBaseFeatures_launch( + unsigned int * mask, float * voxels, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs) +{ + dim3 threads = {32, 32}; + dim3 blocks = {divup(config_.grid_x_size_, threads.x), divup(config_.grid_y_size_, threads.y)}; + + generateBaseFeatures_kernel<<>>( + mask, voxels, config_.grid_y_size_, config_.grid_x_size_, config_.points_per_voxel_, + config_.max_voxels_, pillar_num, voxel_features, voxel_num, voxel_idxs); + cudaError_t err = cudaGetLastError(); + return err; +} + +__global__ void generateSweepPoints_kernel( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, int num_features, float * output_points) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + const float input_x = input_points[point_idx * input_point_step + 0]; + const float input_y = input_points[point_idx * input_point_step + 1]; + const float input_z = input_points[point_idx * input_point_step + 2]; + const float intensity = input_points[point_idx * input_point_step + 3]; + + output_points[point_idx * num_features] = transform_array[0] * input_x + + transform_array[4] * input_y + + transform_array[8] * input_z + transform_array[12]; + output_points[point_idx * num_features + 1] = transform_array[1] * input_x + + transform_array[5] * input_y + + transform_array[9] * input_z + transform_array[13]; + output_points[point_idx * num_features + 2] = transform_array[2] * input_x + + transform_array[6] * input_y + + transform_array[10] * input_z + transform_array[14]; + output_points[point_idx * num_features + 3] = intensity; + output_points[point_idx * num_features + 4] = time_lag; +} + +cudaError_t PreprocessCuda::generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, float * output_points) +{ + int threadNum = config_.threads_for_voxel_; + dim3 blocks((points_size + threadNum - 1) / threadNum); + dim3 threads(threadNum); + + generateSweepPoints_kernel<<>>( + input_points, points_size, input_point_step, time_lag, transform_array, + config_.num_point_feature_size_, output_points); + + cudaError_t err = cudaGetLastError(); + return err; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp new file mode 100644 index 0000000000000..7072e8491af66 --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp @@ -0,0 +1,142 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/preprocess/voxel_generator.hpp" + +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" + +#include + +#include + +namespace lidar_transfusion +{ + +std::ostream & operator<<(std::ostream & os, const CloudInfo & info) +{ + os << "x_offset: " << static_cast(info.x_offset) << std::endl; + os << "y_offset: " << static_cast(info.y_offset) << std::endl; + os << "z_offset: " << static_cast(info.z_offset) << std::endl; + os << "intensity_offset: " << static_cast(info.intensity_offset) << std::endl; + os << "x_datatype: " << static_cast(info.x_datatype) << std::endl; + os << "y_datatype: " << static_cast(info.y_datatype) << std::endl; + os << "z_datatype: " << static_cast(info.z_datatype) << std::endl; + os << "intensity_datatype: " << static_cast(info.intensity_datatype) << std::endl; + os << "x_count: " << static_cast(info.x_count) << std::endl; + os << "y_count: " << static_cast(info.y_count) << std::endl; + os << "z_count: " << static_cast(info.z_count) << std::endl; + os << "intensity_count: " << static_cast(info.intensity_count) << std::endl; + os << "point_step: " << static_cast(info.point_step) << std::endl; + os << "is_bigendian: " << static_cast(info.is_bigendian) << std::endl; + return os; +} + +VoxelGenerator::VoxelGenerator( + const DensificationParam & densification_param, const TransfusionConfig & config, + cudaStream_t & stream) +: config_(config), stream_(stream) +{ + pd_ptr_ = std::make_unique(densification_param, stream_); + pre_ptr_ = std::make_unique(config_, stream_); + cloud_data_d_ = cuda::make_unique(config_.cloud_capacity_ * MAX_CLOUD_STEP_SIZE); + affine_past2current_d_ = cuda::make_unique(AFF_MAT_SIZE); +} + +bool VoxelGenerator::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer) +{ + return pd_ptr_->enqueuePointCloud(msg, tf_buffer); +} + +std::size_t VoxelGenerator::generateSweepPoints( + const sensor_msgs::msg::PointCloud2 & msg, cuda::unique_ptr & points_d) +{ + if (!is_initialized_) { + initCloudInfo(msg); + std::stringstream ss; + ss << "Input point cloud information: " << std::endl << cloud_info_; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_transfusion"), ss.str()); + + CloudInfo default_cloud_info; + if (cloud_info_ != default_cloud_info) { + ss << "Expected point cloud information: " << std::endl << default_cloud_info; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("lidar_transfusion"), ss.str()); + throw std::runtime_error("Input point cloud has unsupported format"); + } + is_initialized_ = true; + } + + size_t point_counter{0}; + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter); + pc_cache_iter++) { + auto sweep_num_points = pc_cache_iter->num_points; + if (point_counter + sweep_num_points >= config_.cloud_capacity_) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Exceeding cloud capacity. Used " + << pd_ptr_->getIdx(pc_cache_iter) << " out of " + << pd_ptr_->getCacheSize() << " sweep(s)"); + break; + } + auto shift = point_counter * config_.num_point_feature_size_; + + auto affine_past2current = + pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; + static_assert(std::is_same::value); + static_assert(!Eigen::Matrix4f::IsRowMajor, "matrices should be col-major."); + + float time_lag = static_cast( + pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds()); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + affine_past2current_d_.get(), affine_past2current.data(), AFF_MAT_SIZE * sizeof(float), + cudaMemcpyHostToDevice, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + pre_ptr_->generateSweepPoints_launch( + pc_cache_iter->points_d.get(), sweep_num_points, cloud_info_.point_step / sizeof(float), + time_lag, affine_past2current_d_.get(), points_d.get() + shift); + point_counter += sweep_num_points; + } + + return point_counter; +} + +void VoxelGenerator::initCloudInfo(const sensor_msgs::msg::PointCloud2 & msg) +{ + std::tie(cloud_info_.x_offset, cloud_info_.x_datatype, cloud_info_.x_count) = + getFieldInfo(msg, "x"); + std::tie(cloud_info_.y_offset, cloud_info_.y_datatype, cloud_info_.y_count) = + getFieldInfo(msg, "y"); + std::tie(cloud_info_.z_offset, cloud_info_.z_datatype, cloud_info_.z_count) = + getFieldInfo(msg, "z"); + std::tie( + cloud_info_.intensity_offset, cloud_info_.intensity_datatype, cloud_info_.intensity_count) = + getFieldInfo(msg, "intensity"); + cloud_info_.point_step = msg.point_step; + cloud_info_.is_bigendian = msg.is_bigendian; +} + +std::tuple VoxelGenerator::getFieldInfo( + const sensor_msgs::msg::PointCloud2 & msg, const std::string & field_name) +{ + for (const auto & field : msg.fields) { + if (field.name == field_name) { + return std::make_tuple(field.offset, field.datatype, field.count); + } + } + throw std::runtime_error("Missing field: " + field_name); +} +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/ros_utils.cpp b/perception/lidar_transfusion/lib/ros_utils.cpp new file mode 100644 index 0000000000000..0ecb34faf732d --- /dev/null +++ b/perception/lidar_transfusion/lib/ros_utils.cpp @@ -0,0 +1,83 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/ros_utils.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ + +using Label = autoware_perception_msgs::msg::ObjectClassification; + +void box3DToDetectedObject( + const Box3D & box3d, const std::vector & class_names, + autoware_perception_msgs::msg::DetectedObject & obj) +{ + obj.existence_probability = box3d.score; + + // classification + autoware_perception_msgs::msg::ObjectClassification classification; + classification.probability = 1.0f; + if (box3d.label >= 0 && static_cast(box3d.label) < class_names.size()) { + classification.label = getSemanticType(class_names[box3d.label]); + } else { + classification.label = Label::UNKNOWN; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Unexpected label: UNKNOWN is set."); + } + + if (object_recognition_utils::isCarLikeVehicle(classification.label)) { + obj.kinematics.orientation_availability = + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + } + + obj.classification.emplace_back(classification); + + // pose and shape + // mmdet3d yaw format to ros yaw format + float yaw = box3d.yaw + tier4_autoware_utils::pi / 2; + obj.kinematics.pose_with_covariance.pose.position = + tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + obj.kinematics.pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw); + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions = + tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); +} + +uint8_t getSemanticType(const std::string & class_name) +{ + if (class_name == "CAR") { + return Label::CAR; + } else if (class_name == "TRUCK") { + return Label::TRUCK; + } else if (class_name == "BUS") { + return Label::BUS; + } else if (class_name == "TRAILER") { + return Label::TRAILER; + } else if (class_name == "MOTORCYCLE") { + return Label::MOTORCYCLE; + } else if (class_name == "BICYCLE") { + return Label::BICYCLE; + } else if (class_name == "PEDESTRIAN") { + return Label::PEDESTRIAN; + } else { // CONSTRUCTION_VEHICLE, BARRIER, TRAFFIC_CONE + return Label::UNKNOWN; + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/transfusion_trt.cpp b/perception/lidar_transfusion/lib/transfusion_trt.cpp new file mode 100644 index 0000000000000..c876c0906162b --- /dev/null +++ b/perception/lidar_transfusion/lib/transfusion_trt.cpp @@ -0,0 +1,205 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/transfusion_trt.hpp" + +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/transfusion_config.hpp" + +#include + +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +TransfusionTRT::TransfusionTRT( + const NetworkParam & network_param, const DensificationParam & densification_param, + const TransfusionConfig & config) +: config_(config) +{ + network_trt_ptr_ = std::make_unique(config_); + + network_trt_ptr_->init( + network_param.onnx_path(), network_param.engine_path(), network_param.trt_precision()); + vg_ptr_ = std::make_unique(densification_param, config_, stream_); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("processing/inner"); + initPtr(); + + CHECK_CUDA_ERROR(cudaStreamCreate(&stream_)); +} + +TransfusionTRT::~TransfusionTRT() +{ + if (stream_) { + cudaStreamSynchronize(stream_); + cudaStreamDestroy(stream_); + } +} + +void TransfusionTRT::initPtr() +{ + // point cloud to voxels + voxel_features_size_ = + config_.max_voxels_ * config_.max_num_points_per_pillar_ * config_.num_point_feature_size_; + voxel_num_size_ = config_.max_voxels_; + voxel_idxs_size_ = config_.max_voxels_ * config_.num_point_values_; + + // output of TRT -- input of post-process + cls_size_ = config_.num_proposals_ * config_.num_classes_; + box_size_ = config_.num_proposals_ * config_.num_box_values_; + dir_cls_size_ = config_.num_proposals_ * 2; // x, y + cls_output_d_ = cuda::make_unique(cls_size_); + box_output_d_ = cuda::make_unique(box_size_); + dir_cls_output_d_ = cuda::make_unique(dir_cls_size_); + + params_input_d_ = cuda::make_unique(); + voxel_features_d_ = cuda::make_unique(voxel_features_size_); + voxel_num_d_ = cuda::make_unique(voxel_num_size_); + voxel_idxs_d_ = cuda::make_unique(voxel_idxs_size_); + points_d_ = cuda::make_unique(config_.cloud_capacity_ * config_.num_point_feature_size_); + pre_ptr_ = std::make_unique(config_, stream_); + post_ptr_ = std::make_unique(config_, stream_); +} + +bool TransfusionTRT::detect( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, + std::vector & det_boxes3d, std::unordered_map & proc_timing) +{ + stop_watch_ptr_->toc("processing/inner", true); + if (!preprocess(msg, tf_buffer)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to preprocess and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/preprocess_ms", stop_watch_ptr_->toc("processing/inner", true)); + + if (!inference()) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to inference and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/inference_ms", stop_watch_ptr_->toc("processing/inner", true)); + + if (!postprocess(det_boxes3d)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to postprocess and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/postprocess_ms", stop_watch_ptr_->toc("processing/inner", true)); + + return true; +} + +bool TransfusionTRT::preprocess( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer) +{ + if (!vg_ptr_->enqueuePointCloud(msg, tf_buffer)) { + return false; + } + + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_.cloud_capacity_ * config_.num_point_feature_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + const auto count = vg_ptr_->generateSweepPoints(msg, points_d_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_transfusion"), "Generated sweep points: " << count); + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + unsigned int params_input; + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + ¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + if (params_input < config_.min_voxel_size_) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "Too few voxels (" << params_input << ") for actual optimization profile (" + << config_.min_voxel_size_ << ")"); + return false; + } + + if (params_input > config_.max_voxels_) { + params_input = config_.max_voxels_; + } + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Generated input voxels: " << params_input); + + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::voxels), voxel_features_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::voxels), + nvinfer1::Dims3{ + static_cast(params_input), static_cast(config_.max_num_points_per_pillar_), + static_cast(config_.num_point_feature_size_)}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::num_points), voxel_num_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::num_points), + nvinfer1::Dims{1, {static_cast(params_input)}}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::coors), voxel_idxs_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::coors), + nvinfer1::Dims2{ + static_cast(params_input), static_cast(config_.num_point_values_)}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::cls_score), cls_output_d_.get()); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::bbox_pred), box_output_d_.get()); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::dir_pred), dir_cls_output_d_.get()); + return true; +} + +bool TransfusionTRT::inference() +{ + auto status = network_trt_ptr_->context->enqueueV3(stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + if (!status) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to enqueue and skip to detect."); + return false; + } + return true; +} + +bool TransfusionTRT::postprocess(std::vector & det_boxes3d) +{ + CHECK_CUDA_ERROR(post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + return true; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/package.xml b/perception/lidar_transfusion/package.xml new file mode 100644 index 0000000000000..29643d0957eb1 --- /dev/null +++ b/perception/lidar_transfusion/package.xml @@ -0,0 +1,34 @@ + + + + lidar_transfusion + 1.0.0 + The lidar_transfusion package + Amadeusz Szymko + Kenzo Lobos-Tsunekawa + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_perception_msgs + launch_ros + object_recognition_utils + pcl_ros + rclcpp + rclcpp_components + tensorrt_common + tf2_eigen + tf2_geometry_msgs + tf2_sensor_msgs + tier4_autoware_utils + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/lidar_transfusion/schema/detection_class_remapper.schema.json b/perception/lidar_transfusion/schema/detection_class_remapper.schema.json new file mode 100644 index 0000000000000..8aaefa295fcc0 --- /dev/null +++ b/perception/lidar_transfusion/schema/detection_class_remapper.schema.json @@ -0,0 +1,72 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Detection Class Remapper", + "type": "object", + "definitions": { + "detection_class_remapper": { + "type": "object", + "properties": { + "allow_remapping_by_area_matrix": { + "type": "array", + "items": { + "type": "integer" + }, + "description": "Whether to allow remapping of classes. The order of 8x8 matrix classes comes from ObjectClassification msg.", + "default": [ + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 + ], + "minItems": 64, + "maxItems": 64 + }, + "min_area_matrix": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Minimum area for specific class to consider class remapping.", + "default": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 12.1, 0.0, 36.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 36.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ], + "minItems": 64, + "maxItems": 64 + }, + "max_area_matrix": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Maximum area for specific class to consider class remapping.", + "default": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36.0, 0.0, 999.999, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 999.999, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ], + "minItems": 64, + "maxItems": 64 + } + }, + "required": ["allow_remapping_by_area_matrix", "min_area_matrix", "max_area_matrix"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/detection_class_remapper" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/lidar_transfusion/schema/transfusion.schema.json new file mode 100644 index 0000000000000..41d8d887236a8 --- /dev/null +++ b/perception/lidar_transfusion/schema/transfusion.schema.json @@ -0,0 +1,160 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for LiDAR TransFusion Node", + "type": "object", + "definitions": { + "transfusion": { + "type": "object", + "properties": { + "class_names": { + "type": "array", + "items": { + "type": "string" + }, + "description": "An array of class names will be predicted.", + "default": ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "trt_precision": { + "type": "string", + "description": "A precision of TensorRT engine.", + "default": "fp16", + "enum": ["fp16", "fp32"] + }, + "voxels_num": { + "type": "array", + "items": { + "type": "integer" + }, + "description": "A maximum number of voxels [min, opt, max].", + "default": [5000, 30000, 60000] + }, + "point_cloud_range": { + "type": "array", + "items": { + "type": "number" + }, + "description": "An array of distance ranges of each class.", + "default": [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0], + "minItems": 6, + "maxItems": 6 + }, + "voxel_size": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Voxels size [x, y, z].", + "default": [0.3, 0.3, 8.0], + "minItems": 3, + "maxItems": 3 + }, + "onnx_path": { + "type": "string", + "description": "A path to ONNX model file.", + "default": "$(var model_path)/transfusion.onnx", + "pattern": "\\.onnx$" + }, + "engine_path": { + "type": "string", + "description": "A path to TensorRT engine file.", + "default": "$(var model_path)/transfusion.engine", + "pattern": "\\.engine$" + }, + "down_sample_factor": { + "type": "integer", + "description": "A scale factor of downsampling points", + "default": 1, + "minimum": 1 + }, + "densification_num_past_frames": { + "type": "integer", + "description": "A number of past frames to be considered as same input frame.", + "default": 1, + "minimum": 0 + }, + "densification_world_frame_id": { + "type": "string", + "description": "A name of frame id where world coordinates system is defined with respect to.", + "default": "map" + }, + "circle_nms_dist_threshold": { + "type": "number", + "description": "A distance threshold between detections in NMS.", + "default": 0.5, + "minimum": 0.0 + }, + "iou_nms_target_class_names": { + "type": "array", + "items": { + "type": "string" + }, + "description": "An array of class names to be target in NMS.", + "default": ["CAR"], + "uniqueItems": true + }, + "iou_nms_search_distance_2d": { + "type": "number", + "description": "A maximum distance value to search the nearest objects.", + "default": 10.0, + "minimum": 0.0 + }, + "iou_nms_threshold": { + "type": "number", + "description": "A threshold value of NMS using IoU score.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + }, + "yaw_norm_thresholds": { + "type": "array", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + }, + "description": "A thresholds array of direction vectors norm, all of objects with vector norm less than this threshold are ignored.", + "default": [0.3, 0.3, 0.3, 0.3, 0.0] + }, + "score_threshold": { + "type": "number", + "description": "A threshold value of confidence score, all of objects with score less than this threshold are ignored.", + "default": 0.2, + "minimum": 0.0 + } + }, + "required": [ + "class_names", + "trt_precision", + "voxels_num", + "point_cloud_range", + "voxel_size", + "onnx_path", + "engine_path", + "densification_num_past_frames", + "densification_world_frame_id", + "circle_nms_dist_threshold", + "iou_nms_target_class_names", + "iou_nms_search_distance_2d", + "iou_nms_threshold", + "yaw_norm_thresholds", + "score_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/transfusion" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp new file mode 100644 index 0000000000000..9313015002973 --- /dev/null +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -0,0 +1,178 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/lidar_transfusion_node.hpp" + +#include "lidar_transfusion/utils.hpp" + +namespace lidar_transfusion +{ + +LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) +: Node("lidar_transfusion", options), tf_buffer_(this->get_clock()) +{ + auto descriptor = rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); + // network + class_names_ = this->declare_parameter>("class_names", descriptor); + const std::string trt_precision = + this->declare_parameter("trt_precision", descriptor); + const auto voxels_num = this->declare_parameter>("voxels_num", descriptor); + const auto point_cloud_range = + this->declare_parameter>("point_cloud_range", descriptor); + const auto voxel_size = this->declare_parameter>("voxel_size", descriptor); + const std::string onnx_path = this->declare_parameter("onnx_path", descriptor); + const std::string engine_path = this->declare_parameter("engine_path", descriptor); + + if (point_cloud_range.size() != 6) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "The size of point_cloud_range != 6: use the default parameters."); + } + if (voxel_size.size() != 3) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "The size of voxel_size != 3: use the default parameters."); + } + + // pre-process + const std::string densification_world_frame_id = + this->declare_parameter("densification_world_frame_id", descriptor); + const int densification_num_past_frames = + this->declare_parameter("densification_num_past_frames", descriptor); + + // post-process + const float circle_nms_dist_threshold = + static_cast(this->declare_parameter("circle_nms_dist_threshold", descriptor)); + { // IoU NMS + NMSParams p; + p.nms_type_ = NMS_TYPE::IoU_BEV; + p.target_class_names_ = + this->declare_parameter>("iou_nms_target_class_names", descriptor); + p.search_distance_2d_ = + this->declare_parameter("iou_nms_search_distance_2d", descriptor); + p.iou_threshold_ = this->declare_parameter("iou_nms_threshold", descriptor); + iou_bev_nms_.setParameters(p); + } + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds", descriptor); + const float score_threshold = + static_cast(this->declare_parameter("score_threshold", descriptor)); + + NetworkParam network_param(onnx_path, engine_path, trt_precision); + DensificationParam densification_param( + densification_world_frame_id, densification_num_past_frames); + TransfusionConfig config( + voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, + score_threshold); + + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix", descriptor); + const auto min_area_matrix = + this->declare_parameter>("min_area_matrix", descriptor); + const auto max_area_matrix = + this->declare_parameter>("max_area_matrix", descriptor); + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + detector_ptr_ = std::make_unique(network_param, densification_param, config); + + cloud_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), + std::bind(&LidarTransfusionNode::cloudCallback, this, std::placeholders::_1)); + + objects_pub_ = this->create_publisher( + "~/output/objects", rclcpp::QoS(1)); + + published_time_pub_ = std::make_unique(this); + + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, this->get_name()); + stop_watch_ptr_->tic("cyclic"); + stop_watch_ptr_->tic("processing/total"); + } + + if (this->declare_parameter("build_only", false, descriptor)) { + RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); + rclcpp::shutdown(); + } +} + +void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + const auto objects_sub_count = + objects_pub_->get_subscription_count() + objects_pub_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + if (stop_watch_ptr_) { + stop_watch_ptr_->toc("processing/total", true); + } + + std::vector det_boxes3d; + std::unordered_map proc_timing; + bool is_success = detector_ptr_->detect(*msg, tf_buffer_, det_boxes3d, proc_timing); + if (!is_success) { + return; + } + + std::vector raw_objects; + raw_objects.reserve(det_boxes3d.size()); + for (const auto & box3d : det_boxes3d) { + autoware_perception_msgs::msg::DetectedObject obj; + box3DToDetectedObject(box3d, class_names_, obj); + raw_objects.emplace_back(obj); + } + + autoware_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = msg->header; + output_msg.objects = iou_bev_nms_.apply(raw_objects); + + detection_class_remapper_.mapClasses(output_msg); + + if (objects_sub_count > 0) { + objects_pub_->publish(output_msg); + published_time_pub_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); + } + + // add processing time for debug + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing/total", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + debug_publisher_ptr_->publish( + "debug/processing_time/total_ms", processing_time_ms); + for (const auto & [topic, time_ms] : proc_timing) { + debug_publisher_ptr_->publish(topic, time_ms); + } + } +} + +} // namespace lidar_transfusion + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(lidar_transfusion::LidarTransfusionNode) diff --git a/perception/multi_object_tracker/src/debugger/debug_object.cpp b/perception/multi_object_tracker/src/debugger/debug_object.cpp index f166e83e136af..e6a0b6242a168 100644 --- a/perception/multi_object_tracker/src/debugger/debug_object.cpp +++ b/perception/multi_object_tracker/src/debugger/debug_object.cpp @@ -134,6 +134,9 @@ void TrackerObjectDebugger::process() { if (!is_initialized_) return; + // Check if object_data_list_ is empty + if (object_data_list_.empty()) return; + // update uuid_int for (auto & object_data : object_data_list_) { current_ids_.insert(object_data.uuid_int); diff --git a/planning/.pages b/planning/.pages index 4f1d928252185..ce08c389c17f5 100644 --- a/planning/.pages +++ b/planning/.pages @@ -10,12 +10,12 @@ nav: - 'Dynamic Drivable Area': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design - 'Turn Signal': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - 'Scene Module': - - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module + - 'Avoidance by Lane Change': planning/autoware_behavior_path_avoidance_by_lane_change_module - 'Dynamic Obstacle Avoidance': planning/autoware_behavior_path_dynamic_obstacle_avoidance_module - 'Goal Planner': planning/behavior_path_goal_planner_module - 'Lane Change': planning/behavior_path_lane_change_module - 'Side Shift': planning/autoware_behavior_path_side_shift_module - - 'Start Planner': planning/behavior_path_start_planner_module + - 'Start Planner': planning/autoware_behavior_path_start_planner_module - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner @@ -25,14 +25,13 @@ nav: - 'Crosswalk': planning/behavior_velocity_crosswalk_module - 'Detection Area': planning/behavior_velocity_detection_area_module - 'Dynamic Obstacle Stop': planning/behavior_velocity_dynamic_obstacle_stop_module - - 'Intersection': planning/behavior_velocity_intersection_module + - 'Intersection': planning/autoware_behavior_velocity_intersection_module - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module - 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module - - 'Occlusion Spot': planning/behavior_velocity_occlusion_spot_module - - 'Out of Lane': planning/behavior_velocity_out_of_lane_module + - 'Occlusion Spot': planning/autoware_behavior_velocity_occlusion_spot_module - 'Run Out': planning/autoware_behavior_velocity_run_out_module - 'Speed Bump': planning/behavior_velocity_speed_bump_module - - 'Stop Line': planning/behavior_velocity_stop_line_module + - 'Stop Line': planning/autoware_behavior_velocity_stop_line_module - 'Traffic Light': planning/behavior_velocity_traffic_light_module - 'Virtual Traffic Light': planning/autoware_behavior_velocity_virtual_traffic_light_module - 'Walkway': planning/autoware_behavior_velocity_walkway_module @@ -53,13 +52,13 @@ nav: - 'Obstacle Stop Planner': planning/obstacle_stop_planner - 'Obstacle Velocity Limiter': planning/obstacle_velocity_limiter - 'Path Smoother': - - 'About Path Smoother': planning/path_smoother - - 'Elastic Band': planning/path_smoother/docs/eb + - 'About Path Smoother': planning/autoware_path_smoother + - 'Elastic Band': planning/autoware_path_smoother/docs/eb - 'Sampling Based Planner': - - 'Path Sample': planning/sampling_based_planner/path_sampler - - 'Common library': planning/sampling_based_planner/sampler_common - - 'Frenet Planner': planning/sampling_based_planner/frenet_planner - - 'Bezier Sampler': planning/sampling_based_planner/bezier_sampler + - 'Path Sample': planning/sampling_based_planner/autoware_path_sampler + - 'Common library': planning/sampling_based_planner/autoware_sampler_common + - 'Frenet Planner': planning/sampling_based_planner/autoware_frenet_planner + - 'Bezier Sampler': planning/sampling_based_planner/autoware_bezier_sampler - 'Surround Obstacle Checker': - 'About Surround Obstacle Checker': planning/autoware_surround_obstacle_checker - 'About Surround Obstacle Checker (Japanese)': planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja @@ -70,14 +69,14 @@ nav: - 'Velocity Smoother': - 'About Velocity Smoother': planning/autoware_velocity_smoother - 'About Velocity Smoother (Japanese)': planning/autoware_velocity_smoother/README.ja - - 'Scenario Selector': planning/scenario_selector + - 'Scenario Selector': planning/autoware_scenario_selector - 'Static Centerline Generator': planning/autoware_static_centerline_generator - 'API and Library': - 'Costmap Generator': planning/costmap_generator - 'External Velocity Limit Selector': planning/autoware_external_velocity_limit_selector - 'Objects of Interest Marker Interface': planning/objects_of_interest_marker_interface - 'Route Handler': planning/route_handler - - 'RTC Interface': planning/rtc_interface + - 'RTC Interface': planning/autoware_rtc_interface - 'Additional Tools': - 'Remaining Distance Time Calculator': planning/autoware_remaining_distance_time_calculator - 'RTC Replayer': planning/rtc_replayer @@ -87,4 +86,4 @@ nav: - 'Planning Test Utils': planning/planning_test_utils - 'Planning Test Manager': planning/autoware_planning_test_manager - 'Planning Topic Converter': planning/autoware_planning_topic_converter - - 'Planning Validator': planning/planning_validator + - 'Planning Validator': planning/autoware_planning_validator diff --git a/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt b/planning/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt similarity index 90% rename from planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt index bca909c7cef7e..2ab88e04f96e5 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_avoidance_by_lane_change_module) +project(autoware_behavior_path_avoidance_by_lane_change_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_avoidance_by_lane_change_module/README.md b/planning/autoware_behavior_path_avoidance_by_lane_change_module/README.md similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/README.md rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/README.md diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml b/planning/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml diff --git a/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg b/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lane_change.svg diff --git a/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg b/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_1.svg diff --git a/planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg b/planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg similarity index 100% rename from planning/behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/images/avoidance_by_lc_trigger_2.svg diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml similarity index 93% rename from planning/behavior_path_avoidance_by_lane_change_module/package.xml rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml index 1b11b6d8d445d..3a75849931228 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/package.xml @@ -1,7 +1,7 @@ - behavior_path_avoidance_by_lane_change_module + autoware_behavior_path_avoidance_by_lane_change_module 0.1.0 The behavior_path_avoidance_by_lane_change_module package @@ -20,13 +20,13 @@ autoware_behavior_path_planner_common autoware_behavior_path_static_obstacle_avoidance_module + autoware_rtc_interface behavior_path_lane_change_module behavior_path_planner lanelet2_extension motion_utils pluginlib rclcpp - rtc_interface tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml b/planning/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..848d978329866 --- /dev/null +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp similarity index 79% rename from planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp index d7fd8b82bd71e..01f2fd7f1ee52 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/data_structs.hpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/data_structs.hpp @@ -11,13 +11,15 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ +#ifndef DATA_STRUCTS_HPP_ +#define DATA_STRUCTS_HPP_ #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::AvoidanceParameters; + struct AvoidanceByLCParameters : public AvoidanceParameters { // execute only when the target object longitudinal distance is larger than this param. @@ -30,6 +32,6 @@ struct AvoidanceByLCParameters : public AvoidanceParameters { } }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__DATA_STRUCTS_HPP_ +#endif // DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp similarity index 92% rename from planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp index bae9112e6d840..c5070540152de 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_by_lane_change_module/interface.hpp" +#include "interface.hpp" #include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" @@ -20,8 +20,11 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::State; +using ::route_handler::Direction; + AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, @@ -63,4 +66,4 @@ void AvoidanceByLaneChangeInterface::updateRTCStatus( uuid_map_.at(direction), isExecutionReady(), state, start_distance, finish_distance, clock_->now()); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp similarity index 77% rename from planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp index 897956a392008..75413c4dccfa6 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/interface.hpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/interface.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#ifndef INTERFACE_HPP_ +#define INTERFACE_HPP_ -#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_avoidance_by_lane_change_module/scene.hpp" #include "behavior_path_lane_change_module/interface.hpp" +#include "data_structs.hpp" +#include "scene.hpp" #include @@ -25,8 +25,12 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeInterface; +using ::behavior_path_planner::ObjectsOfInterestMarkerInterface; +using ::behavior_path_planner::RTCInterface; + class AvoidanceByLaneChangeInterface : public LaneChangeInterface { public: @@ -45,6 +49,6 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface protected: void updateRTCStatus(const double start_distance, const double finish_distance) override; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#endif // INTERFACE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp similarity index 96% rename from planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp index 868b2585170f9..792f0e93ac1b7 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_by_lane_change_module/manager.hpp" +#include "manager.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp" -#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" +#include "data_structs.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" @@ -25,8 +25,10 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::getParameter; +using ::behavior_path_planner::ObjectParameter; void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { @@ -191,9 +193,9 @@ AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() objects_of_interest_marker_interface_ptr_map_); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::AvoidanceByLaneChangeModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager, + ::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp similarity index 73% rename from planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp index 75fa67e7fe1a3..f921aa4d7e31c 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/manager.hpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ -#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" -#include "behavior_path_avoidance_by_lane_change_module/interface.hpp" #include "behavior_path_lane_change_module/manager.hpp" +#include "data_structs.hpp" +#include "interface.hpp" #include @@ -26,9 +26,12 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { -using route_handler::Direction; +using ::behavior_path_planner::LaneChangeModuleManager; +using ::behavior_path_planner::LaneChangeModuleType; +using ::behavior_path_planner::SceneModuleInterface; +using ::route_handler::Direction; class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager { @@ -47,6 +50,6 @@ class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager private: std::shared_ptr avoidance_parameters_; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp similarity index 96% rename from planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index e0ebeebba955c..0647147dff0d0 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_avoidance_by_lane_change_module/scene.hpp" +#include "scene.hpp" #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -28,13 +28,19 @@ #include #include +#include #include #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { -using behavior_path_planner::utils::lane_change::debug::createExecutionArea; +using ::behavior_path_planner::Direction; +using ::behavior_path_planner::LaneChangeModuleType; +using ::behavior_path_planner::ObjectInfo; +using ::behavior_path_planner::Point2d; +using ::behavior_path_planner::utils::lane_change::debug::createExecutionArea; +namespace utils = ::behavior_path_planner::utils; AvoidanceByLaneChange::AvoidanceByLaneChange( const std::shared_ptr & parameters, @@ -293,4 +299,4 @@ double AvoidanceByLaneChange::calcLateralOffset() const } return additional_lat_offset; } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp similarity index 76% rename from planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp index 4ef4c0673c0c5..6b39340da172a 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/include/behavior_path_avoidance_by_lane_change_module/scene.hpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.hpp @@ -12,19 +12,26 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" -#include "behavior_path_avoidance_by_lane_change_module/data_structs.hpp" #include "behavior_path_lane_change_module/scene.hpp" +#include "data_structs.hpp" #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::DebugData; using AvoidanceDebugData = DebugData; -using helper::static_obstacle_avoidance::AvoidanceHelper; +using ::behavior_path_planner::AvoidancePlanningData; +using ::behavior_path_planner::LaneChangeParameters; +using ::behavior_path_planner::NormalLaneChange; +using ::behavior_path_planner::ObjectData; +using ::behavior_path_planner::ObjectDataArray; +using ::behavior_path_planner::PredictedObject; +using ::behavior_path_planner::helper::static_obstacle_avoidance::AvoidanceHelper; class AvoidanceByLaneChange : public NormalLaneChange { @@ -59,6 +66,6 @@ class AvoidanceByLaneChange : public NormalLaneChange double calcMinimumLaneChangeLength() const; double calcLateralOffset() const; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_AVOIDANCE_BY_LANE_CHANGE_MODULE__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp similarity index 79% rename from planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 69ce87aabc7b4..ea7979958e49e 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -23,7 +23,7 @@ #include #include -using behavior_path_planner::BehaviorPathPlannerNode; +using ::behavior_path_planner::BehaviorPathPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -52,26 +52,26 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); + module_names.emplace_back("autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, - {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", - ament_index_cpp::get_package_share_directory( - "autoware_behavior_path_static_obstacle_avoidance_module") + - "/config/static_obstacle_avoidance.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_avoidance_by_lane_change_module") + - "/config/avoidance_by_lane_change.param.yaml"}); + node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + behavior_path_lane_change_module_dir + "/config/lane_change.param.yaml", + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_static_obstacle_avoidance_module") + + "/config/static_obstacle_avoidance.param.yaml", + ament_index_cpp::get_package_share_directory( + "autoware_behavior_path_avoidance_by_lane_change_module") + + "/config/avoidance_by_lane_change.param.yaml"}); return std::make_shared(node_options); } diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml index 26b78f5cf8cf1..9410be5a0011c 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -19,12 +19,12 @@ eigen3_cmake_module autoware_behavior_path_planner_common + autoware_rtc_interface behavior_path_lane_change_module behavior_path_planner motion_utils pluginlib rclcpp - rtc_interface tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp index 97f249e0e7d6e..ebbb795a699b5 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -29,7 +30,6 @@ #include #include #include -#include #include #include #include @@ -56,11 +56,11 @@ namespace behavior_path_planner { +using autoware::rtc_interface::RTCInterface; using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; -using rtc_interface::RTCInterface; using steering_factor_interface::SteeringFactorInterface; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp index 4abbfd419c43b..04873d9ebcfab 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp @@ -18,7 +18,7 @@ #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include @@ -60,10 +60,10 @@ inline int64_t bitShift(int64_t original_id) void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); MarkerArray createPointsMarkerArray( @@ -107,8 +107,9 @@ MarkerArray createDrivableLanesMarkerArray( const std::vector & drivable_lanes, std::string && ns); MarkerArray createPredictedPathMarkerArray( - const PredictedPath & ego_predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, - std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); + const PredictedPath & ego_predicted_path, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, std::string && ns, + const int32_t & id, const float & r, const float & g, const float & b); MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp index 21d4906380b59..2f14e227d05ef 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ #define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ -#include +#include struct ModuleConfigParameters { @@ -66,7 +66,7 @@ struct BehaviorPathPlannerParameters double ego_nearest_yaw_threshold; // vehicle info - vehicle_info_util::VehicleInfo vehicle_info; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; double wheel_base; double front_overhang; double rear_overhang; diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp index 4dda036fecdb7..d658d29710704 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp @@ -237,7 +237,7 @@ class TurnSignalDecider inline bool straddleRoadBound( const ShiftedPath & path, const ShiftLine & shift_line, const lanelet::ConstLanelets & lanes, - const vehicle_info_util::VehicleInfo & vehicle_info) const + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) const { using boost::geometry::intersects; using tier4_autoware_utils::pose2transform; diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 32613948814ae..35da7e4e24382 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ #define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#include #include -#include #include #include @@ -83,7 +83,7 @@ struct DrivableAreaExpansionParameters bool avoid_dynamic_objects{}; bool print_runtime{}; std::vector avoid_linestring_types{}; - vehicle_info_util::VehicleInfo vehicle_info; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; DrivableAreaExpansionParameters() = default; explicit DrivableAreaExpansionParameters(rclcpp::Node & node) { init(node); } @@ -121,7 +121,7 @@ struct DrivableAreaExpansionParameters avoid_linestring_dist = node.declare_parameter(AVOID_LINESTRING_DIST_PARAM); print_runtime = node.declare_parameter(PRINT_RUNTIME_PARAM); - vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); } }; diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 867e76f0db614..fe8dc4c25869a 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -18,7 +18,7 @@ #include "autoware_behavior_path_planner_common/data_manager.hpp" #include "autoware_behavior_path_planner_common/parameters.hpp" -#include +#include #include #include @@ -75,7 +75,8 @@ class GeometricParallelParking bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, - const std::shared_ptr lane_departure_checker); + const std::shared_ptr + autoware_lane_departure_checker); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } void setPlannerData(const std::shared_ptr & planner_data) { @@ -119,7 +120,8 @@ class GeometricParallelParking const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double lane_departure_margin, const double arc_path_interval, - const std::shared_ptr lane_departure_checker); + const std::shared_ptr + autoware_lane_departure_checker); PathWithLaneId generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, const double arc_path_interval, const bool is_left_turn, const bool is_forward); diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index d0693a271379e..018e1c87e2302 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -31,6 +31,7 @@ namespace behavior_path_planner::utils::path_safety_checker { +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; @@ -40,7 +41,6 @@ using geometry_msgs::msg::Twist; using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; bool isTargetObjectOncoming( const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose, @@ -48,13 +48,13 @@ bool isTargetObjectOncoming( bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon); Polygon2d createExtendedPolygon( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug); Polygon2d createExtendedPolygon( diff --git a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp index e58190fa49650..a0bc681144670 100644 --- a/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -73,8 +73,8 @@ std::pair getPathTurnSignal( const BehaviorPathPlannerParameters & common_parameter); PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, - const lanelet::ConstLanelets & lanelets); + const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints, + const double velocity, const lanelet::ConstLanelets & lanelets); std::vector getReversingIndices(const PathWithLaneId & path); diff --git a/planning/autoware_behavior_path_planner_common/package.xml b/planning/autoware_behavior_path_planner_common/package.xml index 21da8dcef1c2d..e9b33f3e71d6f 100644 --- a/planning/autoware_behavior_path_planner_common/package.xml +++ b/planning/autoware_behavior_path_planner_common/package.xml @@ -43,12 +43,14 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_freespace_planning_algorithms + autoware_lane_departure_checker autoware_perception_msgs autoware_planning_msgs - freespace_planning_algorithms + autoware_rtc_interface + autoware_vehicle_info_utils geometry_msgs interpolation - lane_departure_checker lanelet2_extension magic_enum motion_utils @@ -56,12 +58,10 @@ objects_of_interest_marker_interface rclcpp route_handler - rtc_interface tf2 tier4_autoware_utils tier4_planning_msgs traffic_light_utils - vehicle_info_util visualization_msgs ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index 2b150b807c6ce..a22a63ba0fe6d 100644 --- a/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -42,7 +42,7 @@ using visualization_msgs::msg::Marker; void addFootprintMarker( visualization_msgs::msg::Marker & marker, const geometry_msgs::msg::Pose & pose, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const double half_width = vehicle_info.vehicle_width_m / 2.0; const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m; @@ -60,7 +60,7 @@ void addFootprintMarker( } MarkerArray createFootprintMarkerArray( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo vehicle_info, const std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) { const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); @@ -388,8 +388,9 @@ MarkerArray createDrivableLanesMarkerArray( return msg; } MarkerArray createPredictedPathMarkerArray( - const PredictedPath & predicted_path, const vehicle_info_util::VehicleInfo & vehicle_info, - std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) + const PredictedPath & predicted_path, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, std::string && ns, + const int32_t & id, const float & r, const float & g, const float & b) { if (predicted_path.path.empty()) { return MarkerArray{}; diff --git a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 26c089ee0ba09..73f513c9e65eb 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -223,7 +223,8 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) { constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose @@ -364,7 +365,8 @@ std::vector GeometricParallelParking::planOneTrial( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, const double lane_departure_margin, const double arc_path_interval, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) { clearPaths(); diff --git a/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 233e6f47b92cc..7951be43aa799 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -51,7 +51,7 @@ bool isTargetObjectOncoming( bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_offset_pose = @@ -71,7 +71,7 @@ bool isTargetObjectFront( bool isTargetObjectFront( const PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Polygon2d & obj_polygon) { const double base_to_front = vehicle_info.max_longitudinal_offset_m; const auto ego_point = @@ -90,7 +90,7 @@ bool isTargetObjectFront( } Polygon2d createExtendedPolygon( - const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, + const Pose & base_link_pose, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { @@ -189,7 +189,7 @@ Polygon2d createExtendedPolygon( Polygon2d createExtendedPolygonAlongPath( const PathWithLaneId & planned_path, const Pose & base_link_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lon_length, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lon_length, const double lat_margin, const bool is_stopped_obj, CollisionCheckDebug & debug) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; diff --git a/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index 9193db9df11e5..a453ba947331d 100644 --- a/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -330,8 +330,8 @@ std::pair getPathTurnSignal( } PathWithLaneId convertWayPointsToPathWithLaneId( - const freespace_planning_algorithms::PlannerWaypoints & waypoints, const double velocity, - const lanelet::ConstLanelets & lanelets) + const autoware::freespace_planning_algorithms::PlannerWaypoints & waypoints, + const double velocity, const lanelet::ConstLanelets & lanelets) { PathWithLaneId path; path.header = waypoints.header; diff --git a/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp b/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp index 3cb71b1f9c4e2..4f23c9f6c07c0 100644 --- a/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -39,7 +39,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) { using behavior_path_planner::utils::path_safety_checker::createExtendedPolygon; - vehicle_info_util::VehicleInfo vehicle_info; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; vehicle_info.max_longitudinal_offset_m = 4.0; vehicle_info.vehicle_width_m = 2.0; vehicle_info.rear_overhang_m = 1.0; diff --git a/planning/behavior_path_start_planner_module/CMakeLists.txt b/planning/autoware_behavior_path_start_planner_module/CMakeLists.txt similarity index 88% rename from planning/behavior_path_start_planner_module/CMakeLists.txt rename to planning/autoware_behavior_path_start_planner_module/CMakeLists.txt index 6bb2f76cb9842..b2882727f50d8 100644 --- a/planning/behavior_path_start_planner_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_start_planner_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_start_planner_module) +project(autoware_behavior_path_start_planner_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/autoware_behavior_path_start_planner_module/README.md similarity index 99% rename from planning/behavior_path_start_planner_module/README.md rename to planning/autoware_behavior_path_start_planner_module/README.md index 71aa0a1abe704..b2c92f31ded31 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/autoware_behavior_path_start_planner_module/README.md @@ -526,7 +526,7 @@ If a safe path cannot be generated from the current position, search backwards f ### **freespace pull out** If the vehicle gets stuck with pull out along lanes, execute freespace pull out. -To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../autoware_costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` @@ -545,4 +545,4 @@ To run this feature, you need to set `parking_lot` to the map, `activate_by_scen | end_pose_search_end_distance | [m] | double | distance from ego to the end point of the search for the end point in the freespace_pull_out driving lane | 30.0 | | end_pose_search_interval | [m] | bool | interval to search for the end point in the freespace_pull_out driving lane | 2.0 | -See [freespace_planner](../freespace_planner/README.md) for other parameters. +See [freespace_planner](../autoware_freespace_planner/README.md) for other parameters. diff --git a/planning/behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml similarity index 100% rename from planning/behavior_path_start_planner_module/config/start_planner.param.yaml rename to planning/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml diff --git a/planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/collision_check_range_shift_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/freespace_pull_out.png b/planning/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png similarity index 100% rename from planning/behavior_path_start_planner_module/images/freespace_pull_out.png rename to planning/autoware_behavior_path_start_planner_module/images/freespace_pull_out.png diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/geometric_pull_out_path_with_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/priority_order.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/priority_order.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/priority_order.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/pull_out_after_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/pull_out_collision_check.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_from_road_lane.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/shift_pull_out_path_with_back.drawio.svg diff --git a/planning/behavior_path_start_planner_module/images/start_planner_example.png b/planning/autoware_behavior_path_start_planner_module/images/start_planner_example.png similarity index 100% rename from planning/behavior_path_start_planner_module/images/start_planner_example.png rename to planning/autoware_behavior_path_start_planner_module/images/start_planner_example.png diff --git a/planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg b/planning/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg similarity index 100% rename from planning/behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg rename to planning/autoware_behavior_path_start_planner_module/images/start_pose_candidate.drawio.svg diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp similarity index 88% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp index d8b94bb33d00b..7959c4aeec111 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/data_structs.hpp @@ -13,15 +13,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ #include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -34,9 +34,9 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; -using freespace_planning_algorithms::AstarParam; -using freespace_planning_algorithms::PlannerCommonParam; -using freespace_planning_algorithms::RRTStarParam; +using autoware::freespace_planning_algorithms::AstarParam; +using autoware::freespace_planning_algorithms::PlannerCommonParam; +using autoware::freespace_planning_algorithms::RRTStarParam; struct StartPlannerDebugData { @@ -134,4 +134,4 @@ struct StartPlannerParameters } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp similarity index 80% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp index 6aa5584807d90..c252a8f22eda5 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/debug.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/debug.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ -#include "behavior_path_start_planner_module/data_structs.hpp" +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" #include #include @@ -36,4 +36,4 @@ void updateSafetyCheckDebugData( } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__DEBUG_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp similarity index 60% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp index 561ef337fa68c..0c8eb1850b9d5 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/freespace_pull_out.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ -#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include -#include -#include +#include +#include +#include #include @@ -27,16 +27,16 @@ namespace behavior_path_planner { -using freespace_planning_algorithms::AbstractPlanningAlgorithm; -using freespace_planning_algorithms::AstarSearch; -using freespace_planning_algorithms::RRTStar; +using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::RRTStar; class FreespacePullOut : public PullOutPlannerBase { public: FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); PlannerType getPlannerType() const override { return PlannerType::FREESPACE; } @@ -49,4 +49,4 @@ class FreespacePullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__FREESPACE_PULL_OUT_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp similarity index 65% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp index ccf9e933f9383..510fee8fe3dc6 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/geometric_pull_out.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ #include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include +#include #include @@ -32,15 +32,16 @@ class GeometricPullOut : public PullOutPlannerBase public: explicit GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr lane_departure_checker); + const std::shared_ptr + lane_departure_checker); PlannerType getPlannerType() const override { return PlannerType::GEOMETRIC; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; - std::shared_ptr lane_departure_checker_; + std::shared_ptr lane_departure_checker_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp similarity index 84% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp index ac752918a0e32..a7fcd6d820202 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ #include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "behavior_path_start_planner_module/start_planner_module.hpp" +#include "autoware_behavior_path_start_planner_module/start_planner_module.hpp" #include @@ -53,4 +53,4 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp similarity index 83% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp index f7f443c297c61..5674087cdb3df 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_path.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -34,4 +34,4 @@ struct PullOutPath Pose end_pose{}; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp similarity index 85% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp index f2c437883b8a1..38eeb558e85eb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #include "autoware_behavior_path_planner_common/data_manager.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_start_planner_module/data_structs.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include #include @@ -45,7 +45,7 @@ class PullOutPlannerBase public: explicit PullOutPlannerBase(rclcpp::Node & node, const StartPlannerParameters & parameters) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } @@ -95,11 +95,11 @@ class PullOutPlannerBase collision_check_margin_); }; std::shared_ptr planner_data_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; StartPlannerParameters parameters_; double collision_check_margin_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp similarity index 80% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp index 3bded7ee99fbb..29a74ce0e14ef 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/shift_pull_out.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_planner_base.hpp" -#include +#include #include @@ -27,7 +27,7 @@ namespace behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOut : public PullOutPlannerBase { @@ -61,4 +61,4 @@ class ShiftPullOut : public PullOutPlannerBase }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__SHIFT_PULL_OUT_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp similarity index 93% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp index c8e74009d267a..ccbf1eada7378 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/start_planner_module.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ #include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" @@ -21,15 +21,15 @@ #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/data_structs.hpp" -#include "behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" -#include "behavior_path_start_planner_module/shift_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/data_structs.hpp" +#include "autoware_behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" -#include -#include -#include +#include +#include +#include #include @@ -46,13 +46,13 @@ namespace behavior_path_planner { +using autoware::lane_departure_checker::LaneDepartureChecker; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; using geometry_msgs::msg::PoseArray; -using lane_departure_checker::LaneDepartureChecker; using PriorityOrder = std::vector>>; struct PullOutStatus @@ -255,7 +255,7 @@ class StartPlannerModule : public SceneModuleInterface mutable std::shared_ptr ego_predicted_path_params_; mutable std::shared_ptr objects_filtering_params_; mutable std::shared_ptr safety_check_params_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::vector> start_planners_; PullOutStatus status_; @@ -336,4 +336,4 @@ class StartPlannerModule : public SceneModuleInterface }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp similarity index 89% rename from planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp rename to planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp index 9185873cfd770..b9f0b964ed428 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/autoware_behavior_path_start_planner_module/include/autoware_behavior_path_start_planner_module/util.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ -#define BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ #include "autoware_behavior_path_planner_common/data_manager.hpp" #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_start_planner_module/pull_out_path.hpp" +#include "autoware_behavior_path_start_planner_module/pull_out_path.hpp" #include @@ -55,4 +55,4 @@ std::optional extractCollisionCheckSection( const PullOutPath & path, const double collision_check_distance_from_end); } // namespace behavior_path_planner::start_planner_utils -#endif // BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/autoware_behavior_path_start_planner_module/package.xml similarity index 89% rename from planning/behavior_path_start_planner_module/package.xml rename to planning/autoware_behavior_path_start_planner_module/package.xml index 23c57b6d6024a..e4d6de4324bfe 100644 --- a/planning/behavior_path_start_planner_module/package.xml +++ b/planning/autoware_behavior_path_start_planner_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_start_planner_module + autoware_behavior_path_start_planner_module 0.1.0 - The behavior_path_start_planner_module package + The autoware_behavior_path_start_planner_module package Kosuke Takeuchi Kyoichi Sugahara @@ -21,11 +21,11 @@ eigen3_cmake_module autoware_behavior_path_planner_common + autoware_rtc_interface behavior_path_planner motion_utils pluginlib rclcpp - rtc_interface tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_start_planner_module/plugins.xml b/planning/autoware_behavior_path_start_planner_module/plugins.xml similarity index 70% rename from planning/behavior_path_start_planner_module/plugins.xml rename to planning/autoware_behavior_path_start_planner_module/plugins.xml index fde8b673be5bc..92498d5158ae5 100644 --- a/planning/behavior_path_start_planner_module/plugins.xml +++ b/planning/autoware_behavior_path_start_planner_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_path_start_planner_module/src/debug.cpp b/planning/autoware_behavior_path_start_planner_module/src/debug.cpp similarity index 94% rename from planning/behavior_path_start_planner_module/src/debug.cpp rename to planning/autoware_behavior_path_start_planner_module/src/debug.cpp index 0f95470d86d40..4b4aa7c386925 100644 --- a/planning/behavior_path_start_planner_module/src/debug.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/debug.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/debug.hpp" +#include "autoware_behavior_path_start_planner_module/debug.hpp" namespace behavior_path_planner { diff --git a/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp similarity index 94% rename from planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp index d29047d23fffd..1b154138d427e 100644 --- a/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/freespace_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/freespace_pull_out.hpp" #include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include @@ -28,10 +28,10 @@ namespace behavior_path_planner { FreespacePullOut::FreespacePullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) : PullOutPlannerBase{node, parameters}, velocity_{parameters.freespace_planner_velocity} { - freespace_planning_algorithms::VehicleShape vehicle_shape( + autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); if (parameters.freespace_planner_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp similarity index 95% rename from planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp index 18d02ade489e8..d299b3f4799b2 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/geometric_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/geometric_pull_out.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -32,7 +32,8 @@ using start_planner_utils::getPullOutLanes; GeometricPullOut::GeometricPullOut( rclcpp::Node & node, const StartPlannerParameters & parameters, - const std::shared_ptr lane_departure_checker) + const std::shared_ptr + lane_departure_checker) : PullOutPlannerBase{node, parameters}, parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_(lane_departure_checker) diff --git a/planning/behavior_path_start_planner_module/src/manager.cpp b/planning/autoware_behavior_path_start_planner_module/src/manager.cpp similarity index 99% rename from planning/behavior_path_start_planner_module/src/manager.cpp rename to planning/autoware_behavior_path_start_planner_module/src/manager.cpp index d5731f54224b6..23cca1ef05190 100644 --- a/planning/behavior_path_start_planner_module/src/manager.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/manager.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/manager.hpp" +#include "autoware_behavior_path_start_planner_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp similarity index 99% rename from planning/behavior_path_start_planner_module/src/shift_pull_out.cpp rename to planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp index 298510126a4c3..d0a7b827a2285 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/shift_pull_out.hpp" +#include "autoware_behavior_path_start_planner_module/shift_pull_out.hpp" #include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp similarity index 99% rename from planning/behavior_path_start_planner_module/src/start_planner_module.cpp rename to planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp index 82a4709c6b5be..2efd2bc17e6ab 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/start_planner_module.hpp" +#include "autoware_behavior_path_start_planner_module/start_planner_module.hpp" #include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_start_planner_module/debug.hpp" -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/debug.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -60,12 +60,12 @@ StartPlannerModule::StartPlannerModule( objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, is_freespace_planner_cb_running_{false} { lane_departure_checker_ = std::make_shared(); lane_departure_checker_->setVehicleInfo(vehicle_info_); - lane_departure_checker::Param lane_departure_checker_params; + autoware::lane_departure_checker::Param lane_departure_checker_params; lane_departure_checker_params.footprint_extra_margin = parameters->lane_departure_check_expansion_margin; diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/autoware_behavior_path_start_planner_module/src/util.cpp similarity index 98% rename from planning/behavior_path_start_planner_module/src/util.cpp rename to planning/autoware_behavior_path_start_planner_module/src/util.cpp index b1bcf59db86cb..51a6093936154 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/autoware_behavior_path_start_planner_module/src/util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_start_planner_module/util.hpp" +#include "autoware_behavior_path_start_planner_module/util.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index a36a5c73729cb..5a47bed4c712a 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -23,6 +23,8 @@ autoware_behavior_path_planner_common autoware_perception_msgs + autoware_rtc_interface + autoware_vehicle_info_utils behavior_path_planner geometry_msgs lanelet2_extension @@ -32,7 +34,6 @@ pluginlib rclcpp route_handler - rtc_interface sensor_msgs signal_processing tf2 @@ -41,7 +42,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 767c1d21dfc90..9972263cb4fd2 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -1208,32 +1208,45 @@ bool StaticObstacleAvoidanceModule::isValidShiftLine( } } + const auto is_return_shift = + [](const double start_shift_length, const double end_shift_length, const double threshold) { + return std::abs(start_shift_length) > threshold && std::abs(end_shift_length) < threshold; + }; + // check if the vehicle is in road. (yaw angle is not considered) { const auto minimum_distance = 0.5 * planner_data_->parameters.vehicle_width + parameters_->hard_drivable_bound_margin - parameters_->max_deviation_from_lane; - const size_t start_idx = shift_lines.front().start_idx; - const size_t end_idx = shift_lines.back().end_idx; + for (const auto & shift_line : shift_lines) { + const size_t start_idx = shift_line.start_idx; + const size_t end_idx = shift_line.end_idx; - const auto path = shifter_for_validate.getReferencePath(); - const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); - const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); - for (size_t i = start_idx; i <= end_idx; ++i) { - const auto p = getPoint(path.points.at(i)); - lanelet::BasicPoint2d basic_point{p.x, p.y}; - - const auto shift_length = proposed_shift_path.shift_length.at(i); - const auto THRESHOLD = minimum_distance + std::abs(shift_length); + if (is_return_shift( + shift_line.start_shift_length, shift_line.end_shift_length, + parameters_->lateral_small_shift_threshold)) { + continue; + } - if ( - boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < - THRESHOLD) { - RCLCPP_DEBUG_THROTTLE( - getLogger(), *clock_, 1000, - "following latest new shift line may cause deviation from drivable area."); - return false; + const auto path = shifter_for_validate.getReferencePath(); + const auto left_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.left_bound)); + const auto right_bound = lanelet::utils::to2D(toLineString3d(avoid_data_.right_bound)); + for (size_t i = start_idx; i <= end_idx; ++i) { + const auto p = getPoint(path.points.at(i)); + lanelet::BasicPoint2d basic_point{p.x, p.y}; + + const auto shift_length = proposed_shift_path.shift_length.at(i); + const auto THRESHOLD = minimum_distance + std::abs(shift_length); + + if ( + boost::geometry::distance(basic_point, (shift_length > 0.0 ? left_bound : right_bound)) < + THRESHOLD) { + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 1000, + "following latest new shift line may cause deviation from drivable area."); + return false; + } } } } diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 8671c71430b3d..1408c096c7e4e 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -138,7 +138,7 @@ double calcSignedArcLengthToFirstNearestPoint( } geometry_msgs::msg::Polygon createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info, const double offset) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double offset) { const auto & i = vehicle_info; const auto & front_m = i.max_longitudinal_offset_m; diff --git a/planning/behavior_velocity_intersection_module/CMakeLists.txt b/planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt similarity index 93% rename from planning/behavior_velocity_intersection_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt index c02b5a3852722..f3b31001978e9 100644 --- a/planning/behavior_velocity_intersection_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_intersection_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_intersection_module) +project(autoware_behavior_velocity_intersection_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/autoware_behavior_velocity_intersection_module/README.md similarity index 100% rename from planning/behavior_velocity_intersection_module/README.md rename to planning/autoware_behavior_velocity_intersection_module/README.md diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml similarity index 100% rename from planning/behavior_velocity_intersection_module/config/intersection.param.yaml rename to planning/autoware_behavior_velocity_intersection_module/config/intersection.param.yaml diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/data-structure.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-ll-rr.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-lr-rl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention-straight.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/intersection-topology.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/merge_from_private.png b/planning/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/merge_from_private.png rename to planning/autoware_behavior_velocity_intersection_module/docs/merge_from_private.png diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png b/planning/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png rename to planning/autoware_behavior_velocity_intersection_module/docs/static-occlusion-timeout.png diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/traffic-light-specific-behavior.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/ttc.gif b/planning/autoware_behavior_velocity_intersection_module/docs/ttc.gif similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/ttc.gif rename to planning/autoware_behavior_velocity_intersection_module/docs/ttc.gif diff --git a/planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/upstream-velocity.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg b/planning/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg similarity index 100% rename from planning/behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg rename to planning/autoware_behavior_velocity_intersection_module/docs/yield-stuck.drawio.svg diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/autoware_behavior_velocity_intersection_module/package.xml similarity index 83% rename from planning/behavior_velocity_intersection_module/package.xml rename to planning/autoware_behavior_velocity_intersection_module/package.xml index 3694a395b3a53..5ce1ff206a1a9 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/autoware_behavior_velocity_intersection_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_intersection_module + autoware_behavior_velocity_intersection_module 0.1.0 - The behavior_velocity_intersection_module package + The autoware_behavior_velocity_intersection_module package Mamoru Sobue Takayuki Murooka @@ -18,9 +18,11 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common + autoware_rtc_interface + autoware_vehicle_info_utils fmt geometry_msgs interpolation @@ -32,11 +34,9 @@ pluginlib rclcpp route_handler - rtc_interface tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_behavior_velocity_intersection_module/plugins.xml b/planning/autoware_behavior_velocity_intersection_module/plugins.xml new file mode 100644 index 0000000000000..d30fbb46e050e --- /dev/null +++ b/planning/autoware_behavior_velocity_intersection_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/autoware_behavior_velocity_intersection_module/scripts/ttc.py similarity index 100% rename from planning/behavior_velocity_intersection_module/scripts/ttc.py rename to planning/autoware_behavior_velocity_intersection_module/scripts/ttc.py diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/debug.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/debug.cpp index 252d7c2a9f61e..ab09ca2e6e746 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "scene_merge_from_private_road.hpp" -#include -#include +#include +#include #include #include @@ -45,7 +45,7 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = behavior_velocity_planner::planning_utils::bitShift(lane_id); + int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -224,7 +224,7 @@ constexpr std::tuple light_blue() } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -472,7 +472,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); - int32_t uid = behavior_velocity_planner::planning_utils::bitShift(module_id_); + int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( @@ -496,4 +496,4 @@ motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.cpp b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/decision_result.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp index 7ed896d1b4b55..5d1f1a1fcfca2 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp @@ -14,7 +14,7 @@ #include "decision_result.hpp" -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { std::string formatDecisionResult(const DecisionResult & decision_result) { @@ -65,4 +65,4 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return ""; } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/decision_result.hpp b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/decision_result.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp index 5f642db3a462d..26f78616a7b61 100644 --- a/planning/behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp @@ -19,7 +19,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -172,6 +172,6 @@ using DecisionResult = std::variant< std::string formatDecisionResult(const DecisionResult & decision_result); -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // DECISION_RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp index b6ae84aa8488b..d8bada002e37b 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -43,6 +43,6 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERPOLATED_PATH_INFO_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index 555ea424dfef0..be7e9e25cc8bc 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { void IntersectionLanelets::update( @@ -79,4 +79,4 @@ void IntersectionLanelets::update( } } } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp similarity index 98% rename from planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 9624d375de122..a7ca5eb3b0bc1 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -190,6 +190,6 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the // path }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERSECTION_LANELETS_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 99d79d4468b38..98325f0aa8735 100644 --- a/planning/behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -17,7 +17,7 @@ #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -72,6 +72,6 @@ struct IntersectionStopLines */ size_t occlusion_wo_tl_pass_judge_line{0}; }; -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // INTERSECTION_STOPLINES_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp similarity index 98% rename from planning/behavior_velocity_intersection_module/src/manager.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/manager.cpp index ac32ad553d2b7..8f540603791cb 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include -#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -589,11 +589,12 @@ bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegi return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::IntersectionModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::IntersectionModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::MergeFromPrivateModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp similarity index 90% rename from planning/behavior_velocity_intersection_module/src/manager.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/manager.hpp index 7bbc8d51bbe9e..ecf6bb810f313 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -18,9 +18,9 @@ #include "scene_intersection.hpp" #include "scene_merge_from_private_road.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -90,6 +90,6 @@ class MergeFromPrivateModulePlugin : public PluginWrapper ObjectInfoManager::registerObject( const bool belong_intersection_area, const bool is_parked_vehicle) { if (objects_info_.count(uuid) == 0) { - auto object = std::make_shared(uuid); + auto object = std::make_shared(uuid); objects_info_[uuid] = object; } auto object = objects_info_[uuid]; @@ -219,7 +219,7 @@ std::shared_ptr ObjectInfoManager::registerObject( void ObjectInfoManager::registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, const bool belong_intersection_area, const bool is_parked_vehicle, - std::shared_ptr object) + std::shared_ptr object) { objects_info_[uuid] = object; if (belong_attention_area) { @@ -249,7 +249,7 @@ std::vector> ObjectInfoManager::allObjects() const return all_objects; } -std::optional findPassageInterval( +std::optional findPassageInterval( const autoware_perception_msgs::msg::PredictedPath & predicted_path, const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, @@ -284,25 +284,25 @@ std::optional findPassageInterval( if (lanelet::geometry::inside( first_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return intersection::CollisionInterval::LanePosition::FIRST; + return CollisionInterval::LanePosition::FIRST; } } if (second_attention_lane_opt) { if (lanelet::geometry::inside( second_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return intersection::CollisionInterval::LanePosition::SECOND; + return CollisionInterval::LanePosition::SECOND; } } - return intersection::CollisionInterval::LanePosition::ELSE; + return CollisionInterval::LanePosition::ELSE; }(); std::vector path; for (const auto & pose : predicted_path.path) { path.push_back(pose); } - return intersection::CollisionInterval{ + return CollisionInterval{ lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/object_manager.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp index 180496bd6b18d..0ba9947fdb318 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp @@ -52,7 +52,7 @@ struct hash }; } // namespace std -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { /** @@ -234,8 +234,7 @@ class ObjectInfoManager void registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, - const bool belong_intersection_area, const bool is_parked, - std::shared_ptr object); + const bool belong_intersection_area, const bool is_parked, std::shared_ptr object); void clearObjects(); @@ -282,12 +281,12 @@ class ObjectInfoManager /** * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically */ -std::optional findPassageInterval( +std::optional findPassageInterval( const autoware_perception_msgs::msg::PredictedPath & predicted_path, const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, const std::optional & second_attention_lane_opt); -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // OBJECT_MANAGER_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/result.hpp b/planning/autoware_behavior_velocity_intersection_module/src/result.hpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/result.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/result.hpp index 5d82183cee2fb..1a833b268a5d8 100644 --- a/planning/behavior_velocity_intersection_module/src/result.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/result.hpp @@ -18,7 +18,7 @@ #include #include -namespace behavior_velocity_planner::intersection +namespace autoware::behavior_velocity_planner { template @@ -48,6 +48,6 @@ Result make_err(Args &&... args) return Result(Error{std::forward(args)...}); } -} // namespace behavior_velocity_planner::intersection +} // namespace autoware::behavior_velocity_planner #endif // RESULT_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp similarity index 92% rename from planning/behavior_velocity_intersection_module/src/scene_intersection.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index c27483ea1fa51..beed0aab8e382 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,8 +16,8 @@ #include "util.hpp" -#include // for toGeomPoly -#include +#include // for toGeomPoly +#include #include #include #include @@ -38,13 +38,10 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using intersection::make_err; -using intersection::make_ok; -using intersection::Result; using motion_utils::VelocityFactorInterface; IntersectionModule::IntersectionModule( @@ -105,8 +102,8 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * prev_decision_result_ = decision_result; { - const std::string decision_type = "intersection" + std::to_string(module_id_) + " : " + - intersection::formatDecisionResult(decision_result); + const std::string decision_type = + "intersection" + std::to_string(module_id_) + " : " + formatDecisionResult(decision_result); internal_debug_data_.decision_type = decision_type; } @@ -147,7 +144,7 @@ static std::string formatOcclusionType(const IntersectionModule::OcclusionType & return "RTCOccluded"; } -intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( +DecisionResult IntersectionModule::modifyPathVelocityDetail( PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { const auto prepare_data = prepareIntersectionData(path); @@ -187,18 +184,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // ego path has just entered the entry of this intersection // ========================================================================================== if (!intersection_lanelets.first_attention_area()) { - return intersection::InternalError{"attention area is empty"}; + return InternalError{"attention area is empty"}; } const auto first_attention_area = intersection_lanelets.first_attention_area().value(); const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; if (!default_stopline_idx_opt) { - return intersection::InternalError{"default stop line is null"}; + return InternalError{"default stop line is null"}; } const auto default_stopline_idx = default_stopline_idx_opt.value(); const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { - return intersection::InternalError{"occlusion stop line is null"}; + return InternalError{"occlusion stop line is null"}; } const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); @@ -275,9 +272,9 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto closest_idx = intersection_stoplines.closest_idx; const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); - return intersection::OverPassJudge{safety_diag, evasive_diag}; + return OverPassJudge{safety_diag, evasive_diag}; } - return intersection::OverPassJudge{ + return OverPassJudge{ "no collision is detected", "ego can safely pass the intersection at this rate"}; } @@ -288,8 +285,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( // ========================================================================================== std::string safety_report = safety_diag; if (const bool collision_on_1st_attention_lane = - has_collision && - (collision_position == intersection::CollisionInterval::LanePosition::FIRST); + has_collision && (collision_position == CollisionInterval::LanePosition::FIRST); is_over_1st_pass_judge_line && is_over_2nd_pass_judge_line.has_value() && !is_over_2nd_pass_judge_line.value() && collision_on_1st_attention_lane) { safety_report += @@ -329,19 +325,18 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (is_prioritized) { - return intersection::FullyPrioritized{ + return FullyPrioritized{ has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx, safety_report}; } // Safe if (!is_occlusion_state && !has_collision_with_margin) { - return intersection::Safe{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; + return Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Only collision if (!is_occlusion_state && has_collision_with_margin) { - return intersection::NonOccludedCollisionStop{ + return NonOccludedCollisionStop{ closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // Occluded @@ -400,16 +395,16 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto closest_idx = intersection_stoplines.closest_idx; const std::string evasive_diag = generateEgoRiskEvasiveDiagnosis( *path, closest_idx, time_distance_array, too_late_detect_objects, misjudge_objects); - return intersection::OverPassJudge{ + return OverPassJudge{ "already passed maximum peeking line in the absence of traffic light.\n" + safety_report, evasive_diag}; } - return intersection::OverPassJudge{ + return OverPassJudge{ "already passed maximum peeking line in the absence of traffic light safely", "no evasive action required"}; } - return intersection::OccludedAbsenceTrafficLight{ + return OccludedAbsenceTrafficLight{ is_occlusion_cleared_with_margin, has_collision_with_margin, temporal_stop_before_attention_required, @@ -439,8 +434,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool release_static_occlusion_stuck = (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); if (!has_collision_with_margin && release_static_occlusion_stuck) { - return intersection::Safe{ - closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; + return Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx, occlusion_diag}; } // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED const double max_timeout = @@ -453,7 +447,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( occlusion_stop_state_machine_.getDuration()) : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); if (has_collision_with_margin) { - return intersection::OccludedCollisionStop{ + return OccludedCollisionStop{ is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, @@ -463,7 +457,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( static_occlusion_timeout, occlusion_diag}; } else { - return intersection::PeekingTowardOcclusion{ + return PeekingTowardOcclusion{ is_occlusion_cleared_with_margin, temporal_stop_before_attention_required, closest_idx, @@ -478,7 +472,7 @@ intersection::DecisionResult IntersectionModule::modifyPathVelocityDetail( (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? first_attention_stopline_idx : occlusion_stopline_idx; - return intersection::FirstWaitBeforeOcclusion{ + return FirstWaitBeforeOcclusion{ is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline, occlusion_diag}; } @@ -505,7 +499,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::InternalError & result, + [[maybe_unused]] const InternalError & result, [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -515,7 +509,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - [[maybe_unused]] const intersection::OverPassJudge & result, + [[maybe_unused]] const OverPassJudge & result, [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] bool * default_safety, [[maybe_unused]] double * default_distance, [[maybe_unused]] bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) @@ -525,7 +519,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + const StuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -545,9 +539,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::YieldStuckStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, [[maybe_unused]] double * occlusion_distance) + const YieldStuckStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + [[maybe_unused]] double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop"); const auto closest_idx = result.closest_idx; @@ -560,9 +554,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::NonOccludedCollisionStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const NonOccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); const auto closest_idx = result.closest_idx; @@ -579,9 +573,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::FirstWaitBeforeOcclusion & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const FirstWaitBeforeOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); const auto closest_idx = result.closest_idx; @@ -598,9 +592,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::PeekingTowardOcclusion & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const PeekingTowardOcclusion & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); const auto closest_idx = result.closest_idx; @@ -617,9 +611,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::OccludedAbsenceTrafficLight & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const OccludedAbsenceTrafficLight & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); const auto closest_idx = result.closest_idx; @@ -634,9 +628,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::OccludedCollisionStop & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const OccludedCollisionStop & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); const auto closest_idx = result.closest_idx; @@ -653,9 +647,8 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); const auto closest_idx = result.closest_idx; @@ -672,9 +665,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::FullyPrioritized & result, - const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const FullyPrioritized & result, const tier4_planning_msgs::msg::PathWithLaneId & path, + bool * default_safety, double * default_distance, bool * occlusion_safety, + double * occlusion_distance) { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FullyPrioritized"); const auto closest_idx = result.closest_idx; @@ -690,8 +683,7 @@ void prepareRTCByDecisionResult( } void IntersectionModule::prepareRTCStatus( - const intersection::DecisionResult & decision_result, - const tier4_planning_msgs::msg::PathWithLaneId & path) + const DecisionResult & decision_result, const tier4_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; double default_distance = std::numeric_limits::lowest(); @@ -705,7 +697,7 @@ void IntersectionModule::prepareRTCStatus( setSafe(default_safety); setDistance(default_distance); occlusion_first_stop_required_ = - std::holds_alternative(decision_result); + std::holds_alternative(decision_result); } template @@ -723,7 +715,7 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::InternalError & decision_result, + [[maybe_unused]] const InternalError & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, @@ -738,7 +730,7 @@ template <> void reactRTCApprovalByDecisionResult( [[maybe_unused]] const bool rtc_default_approved, [[maybe_unused]] const bool rtc_occlusion_approved, - [[maybe_unused]] const intersection::OverPassJudge & decision_result, + [[maybe_unused]] const OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, @@ -752,7 +744,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::StuckStop & decision_result, + const StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -799,7 +791,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::YieldStuckStop & decision_result, + const YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -832,7 +824,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::NonOccludedCollisionStop & decision_result, + const NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -876,7 +868,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::FirstWaitBeforeOcclusion & decision_result, + const FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) @@ -927,7 +919,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::PeekingTowardOcclusion & decision_result, + const PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) @@ -983,7 +975,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::OccludedCollisionStop & decision_result, + const OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1031,7 +1023,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::OccludedAbsenceTrafficLight & decision_result, + const OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1084,8 +1076,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( - const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::Safe & decision_result, + const bool rtc_default_approved, const bool rtc_occlusion_approved, const Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1128,7 +1119,7 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const intersection::FullyPrioritized & decision_result, + const FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, @@ -1170,8 +1161,8 @@ void reactRTCApprovalByDecisionResult( } void IntersectionModule::reactRTCApproval( - const intersection::DecisionResult & decision_result, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( @@ -1293,7 +1284,7 @@ void IntersectionModule::updateTrafficSignalObservation() IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, - const intersection::IntersectionStopLines & intersection_stoplines) + const IntersectionStopLines & intersection_stoplines) { const auto & current_pose = planner_data_->current_odometry->pose; const auto closest_idx = intersection_stoplines.closest_idx; @@ -1346,12 +1337,11 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat // as "was_safe" // ========================================================================================== const bool was_safe = [&]() { - if (std::holds_alternative(prev_decision_result_)) { + if (std::holds_alternative(prev_decision_result_)) { return true; } - if (std::holds_alternative(prev_decision_result_)) { - const auto & state = - std::get(prev_decision_result_); + if (std::holds_alternative(prev_decision_result_)) { + const auto & state = std::get(prev_decision_result_); return !state.collision_detected; } return false; @@ -1422,4 +1412,4 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp similarity index 89% rename from planning/behavior_velocity_intersection_module/src/scene_intersection.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b86fd77491f54..e25f41a18028a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,8 +22,8 @@ #include "object_manager.hpp" #include "result.hpp" -#include -#include +#include +#include #include #include @@ -43,7 +43,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class IntersectionModule : public SceneModuleInterface @@ -295,11 +295,9 @@ class IntersectionModule : public SceneModuleInterface BLAME_AT_SECOND_PASS_JUDGE, }; const bool collision_detected; - const intersection::CollisionInterval::LanePosition collision_position; - const std::vector>> - too_late_detect_objects; - const std::vector>> - misjudge_objects; + const CollisionInterval::LanePosition collision_position; + const std::vector>> too_late_detect_objects; + const std::vector>> misjudge_objects; }; IntersectionModule( @@ -377,7 +375,7 @@ class IntersectionModule : public SceneModuleInterface */ //! cache IntersectionLanelets struct - std::optional intersection_lanelets_{std::nullopt}; + std::optional intersection_lanelets_{std::nullopt}; //! cache discretized occlusion detection lanelets std::optional> occlusion_attention_divisions_{ @@ -400,7 +398,7 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; //! for checking if ego is over the pass judge lines because previously the situation was SAFE - intersection::DecisionResult prev_decision_result_{intersection::InternalError{""}}; + DecisionResult prev_decision_result_{InternalError{""}}; //! flag if ego passed the 1st_pass_judge_line while peeking. If this is true, 1st_pass_judge_line //! is treated as the same position as occlusion_peeking_stopline @@ -427,7 +425,7 @@ class IntersectionModule : public SceneModuleInterface StateMachine collision_state_machine_; //! container for storing safety status of objects on the attention area - intersection::ObjectInfoManager object_info_manager_; + ObjectInfoManager object_info_manager_; /** @} */ private: @@ -506,21 +504,20 @@ class IntersectionModule : public SceneModuleInterface /** * @brief analyze traffic_light/occupancy/objects context and return DecisionResult */ - intersection::DecisionResult modifyPathVelocityDetail( - PathWithLaneId * path, StopReason * stop_reason); + DecisionResult modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); /** * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const intersection::DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); + const DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); /** * @brief act based on current RTC approval */ void reactRTCApproval( - const intersection::DecisionResult & decision_result, - tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + const DecisionResult & decision_result, tier4_planning_msgs::msg::PathWithLaneId * path, + StopReason * stop_reason); /** @}*/ private: @@ -536,9 +533,9 @@ class IntersectionModule : public SceneModuleInterface */ struct BasicData { - intersection::InterpolatedPathInfo interpolated_path_info; - intersection::IntersectionStopLines intersection_stoplines; - intersection::PathLanelets path_lanelets; + InterpolatedPathInfo interpolated_path_info; + IntersectionStopLines intersection_stoplines; + PathLanelets path_lanelets; }; /** @@ -549,31 +546,30 @@ class IntersectionModule : public SceneModuleInterface * * To simplify modifyPathVelocityDetail(), this function is used at first */ - intersection::Result prepareIntersectionData( - PathWithLaneId * path); + Result prepareIntersectionData(PathWithLaneId * path); /** * @brief find the associated stopline road marking of assigned lanelet */ std::optional getStopLineIndexFromMap( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) const; /** * @brief generate IntersectionStopLines */ - std::optional generateIntersectionStopLines( + std::optional generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, tier4_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets */ - intersection::IntersectionLanelets generateObjectiveLanelets( + IntersectionLanelets generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet) const; @@ -581,9 +577,9 @@ class IntersectionModule : public SceneModuleInterface /** * @brief generate PathLanelets */ - std::optional generatePathLanelets( + std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, @@ -637,10 +633,9 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_, * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() */ - std::optional isStuckStatus( + std::optional isStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::IntersectionStopLines & intersection_stoplines, - const intersection::PathLanelets & path_lanelets) const; + const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const; bool isTargetStuckVehicleType( const autoware_perception_msgs::msg::PredictedObject & object) const; @@ -651,7 +646,7 @@ class IntersectionModule : public SceneModuleInterface /** * @brief check stuck */ - bool checkStuckVehicleInIntersection(const intersection::PathLanelets & path_lanelets) const; + bool checkStuckVehicleInIntersection(const PathLanelets & path_lanelets) const; /** @} */ private: @@ -667,16 +662,16 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_, * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline */ - std::optional isYieldStuckStatus( + std::optional isYieldStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines) const; + const InterpolatedPathInfo & interpolated_path_info, + const IntersectionStopLines & intersection_stoplines) const; /** * @brief check yield stuck */ bool checkYieldStuckVehicleInIntersection( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const; /** @} */ @@ -698,15 +693,14 @@ class IntersectionModule : public SceneModuleInterface bool /* reconciled occlusion disapproval */> getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info); + const InterpolatedPathInfo & interpolated_path_info); /** * @brief calculate detected occlusion status(NOT | STATICALLY | DYNAMICALLY) * @attention this function has access to value() of intersection_lanelets_, * intersection_lanelets.first_attention_area(), occlusion_attention_divisions_ */ - OcclusionType detectOcclusion( - const intersection::InterpolatedPathInfo & interpolated_path_info) const; + OcclusionType detectOcclusion(const InterpolatedPathInfo & interpolated_path_info) const; /** @} */ private: @@ -726,7 +720,7 @@ class IntersectionModule : public SceneModuleInterface */ PassJudgeStatus isOverPassJudgeLinesStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, - const intersection::IntersectionStopLines & intersection_stoplines); + const IntersectionStopLines & intersection_stoplines); /** @} */ private: @@ -751,7 +745,7 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of intersection_lanelets_ */ void updateObjectInfoManagerCollision( - const intersection::PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, + const PathLanelets & path_lanelets, const TimeDistanceArray & time_distance_array, const TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, tier4_debug_msgs::msg::Float64MultiArrayStamped * object_ttc_time_array); @@ -768,20 +762,18 @@ class IntersectionModule : public SceneModuleInterface * @attention this function has access to value() of * intersection_stoplines.occlusion_peeking_stopline */ - std::optional isGreenPseudoCollisionStatus( + std::optional isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines) const; + const IntersectionStopLines & intersection_stoplines) const; /** * @brief generate the message explaining why too_late_detect_objects/misjudge_objects exist and * blame past perception fault */ std::string generateDetectionBlameDiagnosis( - const std::vector< - std::pair>> & + const std::vector>> & too_late_detect_objects, - const std::vector< - std::pair>> & + const std::vector>> & misjudge_objects) const; /** @@ -791,11 +783,9 @@ class IntersectionModule : public SceneModuleInterface std::string generateEgoRiskEvasiveDiagnosis( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const TimeDistanceArray & ego_time_distance_array, - const std::vector< - std::pair>> & + const std::vector>> & too_late_detect_objects, - const std::vector< - std::pair>> & + const std::vector>> & misjudge_objects) const; /** @@ -818,7 +808,7 @@ class IntersectionModule : public SceneModuleInterface */ TimeDistanceArray calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, - const intersection::IntersectionStopLines & intersection_stoplines, + const IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const; /** @} */ @@ -828,6 +818,6 @@ class IntersectionModule : public SceneModuleInterface rclcpp::Publisher::SharedPtr object_ttc_pub_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_INTERSECTION_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 131081c5e8ca0..fb06ed9d811c8 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly -#include // for smoothPath +#include // for toGeomPoly +#include // for smoothPath #include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -140,7 +140,7 @@ void IntersectionModule::updateObjectInfoManagerArea() } void IntersectionModule::updateObjectInfoManagerCollision( - const intersection::PathLanelets & path_lanelets, + const PathLanelets & path_lanelets, const IntersectionModule::TimeDistanceArray & time_distance_array, const IntersectionModule::TrafficPrioritizedLevel & traffic_prioritized_level, const bool passed_1st_judge_line_first_time, const bool passed_2nd_judge_line_first_time, @@ -243,8 +243,8 @@ void IntersectionModule::updateObjectInfoManagerCollision( // path, both will be null, which is interpreted as SAFE. if any of the path is "normal", either // of them has value, not both // ========================================================================================== - std::optional unsafe_interval{std::nullopt}; - std::optional safe_interval{std::nullopt}; + std::optional unsafe_interval{std::nullopt}; + std::optional safe_interval{std::nullopt}; std::optional> object_debug_info{std::nullopt}; for (const auto & predicted_path_ptr : sorted_predicted_paths) { @@ -256,7 +256,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( } cutPredictPathWithinDuration( planner_data_->predicted_objects->header.stamp, passing_time, &predicted_path); - const auto object_passage_interval_opt = intersection::findPassageInterval( + const auto object_passage_interval_opt = findPassageInterval( predicted_path, predicted_object.shape, ego_poly, intersection_lanelets.first_attention_lane(), intersection_lanelets.second_attention_lane()); @@ -362,27 +362,25 @@ void IntersectionModule::updateObjectInfoManagerCollision( } object_info->update_safety(unsafe_interval, safe_interval, safe_under_traffic_control); if (passed_1st_judge_line_first_time) { - object_info->setDecisionAt1stPassJudgeLinePassage(intersection::CollisionKnowledge{ + object_info->setDecisionAt1stPassJudgeLinePassage(CollisionKnowledge{ clock_->now(), // stamp unsafe_interval - ? intersection::CollisionKnowledge::SafeType::UNSAFE - : (safe_under_traffic_control - ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL - : intersection::CollisionKnowledge::SafeType::SAFE), // safe - unsafe_interval ? unsafe_interval : safe_interval, // interval + ? CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control ? CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval predicted_object.kinematics.initial_twist_with_covariance.twist.linear .x // observed_velocity }); } if (passed_2nd_judge_line_first_time) { - object_info->setDecisionAt2ndPassJudgeLinePassage(intersection::CollisionKnowledge{ + object_info->setDecisionAt2ndPassJudgeLinePassage(CollisionKnowledge{ clock_->now(), // stamp unsafe_interval - ? intersection::CollisionKnowledge::SafeType::UNSAFE - : (safe_under_traffic_control - ? intersection::CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL - : intersection::CollisionKnowledge::SafeType::SAFE), // safe - unsafe_interval ? unsafe_interval : safe_interval, // interval + ? CollisionKnowledge::SafeType::UNSAFE + : (safe_under_traffic_control ? CollisionKnowledge::SafeType::SAFE_UNDER_TRAFFIC_CONTROL + : CollisionKnowledge::SafeType::SAFE), // safe + unsafe_interval ? unsafe_interval : safe_interval, // interval predicted_object.kinematics.initial_twist_with_covariance.twist.linear .x // observed_velocity }); @@ -415,10 +413,9 @@ void IntersectionModule::cutPredictPathWithinDuration( } } -std::optional -IntersectionModule::isGreenPseudoCollisionStatus( +std::optional IntersectionModule::isGreenPseudoCollisionStatus( const size_t closest_idx, const size_t collision_stopline_idx, - const intersection::IntersectionStopLines & intersection_stoplines) const + const IntersectionStopLines & intersection_stoplines) const { // ========================================================================================== // if there are any vehicles on the attention area when ego entered the intersection on green @@ -440,7 +437,7 @@ IntersectionModule::isGreenPseudoCollisionStatus( }); if (exist_close_vehicles) { const auto occlusion_stopline_idx = intersection_stoplines.occlusion_peeking_stopline.value(); - return intersection::NonOccludedCollisionStop{ + return NonOccludedCollisionStop{ closest_idx, collision_stopline_idx, occlusion_stopline_idx, std::string("")}; } } @@ -448,11 +445,11 @@ IntersectionModule::isGreenPseudoCollisionStatus( } std::string IntersectionModule::generateDetectionBlameDiagnosis( - const std::vector>> & + const std::vector< + std::pair>> & too_late_detect_objects, - const std::vector>> & + const std::vector< + std::pair>> & misjudge_objects) const { std::string diag; @@ -598,11 +595,11 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const IntersectionModule::TimeDistanceArray & ego_time_distance_array, - const std::vector>> & + const std::vector< + std::pair>> & too_late_detect_objects, - [[maybe_unused]] const std::vector>> & + [[maybe_unused]] const std::vector< + std::pair>> & misjudge_objects) const { static constexpr double min_vel = 1e-2; @@ -684,9 +681,8 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( // that case is both "too late to stop" and "too late to go" for the planner. and basically // detection side is responsible for this fault // ========================================================================================== - std::vector>> - misjudge_objects; - std::vector>> + std::vector>> misjudge_objects; + std::vector>> too_late_detect_objects; for (const auto & object_info : object_info_manager_.attentionObjects()) { if (object_info->is_safe_under_traffic_control()) { @@ -709,14 +705,14 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( // visualized as "misjudge" // ========================================================================================== auto * debug_container = &debug_data_.unsafe_targets.objects; - if (unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + if (unsafe_info.lane_position == CollisionInterval::LanePosition::FIRST) { collision_at_first_lane = true; } else { collision_at_non_first_lane = true; } if ( is_over_1st_pass_judge_line && - unsafe_info.lane_position == intersection::CollisionInterval::LanePosition::FIRST) { + unsafe_info.lane_position == CollisionInterval::LanePosition::FIRST) { const auto & decision_at_1st_pass_judge_opt = object_info->decision_at_1st_pass_judge_line_passage(); if (!decision_at_1st_pass_judge_opt) { @@ -725,9 +721,7 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container = &debug_data_.too_late_detect_targets.objects; } else { const auto & decision_at_1st_pass_judge = decision_at_1st_pass_judge_opt.value(); - if ( - decision_at_1st_pass_judge.safe_type != - intersection::CollisionKnowledge::SafeType::UNSAFE) { + if (decision_at_1st_pass_judge.safe_type != CollisionKnowledge::SafeType::UNSAFE) { misjudge_objects.emplace_back( CollisionStatus::BlameType::BLAME_AT_FIRST_PASS_JUDGE, object_info); debug_container = &debug_data_.misjudge_targets.objects; @@ -747,9 +741,7 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container = &debug_data_.too_late_detect_targets.objects; } else { const auto & decision_at_2nd_pass_judge = decision_at_2nd_pass_judge_opt.value(); - if ( - decision_at_2nd_pass_judge.safe_type != - intersection::CollisionKnowledge::SafeType::UNSAFE) { + if (decision_at_2nd_pass_judge.safe_type != CollisionKnowledge::SafeType::UNSAFE) { misjudge_objects.emplace_back( CollisionStatus::BlameType::BLAME_AT_SECOND_PASS_JUDGE, object_info); debug_container = &debug_data_.misjudge_targets.objects; @@ -763,12 +755,11 @@ IntersectionModule::CollisionStatus IntersectionModule::detectCollision( debug_container->emplace_back(object_info->predicted_object()); } if (collision_at_first_lane) { - return { - true, intersection::CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; + return {true, CollisionInterval::FIRST, too_late_detect_objects, misjudge_objects}; } else if (collision_at_non_first_lane) { - return {true, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + return {true, CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } - return {false, intersection::CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; + return {false, CollisionInterval::ELSE, too_late_detect_objects, misjudge_objects}; } std::optional IntersectionModule::checkAngleForTargetLanelets( @@ -811,7 +802,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, - const intersection::IntersectionStopLines & intersection_stoplines, + const IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { const double intersection_velocity = @@ -973,4 +964,4 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin return time_distance_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp similarity index 98% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index b741d43bb025a..c43f8617897b9 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -25,7 +25,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -34,7 +34,7 @@ std::tuple< bool /* reconciled occlusion disapproval */> IntersectionModule::getOcclusionStatus( const TrafficPrioritizedLevel & traffic_prioritized_level, - const intersection::InterpolatedPathInfo & interpolated_path_info) + const InterpolatedPathInfo & interpolated_path_info) { const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); @@ -99,7 +99,7 @@ IntersectionModule::getOcclusionStatus( } IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( - const intersection::InterpolatedPathInfo & interpolated_path_info) const + const InterpolatedPathInfo & interpolated_path_info) const { const auto & intersection_lanelets = intersection_lanelets_.value(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); @@ -449,4 +449,4 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( debug_data_.static_occlusion = true; return StaticallyOccluded{min_dist}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 9ea5360c3a176..223e8eca84fe8 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for to_bg2d -#include // for planning_utils:: +#include // for to_bg2d +#include // for planning_utils:: #include #include // for lanelet::autoware::RoadMarking #include @@ -106,7 +106,7 @@ std::optional> getFirstPoi const auto & p = path.points.at(i).point.pose.position; for (const auto & polygon : polygons) { const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + is_in_lanelet = bg::within(autoware::behavior_velocity_planner::to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional>( i, polygon); @@ -122,7 +122,7 @@ std::optional> getFirstPoi const auto & p = path.points.at(i).point.pose.position; for (const auto & polygon : polygons) { const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + is_in_lanelet = bg::within(autoware::behavior_velocity_planner::to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional>( i, polygon); @@ -157,16 +157,12 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using intersection::make_err; -using intersection::make_ok; -using intersection::Result; - -Result -IntersectionModule::prepareIntersectionData(PathWithLaneId * path) +Result IntersectionModule::prepareIntersectionData( + PathWithLaneId * path) { const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); @@ -191,13 +187,12 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) const auto interpolated_path_info_opt = util::generateInterpolatedPath( lane_id_, associative_ids_, *path, planner_param_.common.path_interpolation_ds, logger_); if (!interpolated_path_info_opt) { - return make_err( - "splineInterpolate failed"); + return make_err("splineInterpolate failed"); } const auto & interpolated_path_info = interpolated_path_info_opt.value(); if (!interpolated_path_info.lane_id_interval) { - return make_err( + return make_err( "Path has no interval on intersection lane " + std::to_string(lane_id_)); } @@ -231,8 +226,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) const auto & first_conflicting_lane_opt = intersection_lanelets.first_conflicting_lane(); if (conflicting_lanelets.empty() || !first_conflicting_area_opt || !first_conflicting_lane_opt) { // this is abnormal - return make_err( - "conflicting area is empty"); + return make_err("conflicting area is empty"); } const auto & first_conflicting_lane = first_conflicting_lane_opt.value(); const auto & first_conflicting_area = first_conflicting_area_opt.value(); @@ -250,7 +244,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) assigned_lanelet, first_conflicting_area, dummy_first_attention_lane, second_attention_area_opt, interpolated_path_info, path); if (!intersection_stoplines_opt) { - return make_err( + return make_err( "failed to generate intersection_stoplines"); } const auto & intersection_stoplines = intersection_stoplines_opt.value(); @@ -265,7 +259,7 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) lanelets_on_path, interpolated_path_info, first_conflicting_area, conflicting_area, first_attention_area_opt, intersection_lanelets.attention_area(), closest_idx); if (!path_lanelets_opt.has_value()) { - return make_err( + return make_err( "failed to generate PathLanelets"); } const auto & path_lanelets = path_lanelets_opt.value(); @@ -294,13 +288,12 @@ IntersectionModule::prepareIntersectionData(PathWithLaneId * path) } } - return make_ok( + return make_ok( interpolated_path_info, intersection_stoplines, path_lanelets); } std::optional IntersectionModule::getStopLineIndexFromMap( - const intersection::InterpolatedPathInfo & interpolated_path_info, - lanelet::ConstLanelet assigned_lanelet) const + const InterpolatedPathInfo & interpolated_path_info, lanelet::ConstLanelet assigned_lanelet) const { const auto & path = interpolated_path_info.path; const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); @@ -350,12 +343,11 @@ std::optional IntersectionModule::getStopLineIndexFromMap( planner_data_->ego_nearest_yaw_threshold); } -std::optional -IntersectionModule::generateIntersectionStopLines( +std::optional IntersectionModule::generateIntersectionStopLines( lanelet::ConstLanelet assigned_lanelet, const lanelet::CompoundPolygon3d & first_conflicting_area, const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, tier4_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; @@ -555,7 +547,7 @@ IntersectionModule::generateIntersectionStopLines( intersection_stoplines_temp.default_stopline; } - intersection::IntersectionStopLines intersection_stoplines; + IntersectionStopLines intersection_stoplines; intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; if (stuck_stopline_valid) { intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; @@ -585,7 +577,7 @@ IntersectionModule::generateIntersectionStopLines( return intersection_stoplines; } -intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets( +IntersectionLanelets IntersectionModule::generateObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet) const { @@ -736,7 +728,7 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets auto [attention_lanelets, original_attention_lanelet_sequences] = util::mergeLaneletsByTopologicalSort(detection_and_preceding_lanelets, routing_graph_ptr); - intersection::IntersectionLanelets result; + IntersectionLanelets result; result.attention_ = std::move(attention_lanelets); for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that @@ -786,9 +778,9 @@ intersection::IntersectionLanelets IntersectionModule::generateObjectiveLanelets return result; } -std::optional IntersectionModule::generatePathLanelets( +std::optional IntersectionModule::generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, @@ -804,7 +796,7 @@ std::optional IntersectionModule::generatePathLanele const auto assigned_lane_interval = assigned_lane_interval_opt.value(); const auto & path = interpolated_path_info.path; - intersection::PathLanelets path_lanelets; + PathLanelets path_lanelets; // prev path_lanelets.prev = ::getPrevLanelets(lanelets_on_path, associative_ids_); path_lanelets.all = path_lanelets.prev; @@ -915,4 +907,4 @@ std::vector IntersectionModule::generateDetectionLan return detection_divisions; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp similarity index 94% rename from planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index 498a902c032db..eecb02d07c386 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -15,7 +15,7 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly +#include // for toGeomPoly #include #include @@ -114,14 +114,13 @@ lanelet::ConstLanelet createLaneletFromArcLength( } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -std::optional IntersectionModule::isStuckStatus( +std::optional IntersectionModule::isStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::IntersectionStopLines & intersection_stoplines, - const intersection::PathLanelets & path_lanelets) const + const IntersectionStopLines & intersection_stoplines, const PathLanelets & path_lanelets) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { @@ -162,8 +161,7 @@ std::optional IntersectionModule::isStuckStatus( } } if (stopline_idx) { - return intersection::StuckStop{ - closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; + return StuckStop{closest_idx, stopline_idx.value(), occlusion_peeking_stopline_idx_opt}; } } } @@ -230,8 +228,7 @@ bool IntersectionModule::isTargetYieldStuckVehicleType( return false; } -bool IntersectionModule::checkStuckVehicleInIntersection( - const intersection::PathLanelets & path_lanelets) const +bool IntersectionModule::checkStuckVehicleInIntersection(const PathLanelets & path_lanelets) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; @@ -306,10 +303,10 @@ bool IntersectionModule::checkStuckVehicleInIntersection( return false; } -std::optional IntersectionModule::isYieldStuckStatus( +std::optional IntersectionModule::isYieldStuckStatus( const tier4_planning_msgs::msg::PathWithLaneId & path, - const intersection::InterpolatedPathInfo & interpolated_path_info, - const intersection::IntersectionStopLines & intersection_stoplines) const + const InterpolatedPathInfo & interpolated_path_info, + const IntersectionStopLines & intersection_stoplines) const { const auto closest_idx = intersection_stoplines.closest_idx; auto fromEgoDist = [&](const size_t index) { @@ -342,14 +339,14 @@ std::optional IntersectionModule::isYieldStuckStat } } if (stopline_idx) { - return intersection::YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; + return YieldStuckStop{closest_idx, stopline_idx.value(), std::string("")}; } } return std::nullopt; } bool IntersectionModule::checkYieldStuckVehicleInIntersection( - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const lanelet::ConstLanelets & attention_lanelets) const { const bool yield_stuck_detection_direction = [&]() { @@ -425,4 +422,4 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( } return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp similarity index 96% rename from planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 67da3c7a759fe..1aaed779e1b79 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,8 +16,8 @@ #include "util.hpp" -#include -#include +#include +#include #include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -52,7 +52,7 @@ MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( static std::optional getFirstConflictingLanelet( const lanelet::ConstLanelets & conflicting_lanelets, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -206,4 +206,4 @@ lanelet::ConstLanelets MergeFromPrivateRoadModule::getAttentionLanelets() const return attention_lanelets; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp similarity index 91% rename from planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 0b783cf2a7ebd..19e9ea44869ea 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,8 +15,8 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include -#include +#include +#include #include #include @@ -35,7 +35,7 @@ * lanes before entering intersection */ -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class MergeFromPrivateRoadModule : public SceneModuleInterface { @@ -88,6 +88,6 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/util.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/util.cpp index 9c492e7a64cde..09f16bcada3c1 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -16,9 +16,9 @@ #include "interpolated_path_info.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -40,7 +40,7 @@ #include #include -namespace behavior_velocity_planner::util +namespace autoware::behavior_velocity_planner::util { namespace bg = boost::geometry; @@ -129,8 +129,7 @@ std::optional> findLaneIdsInterval( } std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -154,7 +153,7 @@ std::optional> getFirstPointInsidePolygonsByFootprint( const std::vector & polygons, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length) { const auto & path_ip = interpolated_path_info.path; @@ -356,12 +355,12 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { - intersection::InterpolatedPathInfo interpolated_path_info; + InterpolatedPathInfo interpolated_path_info; if (!splineInterpolate(input_path, ds, interpolated_path_info.path, logger)) { return std::nullopt; } @@ -400,4 +399,4 @@ std::vector getPolygon3dFromLanelets( return polys; } -} // namespace behavior_velocity_planner::util +} // namespace autoware::behavior_velocity_planner::util diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp similarity index 93% rename from planning/behavior_velocity_intersection_module/src/util.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/util.hpp index 878253e6943a7..ef826380afa69 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner::util +namespace autoware::behavior_velocity_planner::util { /** @@ -98,7 +98,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); /** * @brief interpolate PathWithLaneId */ -std::optional generateInterpolatedPath( +std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); @@ -124,8 +124,7 @@ mergeLaneletsByTopologicalSort( * polygon */ std::optional getFirstPointInsidePolygonByFootprint( - const lanelet::CompoundPolygon3d & polygon, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const lanelet::CompoundPolygon3d & polygon, const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); /** @@ -136,12 +135,12 @@ std::optional> getFirstPointInsidePolygonsByFootprint( const std::vector & polygons, - const intersection::InterpolatedPathInfo & interpolated_path_info, + const InterpolatedPathInfo & interpolated_path_info, const tier4_autoware_utils::LinearRing2d & footprint, const double vehicle_length); std::vector getPolygon3dFromLanelets( const lanelet::ConstLanelets & ll_vec); -} // namespace behavior_velocity_planner::util +} // namespace autoware::behavior_velocity_planner::util #endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt b/planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt similarity index 93% rename from planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt index 8b327291a60f1..4e53e7bd1790b 100644 --- a/planning/behavior_velocity_occlusion_spot_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_occlusion_spot_module) +project(autoware_behavior_velocity_occlusion_spot_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_occlusion_spot_module/README.md b/planning/autoware_behavior_velocity_occlusion_spot_module/README.md similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/README.md rename to planning/autoware_behavior_velocity_occlusion_spot_module/README.md diff --git a/planning/behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml b/planning/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml rename to planning/autoware_behavior_velocity_occlusion_spot_module/config/occlusion_spot.param.yaml diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/behavior_after_safe_margin.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/collision_free.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/da.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/da.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/da.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/maximum_slowdown_velocity.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/object_info_partition.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/occlusion_spot.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/raycast_shadow.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/safe_motion.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg b/planning/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg similarity index 100% rename from planning/behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg rename to planning/autoware_behavior_velocity_occlusion_spot_module/docs/velocity_planning.drawio.svg diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml similarity index 84% rename from planning/behavior_velocity_occlusion_spot_module/package.xml rename to planning/autoware_behavior_velocity_occlusion_spot_module/package.xml index 5817c2203cc5d..d8177ddc908f3 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_occlusion_spot_module + autoware_behavior_velocity_occlusion_spot_module 0.1.0 - The behavior_velocity_occlusion_spot_module package + The autoware_behavior_velocity_occlusion_spot_module package Taiki Tanaka Tomoya Kimura @@ -16,9 +16,10 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common + autoware_vehicle_info_utils geometry_msgs grid_map_ros grid_map_utils @@ -34,7 +35,6 @@ tf2 tf2_geometry_msgs tier4_autoware_utils - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml b/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml new file mode 100644 index 0000000000000..53a2c460c2e44 --- /dev/null +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp similarity index 97% rename from planning/behavior_velocity_occlusion_spot_module/src/debug.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp index 1170afc490a75..40e5b8f91e6b9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -15,8 +15,8 @@ #include "occlusion_spot_utils.hpp" #include "scene_occlusion_spot.hpp" -#include -#include +#include +#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -227,4 +227,4 @@ motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp similarity index 98% rename from planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index c8adb324b9055..6ab88baf77b20 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -20,7 +20,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace grid_utils { @@ -39,7 +39,7 @@ Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) // std::cout << boost::geometry::wkt(line_poly) << std::endl; // std::cout << boost::geometry::wkt(line) << std::endl; - bg::correct(line_poly); + boost::geometry::correct(line_poly); return line_poly; } @@ -142,7 +142,7 @@ Polygon2d generateOccupancyPolygon(const nav_msgs::msg::MapMetaData & info, cons poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, r, r, 0).position)); poly.outer().emplace_back(to_bg2d(calcOffsetPose(info.origin, 0, r, 0).position)); - bg::correct(poly); + boost::geometry::correct(poly); return poly; } @@ -381,4 +381,4 @@ void denoiseOccupancyGridCV( grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); } } // namespace grid_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp similarity index 92% rename from planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 710f671df609f..8dc59d07173dc 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -15,8 +15,8 @@ #ifndef GRID_UTILS_HPP_ #define GRID_UTILS_HPP_ -#include -#include +#include +#include #include #include #include @@ -49,7 +49,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -61,6 +61,10 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; using nav_msgs::msg::OccupancyGrid; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; + namespace occlusion_cost_value { static constexpr unsigned char FREE_SPACE = 0; @@ -118,6 +122,6 @@ void denoiseOccupancyGridCV( grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, const int num_iter, const bool use_object_footprints, const bool use_object_ray_casts); } // namespace grid_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // GRID_UTILS_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp similarity index 93% rename from planning/behavior_velocity_occlusion_spot_module/src/manager.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp index 64adc112aab2f..c1d345cbc9aac 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -16,7 +16,7 @@ #include "scene_occlusion_spot.hpp" -#include +#include #include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; @@ -109,7 +109,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) pp.grid.free_space_max = getOrDeclareParameter(node, ns + ".grid.free_space_max"); pp.grid.occupied_min = getOrDeclareParameter(node, ns + ".grid.occupied_min"); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.baselink_to_front = vehicle_info.max_longitudinal_offset_m; pp.wheel_tread = vehicle_info.wheel_tread_m; pp.right_overhang = vehicle_info.right_overhang_m; @@ -136,8 +136,9 @@ OcclusionSpotModuleManager::getModuleExpiredFunction( return false; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OcclusionSpotModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::OcclusionSpotModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp similarity index 85% rename from planning/behavior_velocity_occlusion_spot_module/src/manager.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp index 0955e4ae9aab5..123d0ef32afc1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -18,9 +18,9 @@ #include "occlusion_spot_utils.hpp" #include "scene_occlusion_spot.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -37,7 +37,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OcclusionSpotModuleManager : public SceneModuleManagerInterface { @@ -63,6 +63,6 @@ class OcclusionSpotModulePlugin : public PluginWrapper -#include +#include +#include #include #include #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; namespace occlusion_spot_utils @@ -485,4 +485,4 @@ std::optional generateOneNotableCollisionFromOcclusionSpo } } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp similarity index 98% rename from planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 2b6d89680cd37..542c869919339 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -17,7 +17,7 @@ #include "grid_utils.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -246,6 +246,6 @@ bool generatePossibleCollisionsFromGridMap( DebugData & debug_data); } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OCCLUSION_SPOT_UTILS_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp similarity index 95% rename from planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index 96f497ca47afa..9fe06273ea617 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -16,12 +16,12 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace occlusion_spot_utils { @@ -98,4 +98,4 @@ SafeMotion calculateSafeMotion(const Velocity & v, const double ttv) return sm; } } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp similarity index 93% rename from planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 8d7e9d2fdedd5..9def3b0938998 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -22,7 +22,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace occlusion_spot_utils { @@ -38,6 +38,6 @@ void applySafeVelocityConsideringPossibleCollision( SafeMotion calculateSafeMotion(const Velocity & v, const double ttv); } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // RISK_PREDICTIVE_BRAKING_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp similarity index 94% rename from planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 1f5ca1bab904e..e5c5eaec346ff 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -17,10 +17,10 @@ #include "occlusion_spot_utils.hpp" #include "risk_predictive_braking.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -41,7 +41,7 @@ namespace { -namespace utils = behavior_velocity_planner::occlusion_spot_utils; +namespace utils = autoware::behavior_velocity_planner::occlusion_spot_utils; using autoware_perception_msgs::msg::PredictedObject; std::vector extractStuckVehicle( const std::vector & vehicles, const double stop_velocity) @@ -56,7 +56,7 @@ std::vector extractStuckVehicle( } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace utils = occlusion_spot_utils; @@ -201,4 +201,4 @@ bool OcclusionSpotModule::modifyPathVelocity( return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp similarity index 89% rename from planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index b7da7c073cbd9..00126a2ebc39a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -17,8 +17,8 @@ #include "occlusion_spot_utils.hpp" -#include -#include +#include +#include #include #include @@ -37,7 +37,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OcclusionSpotModule : public SceneModuleInterface { @@ -70,6 +70,6 @@ class OcclusionSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp similarity index 93% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index bc526959386e9..a21e0333082a9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -15,7 +15,7 @@ #include "grid_utils.hpp" #include "utils.hpp" -#include +#include #include #include @@ -38,11 +38,11 @@ struct indexEq } }; -using behavior_velocity_planner::LineString2d; -using behavior_velocity_planner::Point2d; -using behavior_velocity_planner::Polygon2d; -using behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; -using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; +using autoware::behavior_velocity_planner::LineString2d; +using autoware::behavior_velocity_planner::Point2d; +using autoware::behavior_velocity_planner::Polygon2d; +using autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; +using autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; namespace bg = boost::geometry; Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp similarity index 87% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 05e73855f2642..60ef5d62a5b7b 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -16,8 +16,8 @@ #include "occlusion_spot_utils.hpp" #include "utils.hpp" -#include -#include +#include +#include #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" @@ -32,8 +32,9 @@ using tier4_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { - using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils:: + calcSlowDownPointsForPossibleCollision; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; @@ -65,8 +66,9 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) { - using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils:: + calcSlowDownPointsForPossibleCollision; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp similarity index 95% rename from planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp index 9b1d92b71a111..4a0071be10b2d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp @@ -23,7 +23,7 @@ TEST(safeMotion, delay_jerk_acceleration) { - namespace utils = behavior_velocity_planner::occlusion_spot_utils; + namespace utils = autoware::behavior_velocity_planner::occlusion_spot_utils; using utils::calculateSafeMotion; /** * @brief check if calculation is correct in below parameter diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp similarity index 90% rename from planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp rename to planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp index f7bb3d4b1ad6f..4b8a7ae3b2f3f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/autoware_behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -49,17 +49,18 @@ inline grid_map::GridMap generateGrid(int w, int h, double res) grid_map::GridMap grid{}; grid_map::Length length(w * res, h * res); grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); - grid.add("layer", behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); + grid.add( + "layer", autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); return grid; } -using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; +using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; inline void generatePossibleCollisions( std::vector & possible_collisions, double x0, double y0, double x, double y, int nb_cols) { - using behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; const double lon = 0.0; // assume col_x = intersection_x const double lat = -1.0; const double velocity = 1.0; diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index af30de556b542..e792860239119 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -11,12 +11,12 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Crosswalk](../behavior_velocity_crosswalk_module/README.md) - [Walkway](../autoware_behavior_velocity_walkway_module/README.md) - [Detection Area](../behavior_velocity_detection_area_module/README.md) -- [Intersection](../behavior_velocity_intersection_module/README.md) +- [Intersection](../autoware_behavior_velocity_intersection_module/README.md) - [MergeFromPrivate](../behavior_velocity_intersection_module/README.md#merge-from-private) -- [Stop Line](../behavior_velocity_stop_line_module/README.md) +- [Stop Line](../autoware_behavior_velocity_stop_line_module/README.md) - [Virtual Traffic Light](../autoware_behavior_velocity_virtual_traffic_light_module/README.md) - [Traffic Light](../behavior_velocity_traffic_light_module/README.md) -- [Occlusion Spot](../behavior_velocity_occlusion_spot_module/README.md) +- [Occlusion Spot](../autoware_behavior_velocity_occlusion_spot_module/README.md) - [No Stopping Area](../behavior_velocity_no_stopping_area_module/README.md) - [Run Out](../autoware_behavior_velocity_run_out_module/README.md) - [Speed Bump](../behavior_velocity_speed_bump_module/README.md) diff --git a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index d4478f87f0610..f52d9637c2134 100644 --- a/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/autoware_behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -24,7 +24,6 @@ - @@ -68,7 +67,6 @@ - diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index 576c4e92887cb..dcec42f80691f 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -34,11 +34,11 @@ rosidl_default_generators + autoware_behavior_velocity_planner_common autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_velocity_smoother - behavior_velocity_planner_common diagnostic_msgs eigen geometry_msgs @@ -65,20 +65,19 @@ ament_cmake_ros ament_lint_auto + autoware_behavior_velocity_intersection_module + autoware_behavior_velocity_occlusion_spot_module autoware_behavior_velocity_run_out_module + autoware_behavior_velocity_stop_line_module autoware_behavior_velocity_virtual_traffic_light_module autoware_behavior_velocity_walkway_module autoware_lint_common behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module behavior_velocity_detection_area_module - behavior_velocity_intersection_module behavior_velocity_no_drivable_lane_module behavior_velocity_no_stopping_area_module - behavior_velocity_occlusion_spot_module - behavior_velocity_out_of_lane_module behavior_velocity_speed_bump_module - behavior_velocity_stop_line_module behavior_velocity_traffic_light_module diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index ffbc4ef9174dc..3ff4e813b3266 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -14,8 +14,8 @@ #include "node.hpp" +#include #include -#include #include #include #include @@ -310,7 +310,7 @@ void BehaviorVelocityPlannerNode::onParam() // constructed. It would be required if it was a callback. std::lock_guard // lock(mutex_); planner_data_.velocity_smoother_ = - std::make_unique(*this); + std::make_unique(*this); planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } @@ -439,14 +439,14 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( // screening const auto filtered_path = - ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + autoware::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + const auto interpolated_path_msg = autoware::behavior_velocity_planner::interpolatePath( filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); + output_path_msg = autoware::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index 62ceef5f04ea6..b2157bfb818ce 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -49,7 +49,6 @@ namespace autoware::behavior_velocity_planner using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; -using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index f462fc963f17b..93209a10180be 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -50,7 +50,8 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") +: plugin_loader_( + "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") { } diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index 9e7f2942bb067..73193a002918d 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -15,8 +15,8 @@ #ifndef PLANNER_MANAGER_HPP_ #define PLANNER_MANAGER_HPP_ -#include -#include +#include +#include #include #include @@ -38,8 +38,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::PluginInterface; class BehaviorVelocityPlannerManager { diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index b21fd6cc0f168..58e7a86e01302 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -68,21 +68,20 @@ std::shared_ptr generateNode() }; std::vector module_names; - module_names.emplace_back("behavior_velocity_planner::CrosswalkModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::TrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::IntersectionModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::MergeFromPrivateModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::BlindSpotModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::DetectionAreaModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); + module_names.emplace_back("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("behavior_velocity_planner::SpeedBumpModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::OutOfLaneModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::NoDrivableLaneModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -100,15 +99,14 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config_no_prefix("crosswalk"), get_behavior_velocity_module_config("walkway"), get_behavior_velocity_module_config_no_prefix("detection_area"), - get_behavior_velocity_module_config_no_prefix("intersection"), + get_behavior_velocity_module_config("intersection"), get_behavior_velocity_module_config_no_prefix("no_stopping_area"), - get_behavior_velocity_module_config_no_prefix("occlusion_spot"), + get_behavior_velocity_module_config("occlusion_spot"), get_behavior_velocity_module_config("run_out"), get_behavior_velocity_module_config_no_prefix("speed_bump"), - get_behavior_velocity_module_config_no_prefix("stop_line"), + get_behavior_velocity_module_config("stop_line"), get_behavior_velocity_module_config_no_prefix("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config_no_prefix("out_of_lane"), get_behavior_velocity_module_config_no_prefix("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt similarity index 93% rename from planning/behavior_velocity_planner_common/CMakeLists.txt rename to planning/autoware_behavior_velocity_planner_common/CMakeLists.txt index c8847164851e8..9cb992312f52a 100644 --- a/planning/behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_planner_common) +project(autoware_behavior_velocity_planner_common) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_planner_common/README.md b/planning/autoware_behavior_velocity_planner_common/README.md similarity index 100% rename from planning/behavior_velocity_planner_common/README.md rename to planning/autoware_behavior_velocity_planner_common/README.md diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp similarity index 88% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp index 51511b94f3e33..df5d083f5dcb1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #include "route_handler/route_handler.hpp" +#include +#include #include -#include -#include #include #include @@ -45,13 +45,13 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class BehaviorVelocityPlannerNode; struct PlannerData { explicit PlannerData(rclcpp::Node & node) - : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { max_stop_acceleration_threshold = node.declare_parameter( "max_accel"); // TODO(someone): read min_acc in velocity_controller.param.yaml? @@ -88,11 +88,11 @@ struct PlannerData bool is_simulation = false; // velocity smoother - std::shared_ptr velocity_smoother_; + std::shared_ptr velocity_smoother_; // route handler std::shared_ptr route_handler_; // parameters - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; // additional parameters double max_stop_acceleration_threshold; @@ -148,6 +148,6 @@ struct PlannerData return std::make_optional(traffic_light_id_map.at(id)); } }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp similarity index 75% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp index dcdb4a7052cc0..86579f06790b2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class PluginInterface @@ -38,6 +38,6 @@ class PluginInterface virtual const char * getModuleName() = 0; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp similarity index 78% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp index abb14dd8b2356..e82211937e55b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { template @@ -48,6 +48,6 @@ class PluginWrapper : public PluginInterface std::unique_ptr scene_manager_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp similarity index 94% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp index 3e7992207f3f1..a97346297c9ac 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ -#include +#include +#include #include #include #include #include -#include #include #include @@ -45,15 +45,15 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using autoware::rtc_interface::RTCInterface; using builtin_interfaces::msg::Time; using motion_utils::PlanningBehavior; using motion_utils::VelocityFactor; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; -using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; @@ -276,6 +276,6 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface } }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp similarity index 94% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp index c9d292536ac13..5ea1ae9fffcc1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#include +#include #include #include @@ -27,7 +27,7 @@ #define EIGEN_MPL2_ONLY #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -195,6 +195,6 @@ std::optional createTargetPoint( const size_t lane_id, const double margin, const double vehicle_offset); } // namespace arc_lane_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp similarity index 88% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index bf238ecad55cb..22bba2b90dcc2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #include @@ -46,7 +46,7 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -77,6 +77,6 @@ Polygon2d upScalePolygon( geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp similarity index 86% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp index ab44af265fbaa..c6e5d45ec4eb6 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace debug { @@ -47,5 +47,5 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); } // namespace debug -} // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp similarity index 78% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp index 55a82db1ae390..e9bccc42e1dc4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include @@ -22,7 +22,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, @@ -33,6 +33,6 @@ autoware_planning_msgs::msg::Path filterLitterPathPoint( const autoware_planning_msgs::msg::Path & path); autoware_planning_msgs::msg::Path filterStopPathPoint( const autoware_planning_msgs::msg::Path & path); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp similarity index 87% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp index 73b0fa7d553a3..12dd4db930745 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ #include @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** * @brief Manage stop-go states with safety margin time. @@ -92,5 +92,5 @@ class StateMachine std::shared_ptr start_time_; //! first time received GO when STOP state }; -} // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp similarity index 76% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 2aadb7883a857..c5040f055c243 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#include +#include #include #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -40,6 +40,6 @@ bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, const std::shared_ptr & planner_data); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp similarity index 93% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp index 4ef4bb91a295d..2fe728847b8f2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #include @@ -35,7 +35,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { struct DetectionRange { @@ -155,11 +155,13 @@ std::unordered_map, lanelet::ConstLanelet> get std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } for (const auto lane_id : unique_lane_ids) { @@ -239,6 +241,6 @@ lanelet::ConstLanelets getConstLaneletsFromIds( } } // namespace planning_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/autoware_behavior_velocity_planner_common/package.xml similarity index 89% rename from planning/behavior_velocity_planner_common/package.xml rename to planning/autoware_behavior_velocity_planner_common/package.xml index ba847d8b1f853..0e317ceb5db66 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/autoware_behavior_velocity_planner_common/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_planner_common + autoware_behavior_velocity_planner_common 0.1.0 - The behavior_velocity_planner_common package + The autoware_behavior_velocity_planner_common package Tomoya Kimura Shumpei Wakabayashi @@ -23,6 +23,8 @@ autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_rtc_interface + autoware_vehicle_info_utils autoware_velocity_smoother diagnostic_msgs eigen @@ -36,7 +38,6 @@ rclcpp rclcpp_components route_handler - rtc_interface sensor_msgs tf2 tf2_eigen @@ -46,7 +47,6 @@ tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp similarity index 97% rename from planning/behavior_velocity_planner_common/src/scene_module_interface.cpp rename to planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index 3092d33418c8b..94cef496f4f75 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include #include @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::StopWatch; @@ -281,4 +281,4 @@ void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp similarity index 94% rename from planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index e5705b1367e0e..d7e88f5f33901 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -62,7 +62,7 @@ return p * v; */ } // namespace -namespace behavior_velocity_planner::arc_lane_utils +namespace autoware::behavior_velocity_planner::arc_lane_utils { double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) @@ -134,4 +134,4 @@ std::optional createTargetPoint( const auto front_idx = offset_segment->first; return std::make_pair(front_idx, target_pose); } -} // namespace behavior_velocity_planner::arc_lane_utils +} // namespace autoware::behavior_velocity_planner::arc_lane_utils diff --git a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp similarity index 91% rename from planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp index 903cf5aab80e8..50f7cd269a904 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) @@ -63,4 +63,4 @@ geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) } return polygon_msg; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp similarity index 94% rename from planning/behavior_velocity_planner_common/src/utilization/debug.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index 00d746c56db85..a8e6828a048ae 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace debug { @@ -124,4 +124,4 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( return msg; } } // namespace debug -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp similarity index 96% rename from planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index fe956e9be9512..eb17242c06b1e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -23,7 +23,7 @@ constexpr double DOUBLE_EPSILON = 1e-6; -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, @@ -168,4 +168,4 @@ autoware_planning_msgs::msg::Path filterStopPathPoint( } return filtered_path; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp similarity index 91% rename from planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index ecc314bca2009..1dc448bea5b86 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -// #include +// #include #include "motion_utils/trajectory/conversion.hpp" +#include #include -#include #include #include @@ -33,7 +33,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -82,11 +82,11 @@ bool smoothPath( traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp similarity index 97% rename from planning/behavior_velocity_planner_common/src/utilization/util.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index b724d01346f1e..4578676aeeee7 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include @@ -89,7 +89,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace planning_utils { @@ -545,11 +545,13 @@ std::vector getLaneletsOnPath( std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } std::vector lanelets; @@ -692,4 +694,4 @@ std::set getAssociativeIntersectionLanelets( } } // namespace planning_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp similarity index 93% rename from planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index fb2418ba9c829..b835aa0034ec4 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -14,17 +14,18 @@ #include "utils.hpp" -#include -#include +#include +#include #include #include -using PathIndexWithPoint2d = behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; -using LineString2d = behavior_velocity_planner::LineString2d; -using Point2d = behavior_velocity_planner::Point2d; -namespace arc_lane_utils = behavior_velocity_planner::arc_lane_utils; +using PathIndexWithPoint2d = + autoware::behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; +using LineString2d = autoware::behavior_velocity_planner::LineString2d; +using Point2d = autoware::behavior_velocity_planner::Point2d; +namespace arc_lane_utils = autoware::behavior_velocity_planner::arc_lane_utils; namespace { @@ -121,7 +122,7 @@ TEST(findOffsetSegment, case_backward_offset_segment) TEST(checkCollision, various_cases) { - using behavior_velocity_planner::arc_lane_utils::checkCollision; + using autoware::behavior_velocity_planner::arc_lane_utils::checkCollision; constexpr double epsilon = 1e-6; { // normal case with collision diff --git a/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp similarity index 92% rename from planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp index ac7c6655ced87..fe6a1e00496b1 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp @@ -14,7 +14,7 @@ #include "utils.hpp" -#include +#include #include #include @@ -23,8 +23,8 @@ #include #include -using StateMachine = behavior_velocity_planner::StateMachine; -using State = behavior_velocity_planner::StateMachine::State; +using StateMachine = autoware::behavior_velocity_planner::StateMachine; +using State = autoware::behavior_velocity_planner::StateMachine::State; int enumToInt(State s) { diff --git a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp similarity index 92% rename from planning/behavior_velocity_planner_common/test/src/test_utilization.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp index fbc5f5d709c5c..5e6c92b662820 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -15,9 +15,9 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "utils.hpp" -#include -#include -#include +#include +#include +#include #include @@ -35,7 +35,7 @@ TEST(is_ahead_of, nominal) { - using behavior_velocity_planner::planning_utils::isAheadOf; + using autoware::behavior_velocity_planner::planning_utils::isAheadOf; geometry_msgs::msg::Pose target = test::generatePose(0); geometry_msgs::msg::Pose origin = test::generatePose(1); bool is_ahead = isAheadOf(target, origin); @@ -47,7 +47,8 @@ TEST(is_ahead_of, nominal) TEST(smoothDeceleration, calculateMaxSlowDownVelocity) { - using behavior_velocity_planner::planning_utils::calcDecelerationVelocityFromDistanceToTarget; + using autoware::behavior_velocity_planner::planning_utils:: + calcDecelerationVelocityFromDistanceToTarget; const double current_accel = 1.0; const double current_velocity = 5.0; const double max_slow_down_jerk = -1.0; @@ -82,9 +83,9 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity) TEST(specialInterpolation, specialInterpolation) { + using autoware::behavior_velocity_planner::interpolatePath; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; - using behavior_velocity_planner::interpolatePath; using motion_utils::calcSignedArcLength; using motion_utils::searchZeroVelocityIndex; diff --git a/planning/behavior_velocity_planner_common/test/src/utils.hpp b/planning/autoware_behavior_velocity_planner_common/test/src/utils.hpp similarity index 100% rename from planning/behavior_velocity_planner_common/test/src/utils.hpp rename to planning/autoware_behavior_velocity_planner_common/test/src/utils.hpp diff --git a/planning/autoware_behavior_velocity_run_out_module/README.md b/planning/autoware_behavior_velocity_run_out_module/README.md index d3e2d813c3799..acc1260723f2f 100644 --- a/planning/autoware_behavior_velocity_run_out_module/README.md +++ b/planning/autoware_behavior_velocity_run_out_module/README.md @@ -182,7 +182,7 @@ APPROACH --> APPROACH : Approach duration is less than threshold ##### Limit velocity with specified jerk and acc limit The maximum slowdown velocity is calculated in order not to slowdown too much. -See the [Occlusion Spot document](../behavior_velocity_occlusion_spot_module/#maximum-slowdown-velocity) for more details. +See the [Occlusion Spot document](../autoware_behavior_velocity_occlusion_spot_module/#maximum-slowdown-velocity) for more details. You can choose whether to use this feature by parameter of `slow_down_limit.enable`. ### Module Parameters diff --git a/planning/autoware_behavior_velocity_run_out_module/package.xml b/planning/autoware_behavior_velocity_run_out_module/package.xml index cb2c2df58ffe2..284797cbed0a8 100644 --- a/planning/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/autoware_behavior_velocity_run_out_module/package.xml @@ -19,10 +19,11 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils behavior_velocity_crosswalk_module - behavior_velocity_planner_common eigen geometry_msgs libboost-dev @@ -38,7 +39,6 @@ tf2_eigen tf2_ros tier4_autoware_utils - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_behavior_velocity_run_out_module/plugins.xml b/planning/autoware_behavior_velocity_run_out_module/plugins.xml index 1ddcaf8e620e1..0fbaf5091d0f7 100644 --- a/planning/autoware_behavior_velocity_run_out_module/plugins.xml +++ b/planning/autoware_behavior_velocity_run_out_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp index 3c6d475950e7c..c38dd63fadba9 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -26,7 +26,6 @@ #include namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::Polygon2d; using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Int32Stamped; diff --git a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index d204c7ec6f2fc..5e8b0382a07c0 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -32,8 +32,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::Point2d; -using ::behavior_velocity_planner::splineInterpolate; namespace { // create quaternion facing to the nearest trajectory point diff --git a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index b7ed815829a2b..8fb79fb9d8eaa 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -18,9 +18,9 @@ #include "debug.hpp" #include "utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -47,7 +47,6 @@ namespace autoware::behavior_velocity_planner using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; -using ::behavior_velocity_planner::PlannerData; using run_out_utils::DynamicObstacle; using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; @@ -56,7 +55,6 @@ using run_out_utils::PredictedPath; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; -using ::behavior_velocity_planner::Polygons2d; /** * @brief base class for creating dynamic obstacles from multiple types of input diff --git a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp index 5ee50863fd162..4da58522165d6 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -22,14 +22,13 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_autoware_utils::getOrDeclareParameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { // Vehicle Parameters { - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); auto & p = planner_param_.vehicle_param; p.base_to_front = vehicle_info.wheel_base_m + vehicle_info.front_overhang_m; p.base_to_rear = vehicle_info.rear_overhang_m; @@ -203,4 +202,4 @@ void RunOutModuleManager::setDynamicObstacleCreator( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::RunOutModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp index f0c49b99c99e5..8cdb17e21d74b 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -17,17 +17,14 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PluginWrapper; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::SceneModuleManagerInterface; class RunOutModuleManager : public SceneModuleManagerInterface { public: diff --git a/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp index 351ed8e7a09b8..e492bc99be615 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -17,8 +17,8 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include "path_utils.hpp" -#include -#include +#include +#include #include #include #include @@ -37,11 +37,8 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using ::behavior_velocity_planner::PlanningBehavior; using object_recognition_utils::convertLabelToString; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; -using ::behavior_velocity_planner::getCrosswalksOnPath; -using ::behavior_velocity_planner::Polygon2d; +namespace planning_utils = autoware::behavior_velocity_planner::planning_utils; RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, diff --git a/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp index 85da1340a5a5d..e3fd30a4e36ce 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -21,7 +21,7 @@ #include "state_machine.hpp" #include "utils.hpp" -#include +#include #include #include @@ -39,11 +39,6 @@ using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; -using ::behavior_velocity_planner::PathWithLaneId; -using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::Polygon2d; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; class RunOutModule : public SceneModuleInterface { diff --git a/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp index ec1cd460e1bb0..ed745cbb92679 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -35,9 +35,6 @@ #include namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::DetectionRange; -using ::behavior_velocity_planner::PathPointWithLaneId; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace run_out_utils { Polygon2d createBoostPolyFromMsg(const std::vector & input_poly) diff --git a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp index 10f856b257a61..90f8238d0d543 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -17,9 +17,9 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -34,19 +34,17 @@ namespace autoware::behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; +using autoware::vehicle_info_utils::VehicleInfo; 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 ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::Polygons2d; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathWithLaneId; -using vehicle_info_util::VehicleInfo; using PathPointsWithLaneId = std::vector; struct CommonParam { diff --git a/planning/behavior_velocity_stop_line_module/CMakeLists.txt b/planning/autoware_behavior_velocity_stop_line_module/CMakeLists.txt similarity index 86% rename from planning/behavior_velocity_stop_line_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_stop_line_module/CMakeLists.txt index bef98aafe6f75..402eb5e20aa24 100644 --- a/planning/behavior_velocity_stop_line_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_stop_line_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_stop_line_module) +project(autoware_behavior_velocity_stop_line_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_stop_line_module/README.md b/planning/autoware_behavior_velocity_stop_line_module/README.md similarity index 100% rename from planning/behavior_velocity_stop_line_module/README.md rename to planning/autoware_behavior_velocity_stop_line_module/README.md diff --git a/planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml b/planning/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml similarity index 100% rename from planning/behavior_velocity_stop_line_module/config/stop_line.param.yaml rename to planning/autoware_behavior_velocity_stop_line_module/config/stop_line.param.yaml diff --git a/planning/behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg b/planning/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg similarity index 100% rename from planning/behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg rename to planning/autoware_behavior_velocity_stop_line_module/docs/calculate_stop_pose.drawio.svg diff --git a/planning/behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg b/planning/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg similarity index 100% rename from planning/behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg rename to planning/autoware_behavior_velocity_stop_line_module/docs/find_collision_segment.drawio.svg diff --git a/planning/behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg b/planning/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg similarity index 100% rename from planning/behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg rename to planning/autoware_behavior_velocity_stop_line_module/docs/find_offset_segment.drawio.svg diff --git a/planning/behavior_velocity_stop_line_module/docs/keep_stopping.svg b/planning/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg similarity index 100% rename from planning/behavior_velocity_stop_line_module/docs/keep_stopping.svg rename to planning/autoware_behavior_velocity_stop_line_module/docs/keep_stopping.svg diff --git a/planning/behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg b/planning/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg similarity index 100% rename from planning/behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg rename to planning/autoware_behavior_velocity_stop_line_module/docs/node_and_segment.drawio.svg diff --git a/planning/behavior_velocity_stop_line_module/docs/restart.svg b/planning/autoware_behavior_velocity_stop_line_module/docs/restart.svg similarity index 100% rename from planning/behavior_velocity_stop_line_module/docs/restart.svg rename to planning/autoware_behavior_velocity_stop_line_module/docs/restart.svg diff --git a/planning/behavior_velocity_stop_line_module/docs/restart_prevention.svg b/planning/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg similarity index 100% rename from planning/behavior_velocity_stop_line_module/docs/restart_prevention.svg rename to planning/autoware_behavior_velocity_stop_line_module/docs/restart_prevention.svg diff --git a/planning/behavior_velocity_stop_line_module/docs/stop_line.svg b/planning/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg similarity index 100% rename from planning/behavior_velocity_stop_line_module/docs/stop_line.svg rename to planning/autoware_behavior_velocity_stop_line_module/docs/stop_line.svg diff --git a/planning/behavior_velocity_stop_line_module/package.xml b/planning/autoware_behavior_velocity_stop_line_module/package.xml similarity index 86% rename from planning/behavior_velocity_stop_line_module/package.xml rename to planning/autoware_behavior_velocity_stop_line_module/package.xml index c71ecb83098fb..c3d3baa965834 100644 --- a/planning/behavior_velocity_stop_line_module/package.xml +++ b/planning/autoware_behavior_velocity_stop_line_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_stop_line_module + autoware_behavior_velocity_stop_line_module 0.1.0 - The behavior_velocity_stop_line_module package + The autoware_behavior_velocity_stop_line_module package Fumiya Watanabe Zhe Shen @@ -18,8 +18,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/autoware_behavior_velocity_stop_line_module/plugins.xml b/planning/autoware_behavior_velocity_stop_line_module/plugins.xml new file mode 100644 index 0000000000000..6765a4bc1bb13 --- /dev/null +++ b/planning/autoware_behavior_velocity_stop_line_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/autoware_behavior_velocity_stop_line_module/src/debug.cpp similarity index 95% rename from planning/behavior_velocity_stop_line_module/src/debug.cpp rename to planning/autoware_behavior_velocity_stop_line_module/src/debug.cpp index f63bdca5068a2..bfcd5a5828556 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_stop_line_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -24,7 +24,7 @@ #include #endif -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -111,4 +111,4 @@ motion_utils::VirtualWalls StopLineModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/autoware_behavior_velocity_stop_line_module/src/manager.cpp similarity index 94% rename from planning/behavior_velocity_stop_line_module/src/manager.cpp rename to planning/autoware_behavior_velocity_stop_line_module/src/manager.cpp index b37b3b119178c..80f48fa65efaf 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_stop_line_module/src/manager.cpp @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::TrafficSign; using tier4_autoware_utils::getOrDeclareParameter; @@ -102,8 +102,9 @@ StopLineModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::StopLineModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::StopLineModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_stop_line_module/src/manager.hpp b/planning/autoware_behavior_velocity_stop_line_module/src/manager.hpp similarity index 85% rename from planning/behavior_velocity_stop_line_module/src/manager.hpp rename to planning/autoware_behavior_velocity_stop_line_module/src/manager.hpp index af9dbaa083c33..eca5132a75c5f 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_stop_line_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using StopLineWithLaneId = std::pair; @@ -62,6 +62,6 @@ class StopLineModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_stop_line_module/src/scene.cpp b/planning/autoware_behavior_velocity_stop_line_module/src/scene.cpp similarity index 96% rename from planning/behavior_velocity_stop_line_module/src/scene.cpp rename to planning/autoware_behavior_velocity_stop_line_module/src/scene.cpp index 976acd98fe42b..1bf777338e43c 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_stop_line_module/src/scene.cpp @@ -14,14 +14,14 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -177,4 +177,4 @@ geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; return center_point; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_stop_line_module/src/scene.hpp b/planning/autoware_behavior_velocity_stop_line_module/src/scene.hpp similarity index 88% rename from planning/behavior_velocity_stop_line_module/src/scene.hpp rename to planning/autoware_behavior_velocity_stop_line_module/src/scene.hpp index 70c67df623c85..0ae0a885f036f 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_stop_line_module/src/scene.hpp @@ -24,16 +24,16 @@ #define EIGEN_MPL2_ONLY #include #include -#include -#include -#include +#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class StopLineModule : public SceneModuleInterface { @@ -106,6 +106,6 @@ class StopLineModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/autoware_behavior_velocity_template_module/package.xml b/planning/autoware_behavior_velocity_template_module/package.xml index a6bcbf5c34e76..eb6a6668fcee9 100644 --- a/planning/autoware_behavior_velocity_template_module/package.xml +++ b/planning/autoware_behavior_velocity_template_module/package.xml @@ -13,8 +13,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/autoware_behavior_velocity_template_module/src/manager.cpp index 8f6621aaab835..8aaf6be9bbfe9 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.cpp @@ -61,4 +61,4 @@ TemplateModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::TemplateModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/autoware_behavior_velocity_template_module/src/manager.hpp index 8e95f516c337d..b92913b2a95ce 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -37,7 +37,8 @@ namespace autoware::behavior_velocity_planner * * @param node A reference to the ROS node. */ -class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleManagerInterface +class TemplateModuleManager +: public autoware::behavior_velocity_planner::SceneModuleManagerInterface { public: explicit TemplateModuleManager(rclcpp::Node & node); @@ -85,7 +86,7 @@ class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleMan * Velocity Planner. */ class TemplateModulePlugin -: public ::behavior_velocity_planner::PluginWrapper +: public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/autoware_behavior_velocity_template_module/src/scene.hpp index 3ce5ddbd8729d..0fc4e7dd9ae91 100644 --- a/planning/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/scene.hpp @@ -15,7 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include +#include #include #include @@ -23,8 +23,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; using tier4_planning_msgs::msg::PathWithLaneId; class TemplateModule : public SceneModuleInterface diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index cd35d4308c26a..415dcd983a8df 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -17,8 +17,9 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common + autoware_vehicle_info_utils geometry_msgs lanelet2_extension motion_utils @@ -29,7 +30,6 @@ tier4_autoware_utils tier4_planning_msgs tier4_v2x_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml index 2402fc13469b9..2e2abef259778 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 5a32cfd2f74f0..39dca1f8f6303 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ namespace autoware::behavior_velocity_planner { using lanelet::autoware::VirtualTrafficLight; using tier4_autoware_utils::getOrDeclareParameter; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; +namespace planning_utils = autoware::behavior_velocity_planner::planning_utils; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -82,4 +82,4 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index c73bb0d706008..6b0436a4290ac 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -29,9 +29,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PluginWrapper; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::SceneModuleManagerInterface; class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface { public: diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 7d91a954171dd..a87d8867adf7e 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include @@ -25,11 +25,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PlanningBehavior; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::VelocityFactor; -namespace arc_lane_utils = ::behavior_velocity_planner::arc_lane_utils; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { using tier4_autoware_utils::calcDistance2d; @@ -181,7 +176,8 @@ std::optional insertStopVelocityAtCollision( auto insert_point = path->points.at(insert_index); insert_point.point.pose = interpolated_pose; // Insert 0 velocity after stop point or replace velocity with 0 - behavior_velocity_planner::planning_utils::insertVelocity(*path, insert_point, 0.0, insert_index); + autoware::behavior_velocity_planner::planning_utils::insertVelocity( + *path, insert_point, 0.0, insert_index); return insert_index; } } // namespace diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index c83ff4e0607ef..d413a8721c09e 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -15,14 +15,14 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include +#include +#include #include #include #include #include #include #include -#include #include #include @@ -32,10 +32,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PathWithLaneId; -using ::behavior_velocity_planner::Pose; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; class VirtualTrafficLightModule : public SceneModuleInterface { public: diff --git a/planning/autoware_behavior_velocity_walkway_module/package.xml b/planning/autoware_behavior_velocity_walkway_module/package.xml index 59fa84acefcbb..60ef7593ab8ff 100644 --- a/planning/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/autoware_behavior_velocity_walkway_module/package.xml @@ -17,9 +17,10 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_planning_msgs + autoware_vehicle_info_utils behavior_velocity_crosswalk_module - behavior_velocity_planner_common geometry_msgs lanelet2_extension libboost-dev @@ -28,7 +29,6 @@ rclcpp route_handler tier4_autoware_utils - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_behavior_velocity_walkway_module/plugins.xml b/planning/autoware_behavior_velocity_walkway_module/plugins.xml index d7f9e948154c1..9a5803807ae8d 100644 --- a/planning/autoware_behavior_velocity_walkway_module/plugins.xml +++ b/planning/autoware_behavior_velocity_walkway_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp b/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp index d26a47e512bc4..80badf59434da 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene_walkway.hpp" -#include +#include #include #include #include @@ -34,7 +34,6 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { diff --git a/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp index 1adea85788d5b..cf3efe3700a05 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -26,12 +26,8 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleManagerInterface; using lanelet::autoware::Crosswalk; using tier4_autoware_utils::getOrDeclareParameter; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; -using ::behavior_velocity_planner::getCrosswalkIdSetOnPath; -using ::behavior_velocity_planner::getCrosswalksOnPath; WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -109,4 +105,4 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::WalkwayModulePlugin, - ::behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp b/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp index d19bf9c1c1d5f..592181dd3ac75 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_walkway.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -33,9 +33,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PluginWrapper; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface diff --git a/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 236654e598216..73ae5a52e0183 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -14,7 +14,7 @@ #include "scene_walkway.hpp" -#include +#include #include #include @@ -22,19 +22,11 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using ::behavior_velocity_planner::getLinestringIntersects; -using ::behavior_velocity_planner::getPolygonIntersects; -using ::behavior_velocity_planner::getStopLineFromMap; -using ::behavior_velocity_planner::PlanningBehavior; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopFactor; -using ::behavior_velocity_planner::VelocityFactor; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPose; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, diff --git a/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index 1928466400134..e6c8f61ba656e 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -18,7 +18,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include "scene_walkway.hpp" -#include +#include #include #include @@ -33,11 +33,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::DebugData; -using ::behavior_velocity_planner::PathWithLaneId; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; - class WalkwayModule : public SceneModuleInterface { public: diff --git a/planning/costmap_generator/CMakeLists.txt b/planning/autoware_costmap_generator/CMakeLists.txt similarity index 80% rename from planning/costmap_generator/CMakeLists.txt rename to planning/autoware_costmap_generator/CMakeLists.txt index 620a997990001..b8b15a7312324 100644 --- a/planning/costmap_generator/CMakeLists.txt +++ b/planning/autoware_costmap_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(costmap_generator) +project(autoware_costmap_generator) find_package(autoware_cmake REQUIRED) autoware_package() @@ -16,9 +16,9 @@ include_directories( ) ament_auto_add_library(costmap_generator_lib SHARED - nodes/costmap_generator/points_to_costmap.cpp - nodes/costmap_generator/objects_to_costmap.cpp - nodes/costmap_generator/object_map_utils.cpp + nodes/autoware_costmap_generator/points_to_costmap.cpp + nodes/autoware_costmap_generator/objects_to_costmap.cpp + nodes/autoware_costmap_generator/object_map_utils.cpp ) target_link_libraries(costmap_generator_lib ${PCL_LIBRARIES} @@ -33,7 +33,7 @@ if(${PCL_VERSION} GREATER_EQUAL 1.12.1) endif() ament_auto_add_library(costmap_generator_node SHARED - nodes/costmap_generator/costmap_generator_node.cpp + nodes/autoware_costmap_generator/costmap_generator_node.cpp ) target_link_libraries(costmap_generator_node ${PCL_LIBRARIES} @@ -41,7 +41,7 @@ target_link_libraries(costmap_generator_node ) rclcpp_components_register_node(costmap_generator_node - PLUGIN "CostmapGenerator" + PLUGIN "autoware::costmap_generator::CostmapGenerator" EXECUTABLE costmap_generator ) diff --git a/planning/costmap_generator/README.md b/planning/autoware_costmap_generator/README.md similarity index 100% rename from planning/costmap_generator/README.md rename to planning/autoware_costmap_generator/README.md diff --git a/planning/costmap_generator/config/costmap_generator.param.yaml b/planning/autoware_costmap_generator/config/costmap_generator.param.yaml similarity index 100% rename from planning/costmap_generator/config/costmap_generator.param.yaml rename to planning/autoware_costmap_generator/config/costmap_generator.param.yaml diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp similarity index 94% rename from planning/costmap_generator/include/costmap_generator/costmap_generator.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp index 31c8a05cc3c60..093c88541e9c8 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp @@ -42,11 +42,11 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ -#define COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ -#include "costmap_generator/objects_to_costmap.hpp" -#include "costmap_generator/points_to_costmap.hpp" +#include "autoware_costmap_generator/objects_to_costmap.hpp" +#include "autoware_costmap_generator/points_to_costmap.hpp" #include #include @@ -72,6 +72,8 @@ #include #include +namespace autoware::costmap_generator +{ class CostmapGenerator : public rclcpp::Node { public: @@ -197,5 +199,6 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost for final output grid_map::Matrix generateCombinedCostmap(); }; +} // namespace autoware::costmap_generator -#endif // COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/object_map_utils.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp index 282bd6dcc1beb..f4911cc428d36 100644 --- a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp @@ -30,8 +30,8 @@ * */ -#ifndef COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ -#define COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ #include #include @@ -98,4 +98,4 @@ void FillPolygonAreas( } // namespace object_map -#endif // COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp index a88bb97a623e2..aa11a98830dc3 100644 --- a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp @@ -42,8 +42,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ -#define COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ #include @@ -57,6 +57,8 @@ #include +namespace autoware::costmap_generator +{ class ObjectsToCostmap { public: @@ -123,5 +125,5 @@ class ObjectsToCostmap const grid_map::Polygon & polygon, const std::string & gridmap_layer_name, const float score, grid_map::GridMap & objects_costmap); }; - -#endif // COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +} // namespace autoware::costmap_generator +#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp index 3b179d63f1d2a..0e3abbd69ec20 100644 --- a/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp @@ -42,8 +42,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ -#define COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ #include @@ -52,6 +52,9 @@ #include #include +namespace autoware::costmap_generator + +{ class PointsToCostmap { public: @@ -115,5 +118,6 @@ class PointsToCostmap const std::string & gridmap_layer_name, const std::vector>> grid_vec); }; +} // namespace autoware::costmap_generator -#endif // COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ diff --git a/planning/costmap_generator/launch/costmap_generator.launch.xml b/planning/autoware_costmap_generator/launch/costmap_generator.launch.xml similarity index 84% rename from planning/costmap_generator/launch/costmap_generator.launch.xml rename to planning/autoware_costmap_generator/launch/costmap_generator.launch.xml index ba8a71f5f428a..e7c378565dd0f 100644 --- a/planning/costmap_generator/launch/costmap_generator.launch.xml +++ b/planning/autoware_costmap_generator/launch/costmap_generator.launch.xml @@ -6,9 +6,9 @@ - + - + diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 777dfc7a50a6e..4302ac6ada645 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -42,8 +42,8 @@ * OF private_node SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/costmap_generator.hpp" -#include "costmap_generator/object_map_utils.hpp" +#include "autoware_costmap_generator/costmap_generator.hpp" +#include "autoware_costmap_generator/object_map_utils.hpp" #include #include @@ -157,6 +157,8 @@ pcl::PointCloud getTransformedPointCloud( } // namespace +namespace autoware::costmap_generator +{ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) : Node("costmap_generator", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -476,6 +478,7 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) out_gridmap_msg->header = header; pub_costmap_->publish(*out_gridmap_msg); } +} // namespace autoware::costmap_generator #include -RCLCPP_COMPONENTS_REGISTER_NODE(CostmapGenerator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::costmap_generator::CostmapGenerator) diff --git a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp index 6955798be3e8a..4ce452814008c 100644 --- a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp @@ -30,7 +30,7 @@ * */ -#include "costmap_generator/object_map_utils.hpp" +#include "autoware_costmap_generator/object_map_utils.hpp" #include #include diff --git a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp index f6f024fb92a4e..afb2dbb9874c9 100644 --- a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp @@ -42,13 +42,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/objects_to_costmap.hpp" +#include "autoware_costmap_generator/objects_to_costmap.hpp" #include #include #include +namespace autoware::costmap_generator +{ // Constructor ObjectsToCostmap::ObjectsToCostmap() : NUMBER_OF_POINTS(4), @@ -196,3 +198,4 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( return objects_costmap[OBJECTS_COSTMAP_LAYER_]; } +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp similarity index 97% rename from planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp index e875f68973a12..723c27201ac8a 100644 --- a/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp @@ -42,11 +42,14 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/points_to_costmap.hpp" +#include "autoware_costmap_generator/points_to_costmap.hpp" #include #include +namespace autoware::costmap_generator +{ + void PointsToCostmap::initGridmapParam(const grid_map::GridMap & gridmap) { grid_length_x_ = gridmap.getLength().x(); @@ -140,3 +143,5 @@ grid_map::Matrix PointsToCostmap::makeCostmapFromPoints( gridmap_layer_name, grid_vec); return costmap; } + +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/package.xml b/planning/autoware_costmap_generator/package.xml similarity index 92% rename from planning/costmap_generator/package.xml rename to planning/autoware_costmap_generator/package.xml index dc94d74dce8ae..aa1ae80eaf953 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/autoware_costmap_generator/package.xml @@ -1,9 +1,9 @@ - costmap_generator + autoware_costmap_generator 0.1.0 - The costmap_generator package + The autoware_costmap_generator package Kosuke Takeuchi Takamasa Horibe Takayuki Murooka diff --git a/planning/costmap_generator/schema/costmap_generator.schema.json b/planning/autoware_costmap_generator/schema/costmap_generator.schema.json similarity index 100% rename from planning/costmap_generator/schema/costmap_generator.schema.json rename to planning/autoware_costmap_generator/schema/costmap_generator.schema.json diff --git a/planning/costmap_generator/test/test_objects_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp similarity index 97% rename from planning/costmap_generator/test/test_objects_to_costmap.cpp rename to planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp index 15b853d0782aa..8d437865b8793 100644 --- a/planning/costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include +namespace autoware::costmap_generator +{ using LABEL = autoware_perception_msgs::msg::ObjectClassification; class ObjectsToCostMapTest : public ::testing::Test @@ -134,3 +136,4 @@ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) } } } +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/test/test_points_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp similarity index 98% rename from planning/costmap_generator/test/test_points_to_costmap.cpp rename to planning/autoware_costmap_generator/test/test_points_to_costmap.cpp index 2366dffc7fc6f..6c52e2dc6ce9d 100644 --- a/planning/costmap_generator/test/test_points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp @@ -12,9 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include + +namespace autoware::costmap_generator +{ using pointcloud = pcl::PointCloud; class PointsToCostmapTest : public ::testing::Test { @@ -216,3 +219,4 @@ TEST_F(PointsToCostmapTest, TestMakeCostmapFromPoints_invalidPoints_outOfGrid) EXPECT_EQ(nonempty_grid_cell_num, 0); } +} // namespace autoware::costmap_generator diff --git a/planning/freespace_planner/CMakeLists.txt b/planning/autoware_freespace_planner/CMakeLists.txt similarity index 64% rename from planning/freespace_planner/CMakeLists.txt rename to planning/autoware_freespace_planner/CMakeLists.txt index a33c9859afe75..7901be70fbc63 100644 --- a/planning/freespace_planner/CMakeLists.txt +++ b/planning/autoware_freespace_planner/CMakeLists.txt @@ -1,24 +1,24 @@ cmake_minimum_required(VERSION 3.14) -project(freespace_planner) +project(autoware_freespace_planner) find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(freespace_planner_node SHARED - src/freespace_planner/freespace_planner_node.cpp + src/autoware_freespace_planner/freespace_planner_node.cpp ) rclcpp_components_register_node(freespace_planner_node - PLUGIN "freespace_planner::FreespacePlannerNode" + PLUGIN "autoware::freespace_planner::FreespacePlannerNode" EXECUTABLE freespace_planner ) if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - test/test_${PROJECT_NAME}_node_interface.cpp + test/test_freespace_planner_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME}_node + freespace_planner_node ) endif() diff --git a/planning/freespace_planner/README.md b/planning/autoware_freespace_planner/README.md similarity index 99% rename from planning/freespace_planner/README.md rename to planning/autoware_freespace_planner/README.md index 6ed0ab605ea05..6bf13a81c0413 100644 --- a/planning/freespace_planner/README.md +++ b/planning/autoware_freespace_planner/README.md @@ -1,4 +1,4 @@ -# The `freespace_planner` +# The `autoware_freespace_planner` ## freespace_planner_node diff --git a/planning/freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml similarity index 100% rename from planning/freespace_planner/config/freespace_planner.param.yaml rename to planning/autoware_freespace_planner/config/freespace_planner.param.yaml diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/autoware_freespace_planner/include/autoware_freespace_planner/freespace_planner_node.hpp similarity index 84% rename from planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp rename to planning/autoware_freespace_planner/include/autoware_freespace_planner/freespace_planner_node.hpp index 25f3bca215063..2084d8a3aad24 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/autoware_freespace_planner/include/autoware_freespace_planner/freespace_planner_node.hpp @@ -28,15 +28,15 @@ * limitations under the License. */ -#ifndef FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ -#define FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#ifndef AUTOWARE_FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#define AUTOWARE_FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include -#include +#include +#include +#include #include -#include #include #include @@ -63,17 +63,17 @@ #include #include -namespace freespace_planner +namespace autoware::freespace_planner { +using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; +using autoware::freespace_planning_algorithms::AstarParam; +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::PlannerCommonParam; +using autoware::freespace_planning_algorithms::RRTStar; +using autoware::freespace_planning_algorithms::RRTStarParam; +using autoware::freespace_planning_algorithms::VehicleShape; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::Trajectory; -using freespace_planning_algorithms::AbstractPlanningAlgorithm; -using freespace_planning_algorithms::AstarParam; -using freespace_planning_algorithms::AstarSearch; -using freespace_planning_algorithms::PlannerCommonParam; -using freespace_planning_algorithms::RRTStar; -using freespace_planning_algorithms::RRTStarParam; -using freespace_planning_algorithms::VehicleShape; using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::TransformStamped; @@ -165,6 +165,6 @@ class FreespacePlannerNode : public rclcpp::Node std::unique_ptr logger_configure_; }; -} // namespace freespace_planner +} // namespace autoware::freespace_planner -#endif // FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ +#endif // AUTOWARE_FREESPACE_PLANNER__FREESPACE_PLANNER_NODE_HPP_ diff --git a/planning/freespace_planner/launch/freespace_planner.launch.xml b/planning/autoware_freespace_planner/launch/freespace_planner.launch.xml similarity index 86% rename from planning/freespace_planner/launch/freespace_planner.launch.xml rename to planning/autoware_freespace_planner/launch/freespace_planner.launch.xml index 7c2408b0311f5..e92a04307fed2 100644 --- a/planning/freespace_planner/launch/freespace_planner.launch.xml +++ b/planning/autoware_freespace_planner/launch/freespace_planner.launch.xml @@ -9,9 +9,9 @@ - + - + diff --git a/planning/freespace_planner/package.xml b/planning/autoware_freespace_planner/package.xml similarity index 86% rename from planning/freespace_planner/package.xml rename to planning/autoware_freespace_planner/package.xml index 8445bd2355b25..8a2ecad429f1b 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/autoware_freespace_planner/package.xml @@ -1,9 +1,9 @@ - freespace_planner + autoware_freespace_planner 0.1.0 - The freespace_planner package + The autoware_freespace_planner package Kosuke Takeuchi Takamasa Horibe Takayuki Murooka @@ -15,9 +15,10 @@ ament_cmake_auto autoware_cmake + autoware_freespace_planning_algorithms autoware_planning_msgs autoware_planning_test_manager - freespace_planning_algorithms + autoware_vehicle_info_utils geometry_msgs motion_utils nav_msgs @@ -29,7 +30,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp similarity index 96% rename from planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp rename to planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp index b0d9e9cf3edcd..797c140aa9dd9 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp @@ -28,9 +28,9 @@ * limitations under the License. */ -#include "freespace_planner/freespace_planner_node.hpp" +#include "autoware_freespace_planner/freespace_planner_node.hpp" -#include "freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" #include #include @@ -47,10 +47,10 @@ namespace using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using freespace_planning_algorithms::AstarSearch; -using freespace_planning_algorithms::PlannerWaypoint; -using freespace_planning_algorithms::PlannerWaypoints; -using freespace_planning_algorithms::RRTStar; +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::PlannerWaypoint; +using autoware::freespace_planning_algorithms::PlannerWaypoints; +using autoware::freespace_planning_algorithms::RRTStar; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using geometry_msgs::msg::PoseStamped; @@ -217,7 +217,7 @@ bool isStopped( } // namespace -namespace freespace_planner +namespace autoware::freespace_planner { FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_options) : Node("freespace_planner", node_options) @@ -241,7 +241,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti // set vehicle_info { - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = + autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_shape_.length = vehicle_info.vehicle_length_m; vehicle_shape_.width = vehicle_info.vehicle_width_m; vehicle_shape_.base2back = vehicle_info.rear_overhang_m; @@ -562,7 +563,7 @@ TransformStamped FreespacePlannerNode::getTransform( void FreespacePlannerNode::initializePlanningAlgorithm() { // Extend robot shape - freespace_planning_algorithms::VehicleShape extended_vehicle_shape = vehicle_shape_; + autoware::freespace_planning_algorithms::VehicleShape extended_vehicle_shape = vehicle_shape_; const double margin = node_param_.vehicle_shape_margin_m; extended_vehicle_shape.length += margin; extended_vehicle_shape.width += margin; @@ -582,7 +583,7 @@ void FreespacePlannerNode::initializePlanningAlgorithm() } RCLCPP_INFO_STREAM(get_logger(), "initialize planning algorithm: " << algo_name); } -} // namespace freespace_planner +} // namespace autoware::freespace_planner #include -RCLCPP_COMPONENTS_REGISTER_NODE(freespace_planner::FreespacePlannerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::freespace_planner::FreespacePlannerNode) diff --git a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp similarity index 94% rename from planning/freespace_planner/test/test_freespace_planner_node_interface.cpp rename to planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp index 881ce269a7895..621bed12fdabc 100644 --- a/planning/freespace_planner/test/test_freespace_planner_node_interface.cpp +++ b/planning/autoware_freespace_planner/test/test_freespace_planner_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "freespace_planner/freespace_planner_node.hpp" +#include "autoware_freespace_planner/freespace_planner_node.hpp" #include #include @@ -22,7 +22,7 @@ #include -using freespace_planner::FreespacePlannerNode; +using autoware::freespace_planner::FreespacePlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -41,7 +41,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto freespace_planner_dir = - ament_index_cpp::get_package_share_directory("freespace_planner"); + ament_index_cpp::get_package_share_directory("autoware_freespace_planner"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", diff --git a/planning/freespace_planning_algorithms/CMakeLists.txt b/planning/autoware_freespace_planning_algorithms/CMakeLists.txt similarity index 79% rename from planning/freespace_planning_algorithms/CMakeLists.txt rename to planning/autoware_freespace_planning_algorithms/CMakeLists.txt index 726aa47f7a5e5..4f678cd9fdec5 100644 --- a/planning/freespace_planning_algorithms/CMakeLists.txt +++ b/planning/autoware_freespace_planning_algorithms/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(freespace_planning_algorithms) +project(autoware_freespace_planning_algorithms) find_package(autoware_cmake REQUIRED) find_package(python_cmake_module REQUIRED) @@ -19,23 +19,23 @@ target_link_libraries(rrtstar_core reeds_shepp ) -ament_auto_add_library(freespace_planning_algorithms SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/abstract_algorithm.cpp src/astar_search.cpp src/rrtstar.cpp ) -target_link_libraries(freespace_planning_algorithms +target_link_libraries(${PROJECT_NAME} reeds_shepp ) if(BUILD_TESTING) find_package(ament_cmake_ros REQUIRED) - ament_add_ros_isolated_gtest(freespace_planning_algorithms-test + ament_add_ros_isolated_gtest(${PROJECT_NAME}-test test/src/test_freespace_planning_algorithms.cpp ) - target_link_libraries(freespace_planning_algorithms-test - freespace_planning_algorithms + target_link_libraries(${PROJECT_NAME}-test + ${PROJECT_NAME} ) find_package(ament_cmake_ros REQUIRED) @@ -43,7 +43,7 @@ if(BUILD_TESTING) test/src/test_rrtstar_core_informed.cpp ) target_link_libraries(rrtstar_core_informed-test - freespace_planning_algorithms + ${PROJECT_NAME} ) endif() diff --git a/planning/freespace_planning_algorithms/README.md b/planning/autoware_freespace_planning_algorithms/README.md similarity index 96% rename from planning/freespace_planning_algorithms/README.md rename to planning/autoware_freespace_planning_algorithms/README.md index 8bd015589d58d..c5ea6b3df8183 100644 --- a/planning/freespace_planning_algorithms/README.md +++ b/planning/autoware_freespace_planning_algorithms/README.md @@ -46,8 +46,8 @@ Some selection criteria would be: Building the package with ros-test and run tests: ```sh -colcon build --packages-select freespace_planning_algorithms -colcon test --packages-select freespace_planning_algorithms +colcon build --packages-select autoware_freespace_planning_algorithms +colcon test --packages-select autoware_freespace_planning_algorithms ``` diff --git a/planning/freespace_planning_algorithms/freespace_planning_algorithms/__init__.py b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/__init__.py similarity index 100% rename from planning/freespace_planning_algorithms/freespace_planning_algorithms/__init__.py rename to planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/__init__.py diff --git a/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py similarity index 95% rename from planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py rename to planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py index c9dcc351e3ce2..2ffa20150fb0c 100644 --- a/planning/freespace_planning_algorithms/freespace_planning_algorithms/astar_search.py +++ b/planning/autoware_freespace_planning_algorithms/autoware_freespace_planning_algorithms/astar_search.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -import freespace_planning_algorithms.freespace_planning_algorithms_pybind as _fp +import autoware_freespace_planning_algorithms.autoware_freespace_planning_algorithms_pybind as _fp from geometry_msgs.msg import Point from geometry_msgs.msg import Pose from geometry_msgs.msg import Quaternion diff --git a/planning/freespace_planning_algorithms/figs/summary-astar_single.png b/planning/autoware_freespace_planning_algorithms/figs/summary-astar_single.png similarity index 100% rename from planning/freespace_planning_algorithms/figs/summary-astar_single.png rename to planning/autoware_freespace_planning_algorithms/figs/summary-astar_single.png diff --git a/planning/freespace_planning_algorithms/figs/summary-rrtstar_fastest.png b/planning/autoware_freespace_planning_algorithms/figs/summary-rrtstar_fastest.png similarity index 100% rename from planning/freespace_planning_algorithms/figs/summary-rrtstar_fastest.png rename to planning/autoware_freespace_planning_algorithms/figs/summary-rrtstar_fastest.png diff --git a/planning/freespace_planning_algorithms/figs/summary-rrtstar_informed_update.png b/planning/autoware_freespace_planning_algorithms/figs/summary-rrtstar_informed_update.png similarity index 100% rename from planning/freespace_planning_algorithms/figs/summary-rrtstar_informed_update.png rename to planning/autoware_freespace_planning_algorithms/figs/summary-rrtstar_informed_update.png diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp similarity index 90% rename from planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp index b4e3b2befae40..394900c9a704c 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/abstract_algorithm.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ -#define FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#include #include -#include #include #include @@ -25,7 +25,7 @@ #include -namespace freespace_planning_algorithms +namespace autoware::freespace_planning_algorithms { int discretizeAngle(const double theta, const int theta_size); @@ -69,7 +69,7 @@ struct VehicleShape } explicit VehicleShape( - const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) : length(vehicle_info.vehicle_length_m + margin), width(vehicle_info.vehicle_width_m + margin), base2back(vehicle_info.rear_overhang_m + margin / 2.0) @@ -125,7 +125,7 @@ class AbstractPlanningAlgorithm AbstractPlanningAlgorithm( const PlannerCommonParam & planner_common_param, - const vehicle_info_util::VehicleInfo & vehicle_info, const double margin = 0.0) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin) { } @@ -187,6 +187,6 @@ class AbstractPlanningAlgorithm PlannerWaypoints waypoints_; }; -} // namespace freespace_planning_algorithms +} // namespace autoware::freespace_planning_algorithms -#endif // FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ +#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ABSTRACT_ALGORITHM_HPP_ diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/astar_search.hpp similarity index 91% rename from planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/astar_search.hpp index 3dced3eb1d01d..99cf3fc743a82 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/astar_search.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ -#define FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ +#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ +#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ -#include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include "freespace_planning_algorithms/reeds_shepp.hpp" +#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware_freespace_planning_algorithms/reeds_shepp.hpp" #include @@ -32,7 +32,7 @@ #include #include -namespace freespace_planning_algorithms +namespace autoware::freespace_planning_algorithms { enum class NodeStatus : uint8_t { None, Open, Closed }; @@ -169,6 +169,6 @@ class AstarSearch : public AbstractPlanningAlgorithm int x_scale_; int y_scale_; }; -} // namespace freespace_planning_algorithms +} // namespace autoware::freespace_planning_algorithms -#endif // FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ +#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__ASTAR_SEARCH_HPP_ diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/reeds_shepp.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/reeds_shepp.hpp similarity index 94% rename from planning/freespace_planning_algorithms/include/freespace_planning_algorithms/reeds_shepp.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/reeds_shepp.hpp index f14fef33d26da..ece30175fad86 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/reeds_shepp.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/reeds_shepp.hpp @@ -78,14 +78,14 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#ifndef FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ -#define FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ +#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ +#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ #include #include #include -namespace freespace_planning_algorithms +namespace autoware::freespace_planning_algorithms { using ReedsSheppPathSamplingCallback = std::function; using ReedsSheppPathTypeCallback = std::function; @@ -137,6 +137,6 @@ class ReedsSheppStateSpace /** \brief Turning radius */ double rho_; }; -} // namespace freespace_planning_algorithms +} // namespace autoware::freespace_planning_algorithms -#endif // FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ +#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__REEDS_SHEPP_HPP_ diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar.hpp similarity index 83% rename from planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar.hpp index ac21f7333df37..6a203d84a47c9 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ -#define FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ +#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ +#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ -#include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include "freespace_planning_algorithms/rrtstar_core.hpp" +#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware_freespace_planning_algorithms/rrtstar_core.hpp" #include #include -namespace freespace_planning_algorithms +namespace autoware::freespace_planning_algorithms { struct RRTStarParam { @@ -69,6 +69,6 @@ class RRTStar : public AbstractPlanningAlgorithm const VehicleShape original_vehicle_shape_; }; -} // namespace freespace_planning_algorithms +} // namespace autoware::freespace_planning_algorithms -#endif // FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ +#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_HPP_ diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar_core.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar_core.hpp similarity index 90% rename from planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar_core.hpp rename to planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar_core.hpp index 257a5693e118a..633da7a86a8c6 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/rrtstar_core.hpp +++ b/planning/autoware_freespace_planning_algorithms/include/autoware_freespace_planning_algorithms/rrtstar_core.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ -#define FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ +#ifndef AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ +#define AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ -#include "freespace_planning_algorithms/reeds_shepp.hpp" +#include "autoware_freespace_planning_algorithms/reeds_shepp.hpp" #include @@ -28,11 +28,11 @@ #include #include -namespace rrtstar_core +namespace autoware::freespace_planning_algorithms::rrtstar_core { -using Path = freespace_planning_algorithms::ReedsSheppStateSpace::ReedsSheppPath; -using Pose = freespace_planning_algorithms::ReedsSheppStateSpace::StateXYT; -using ReedsSheppStateSpace = freespace_planning_algorithms::ReedsSheppStateSpace; +using Path = ReedsSheppStateSpace::ReedsSheppPath; +using Pose = ReedsSheppStateSpace::StateXYT; +using ReedsSheppStateSpace = ReedsSheppStateSpace; const double inf = std::numeric_limits::infinity(); // cspell: ignore rsspace @@ -156,6 +156,6 @@ class RRTStar CSpace cspace_; }; -} // namespace rrtstar_core +} // namespace autoware::freespace_planning_algorithms::rrtstar_core -#endif // FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ +#endif // AUTOWARE_FREESPACE_PLANNING_ALGORITHMS__RRTSTAR_CORE_HPP_ diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/autoware_freespace_planning_algorithms/package.xml similarity index 87% rename from planning/freespace_planning_algorithms/package.xml rename to planning/autoware_freespace_planning_algorithms/package.xml index 9b19d8a81bb51..61173f0a877e4 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/autoware_freespace_planning_algorithms/package.xml @@ -1,9 +1,9 @@ - freespace_planning_algorithms + autoware_freespace_planning_algorithms 0.1.0 - The freespace_planning_algorithms package + The autoware_freespace_planning_algorithms package Kosuke Takeuchi Takamasa Horibe Takayuki Murooka @@ -16,6 +16,7 @@ autoware_cmake python_cmake_module + autoware_vehicle_info_utils geometry_msgs nav_msgs nlohmann-json-dev @@ -25,7 +26,6 @@ tf2 tf2_geometry_msgs tier4_autoware_utils - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/planning/freespace_planning_algorithms/rrtstar.md b/planning/autoware_freespace_planning_algorithms/rrtstar.md similarity index 100% rename from planning/freespace_planning_algorithms/rrtstar.md rename to planning/autoware_freespace_planning_algorithms/rrtstar.md diff --git a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp similarity index 95% rename from planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp rename to planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp index 9d10cf311fb16..b872ee993c5e8 100644 --- a/planning/freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp +++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include "freespace_planning_algorithms/astar_search.hpp" +#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware_freespace_planning_algorithms/astar_search.hpp" #include #include @@ -24,6 +24,8 @@ #include #include +namespace autoware::freespace_planning_algorithms +{ struct PlannerWaypointsVector { std::vector> waypoints; @@ -32,7 +34,7 @@ struct PlannerWaypointsVector class AstarSearchPython : public freespace_planning_algorithms::AstarSearch { - using freespace_planning_algorithms::AstarSearch::AstarSearch; + using AstarSearch::AstarSearch; public: void setMapByte(const std::string & costmap_byte) @@ -97,7 +99,7 @@ class AstarSearchPython : public freespace_planning_algorithms::AstarSearch namespace py = pybind11; -PYBIND11_MODULE(freespace_planning_algorithms_pybind, p) +PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p) { auto pyPlannerWaypointsVector = py::class_(p, "PlannerWaypointsVector", py::dynamic_attr()) @@ -165,3 +167,4 @@ PYBIND11_MODULE(freespace_planning_algorithms_pybind, p) .def("makePlan", &AstarSearchPython::makePlanByte) .def("getWaypoints", &AstarSearchPython::getWaypointsAsVector); } +} // namespace autoware::freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py similarity index 97% rename from planning/freespace_planning_algorithms/scripts/example/example.py rename to planning/autoware_freespace_planning_algorithms/scripts/example/example.py index 73f53e23bc540..c0cdf8c2ec6ba 100644 --- a/planning/freespace_planning_algorithms/scripts/example/example.py +++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py @@ -12,7 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -import freespace_planning_algorithms.astar_search as fp +import autoware_freespace_planning_algorithms.astar_search as fp from geometry_msgs.msg import Pose from nav_msgs.msg import OccupancyGrid from pyquaternion import Quaternion diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp similarity index 97% rename from planning/freespace_planning_algorithms/src/abstract_algorithm.cpp rename to planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp index 2c7340072eb53..d946ea87cff81 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" #include #include #include -namespace freespace_planning_algorithms +namespace autoware::freespace_planning_algorithms { using tier4_autoware_utils::createQuaternionFromYaw; using tier4_autoware_utils::normalizeRadian; @@ -250,4 +250,4 @@ bool AbstractPlanningAlgorithm::hasObstacleOnTrajectory( return false; } -} // namespace freespace_planning_algorithms +} // namespace autoware::freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp similarity index 98% rename from planning/freespace_planning_algorithms/src/astar_search.cpp rename to planning/autoware_freespace_planning_algorithms/src/astar_search.cpp index 3cc3801575116..1a7b23251111c 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "freespace_planning_algorithms/astar_search.hpp" +#include "autoware_freespace_planning_algorithms/astar_search.hpp" #include #include @@ -27,7 +27,7 @@ #include -namespace freespace_planning_algorithms +namespace autoware::freespace_planning_algorithms { double calcReedsSheppDistance( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, double radius) @@ -368,4 +368,4 @@ geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const return pose_local; } -} // namespace freespace_planning_algorithms +} // namespace autoware::freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/reeds_shepp.cpp b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp similarity index 99% rename from planning/freespace_planning_algorithms/src/reeds_shepp.cpp rename to planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp index c6e0b0002ace8..e8e846dde5681 100644 --- a/planning/freespace_planning_algorithms/src/reeds_shepp.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/reeds_shepp.cpp @@ -78,7 +78,7 @@ // ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include "freespace_planning_algorithms/reeds_shepp.hpp" +#include "autoware_freespace_planning_algorithms/reeds_shepp.hpp" #include #include @@ -87,7 +87,7 @@ namespace { // The comments, variable names, etc. use the nomenclature from the Reeds & Shepp paper. -using freespace_planning_algorithms::ReedsSheppStateSpace; +using autoware::freespace_planning_algorithms::ReedsSheppStateSpace; const double pi = M_PI; const double twopi = 2. * pi; @@ -617,7 +617,7 @@ ReedsSheppStateSpace::ReedsSheppPath reedsShepp(double x, double y, double phi) } } // namespace -namespace freespace_planning_algorithms +namespace autoware::freespace_planning_algorithms { const ReedsSheppStateSpace::ReedsSheppPathSegmentType ReedsSheppStateSpace::reedsSheppPathType[18][5] = { @@ -716,4 +716,4 @@ ReedsSheppStateSpace::StateXYT ReedsSheppStateSpace::interpolate( s_out.y = s_out.y * rho_ + s0.y; return s_out; } -} // namespace freespace_planning_algorithms +} // namespace autoware::freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp similarity index 97% rename from planning/freespace_planning_algorithms/src/rrtstar.cpp rename to planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp index ae1f0c10e74a8..587771c35d4df 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp @@ -12,21 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "freespace_planning_algorithms/rrtstar.hpp" +#include "autoware_freespace_planning_algorithms/rrtstar.hpp" -namespace +namespace autoware::freespace_planning_algorithms { - rrtstar_core::Pose poseMsgToPose(const geometry_msgs::msg::Pose & pose_msg) { return rrtstar_core::Pose{ pose_msg.position.x, pose_msg.position.y, tf2::getYaw(pose_msg.orientation)}; } -} // namespace - -namespace freespace_planning_algorithms -{ RRTStar::RRTStar( const PlannerCommonParam & planner_common_param, const VehicleShape & original_vehicle_shape, const RRTStarParam & rrtstar_param) @@ -167,4 +162,4 @@ void RRTStar::setRRTPath(const std::vector & waypoints) } } -} // namespace freespace_planning_algorithms +} // namespace autoware::freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/rrtstar_core.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp similarity index 98% rename from planning/freespace_planning_algorithms/src/rrtstar_core.cpp rename to planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp index e638a3733adb5..8f6405bab80cf 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar_core.cpp +++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "freespace_planning_algorithms/rrtstar_core.hpp" +#include "autoware_freespace_planning_algorithms/rrtstar_core.hpp" #include @@ -26,7 +26,7 @@ // cspell: ignore rsspace // In this case, RSSpace means "Reeds Shepp state Space" -namespace rrtstar_core +namespace autoware::freespace_planning_algorithms::rrtstar_core { CSpace::CSpace( const Pose & lo, const Pose & hi, double r, std::function is_obstacle_free) @@ -459,4 +459,4 @@ void RRTStar::reconnect(const NodeSharedPtr & node_new, const NodeSharedPtr & no } } -} // namespace rrtstar_core +} // namespace autoware::freespace_planning_algorithms::rrtstar_core diff --git a/planning/freespace_planning_algorithms/test/debug_plot.py b/planning/autoware_freespace_planning_algorithms/test/debug_plot.py similarity index 100% rename from planning/freespace_planning_algorithms/test/debug_plot.py rename to planning/autoware_freespace_planning_algorithms/test/debug_plot.py diff --git a/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py b/planning/autoware_freespace_planning_algorithms/test/debug_plot_rrtstar_core.py similarity index 100% rename from planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py rename to planning/autoware_freespace_planning_algorithms/test/debug_plot_rrtstar_core.py diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp similarity index 97% rename from planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp rename to planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index c11c3df73d467..5cfb004a3b1c3 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "freespace_planning_algorithms/abstract_algorithm.hpp" -#include "freespace_planning_algorithms/astar_search.hpp" -#include "freespace_planning_algorithms/rrtstar.hpp" +#include "autoware_freespace_planning_algorithms/abstract_algorithm.hpp" +#include "autoware_freespace_planning_algorithms/astar_search.hpp" +#include "autoware_freespace_planning_algorithms/rrtstar.hpp" #include #include @@ -33,7 +33,7 @@ #include #include -namespace fpa = freespace_planning_algorithms; +namespace fpa = autoware::freespace_planning_algorithms; const double length_lexus = 5.5; const double width_lexus = 2.75; diff --git a/planning/freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp similarity index 94% rename from planning/freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp rename to planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp index 40527e51f8436..e08afdb236192 100644 --- a/planning/freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp +++ b/planning/autoware_freespace_planning_algorithms/test/src/test_rrtstar_core_informed.cpp @@ -12,13 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "freespace_planning_algorithms/rrtstar_core.hpp" +#include "autoware_freespace_planning_algorithms/rrtstar_core.hpp" #include #include #include +namespace autoware::freespace_planning_algorithms::rrtstar_core +{ bool checkAllNodeConnected(const rrtstar_core::RRTStar & tree) { const auto & nodes = tree.getNodes(); @@ -111,3 +113,4 @@ int main(int argc, char ** argv) testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } +} // namespace autoware::freespace_planning_algorithms::rrtstar_core diff --git a/planning/autoware_path_optimizer/CMakeLists.txt b/planning/autoware_path_optimizer/CMakeLists.txt index 3ceeae1022b10..78077e6c42720 100644 --- a/planning/autoware_path_optimizer/CMakeLists.txt +++ b/planning/autoware_path_optimizer/CMakeLists.txt @@ -41,7 +41,7 @@ target_include_directories(autoware_path_optimizer # register node rclcpp_components_register_node(autoware_path_optimizer - PLUGIN "autoware_path_optimizer::PathOptimizer" + PLUGIN "autoware::path_optimizer::PathOptimizer" EXECUTABLE path_optimizer_node ) diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp index b27eff787ca5a..d2de8d201d9fa 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct ReferencePoint; struct Bounds; @@ -153,6 +153,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp index d1d2abaeaad61..cea58a11a530a 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp @@ -16,20 +16,20 @@ #include "autoware_path_optimizer/common_structs.hpp" #include "autoware_path_optimizer/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/clock.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); -} // namespace autoware_path_optimizer + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool publish_extra_marker); +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp index 33da339c62e40..98d7e131f4bdc 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp @@ -18,12 +18,12 @@ #include "autoware_path_optimizer/common_structs.hpp" #include "autoware_path_optimizer/state_equation_generator.hpp" #include "autoware_path_optimizer/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "gtest/gtest.h" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -34,7 +34,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct Bounds { @@ -105,8 +105,8 @@ class MPTOptimizer public: MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, - const vehicle_info_util::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, - const std::shared_ptr debug_data_ptr, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, const std::shared_ptr time_keeper_ptr); std::vector optimizeTrajectory(const PlannerData & planner_data); @@ -142,7 +142,8 @@ class MPTOptimizer struct MPTParam { - explicit MPTParam(rclcpp::Node * node, const vehicle_info_util::VehicleInfo & vehicle_info); + explicit MPTParam( + rclcpp::Node * node, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); MPTParam() = default; void onParam(const std::vector & parameters); @@ -218,7 +219,7 @@ class MPTOptimizer // argument bool enable_debug_info_; EgoNearestParam ego_nearest_param_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; TrajectoryParam traj_param_; mutable std::shared_ptr debug_data_ptr_; mutable std::shared_ptr time_keeper_ptr_; @@ -308,5 +309,5 @@ class MPTOptimizer size_t getNumberOfSlackVariables() const; std::optional calcNormalizedAvoidanceCost(const ReferencePoint & ref_point) const; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp index a0e776628e3cb..c1f644d31aa9a 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp @@ -19,11 +19,11 @@ #include "autoware_path_optimizer/mpt_optimizer.hpp" #include "autoware_path_optimizer/replan_checker.hpp" #include "autoware_path_optimizer/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -33,7 +33,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { class PathOptimizer : public rclcpp::Node { @@ -63,7 +63,7 @@ class PathOptimizer : public rclcpp::Node DrivingDirectionChecker driving_direction_checker_{}; // argument variables - vehicle_info_util::VehicleInfo vehicle_info_{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable std::shared_ptr debug_data_ptr_{nullptr}; mutable std::shared_ptr time_keeper_ptr_{nullptr}; @@ -141,6 +141,6 @@ class PathOptimizer : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp index 8270bb631e0ae..26d9599f07091 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp @@ -24,7 +24,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { class ReplanChecker { @@ -66,6 +66,6 @@ class ReplanChecker bool isPathGoalChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp index 6ee4fcb0ab7a5..0c41da0ee5f62 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp @@ -22,7 +22,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct ReferencePoint; @@ -58,5 +58,5 @@ class StateEquationGenerator std::unique_ptr vehicle_model_ptr_; mutable std::shared_ptr time_keeper_ptr_; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ 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 755d0b2ace297..63a5840789a6b 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 @@ -28,7 +28,7 @@ #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { // std_msgs using std_msgs::msg::Header; @@ -45,6 +45,6 @@ using visualization_msgs::msg::MarkerArray; // debug using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp index 32ef8cd5941fc..aa44a515a08ec 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp @@ -17,11 +17,11 @@ #include "autoware_path_optimizer/common_structs.hpp" #include "autoware_path_optimizer/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -39,13 +39,13 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace geometry_utils { @@ -65,8 +65,8 @@ bool isSamePoint(const T1 & t1, const T2 & t2) bool isOutsideDrivableAreaFromRectangleFootprint( const geometry_msgs::msg::Pose & pose, const std::vector & left_bound, const std::vector & right_bound, - const vehicle_info_util::VehicleInfo & vehicle_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool use_footprint_polygon_for_outside_drivable_area_check); } // namespace geometry_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp index 1056d80ef37aa..f3f528df2ec42 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp @@ -38,16 +38,16 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); template <> -double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p); +double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace trajectory_utils { @@ -214,5 +214,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index f02473ebd015e..c158815999398 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -16,6 +16,7 @@ autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils geometry_msgs interpolation motion_utils @@ -28,7 +29,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index 3127d521160c2..c97d5c5adec46 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -18,7 +18,7 @@ #include "visualization_msgs/msg/marker_array.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -29,7 +29,7 @@ namespace { MarkerArray getFootprintsMarkerArray( const std::vector & mpt_traj, - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t sampling_num) { auto marker = createDefaultMarker( "map", rclcpp::Clock().now(), "mpt_footprints", 0, Marker::LINE_STRIP, @@ -73,7 +73,7 @@ MarkerArray getFootprintsMarkerArray( MarkerArray getBoundsWidthMarkerArray( const std::vector & ref_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t sampling_num) { const auto current_time = rclcpp::Clock().now(); MarkerArray marker_array; @@ -146,7 +146,7 @@ MarkerArray getBoundsWidthMarkerArray( MarkerArray getBoundsLineMarkerArray( const std::vector & ref_points, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { MarkerArray marker_array; @@ -183,7 +183,7 @@ MarkerArray getBoundsLineMarkerArray( MarkerArray getVehicleCircleLinesMarkerArray( const std::vector & ref_points, const std::vector & vehicle_circle_longitudinal_offsets, - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t sampling_num, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t sampling_num, const std::string & ns) { const auto current_time = rclcpp::Clock().now(); @@ -339,8 +339,9 @@ visualization_msgs::msg::MarkerArray getPointsTextMarkerArray( } visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( - const geometry_msgs::msg::Pose & stop_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const std::string & ns, const double r, const double g, const double b) + const geometry_msgs::msg::Pose & stop_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const std::string & ns, + const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; @@ -373,7 +374,7 @@ visualization_msgs::msg::MarkerArray getFootprintByDrivableAreaMarkerArray( MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool publish_extra_marker) { MarkerArray marker_array; @@ -432,4 +433,4 @@ MarkerArray getDebugMarker( return marker_array; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index ece301e64c97e..0652085d85c48 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -29,12 +29,12 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace { std::tuple, std::vector> calcVehicleCirclesByUniformCircle( - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t circle_num, const double radius_ratio) { const double lateral_offset = @@ -56,7 +56,7 @@ std::tuple, std::vector> calcVehicleCirclesByUniform } std::tuple, std::vector> calcVehicleCirclesByBicycleModel( - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t circle_num, const double rear_radius_ratio, const double front_radius_ratio) { if (circle_num < 2) { @@ -84,7 +84,7 @@ std::tuple, std::vector> calcVehicleCirclesByBicycle } std::tuple, std::vector> calcVehicleCirclesByFittingUniformCircle( - const vehicle_info_util::VehicleInfo & vehicle_info, const size_t circle_num) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const size_t circle_num) { if (circle_num < 2) { throw std::invalid_argument("circle_num is less than 2."); @@ -169,7 +169,7 @@ double calcLateralDistToBounds( } // namespace MPTOptimizer::MPTParam::MPTParam( - rclcpp::Node * node, const vehicle_info_util::VehicleInfo & vehicle_info) + rclcpp::Node * node, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { { // option steer_limit_constraint = node->declare_parameter("mpt.option.steer_limit_constraint"); @@ -393,8 +393,8 @@ void MPTOptimizer::MPTParam::onParam(const std::vector & para MPTOptimizer::MPTOptimizer( rclcpp::Node * node, const bool enable_debug_info, const EgoNearestParam ego_nearest_param, - const vehicle_info_util::VehicleInfo & vehicle_info, const TrajectoryParam & traj_param, - const std::shared_ptr debug_data_ptr, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const TrajectoryParam & traj_param, const std::shared_ptr debug_data_ptr, const std::shared_ptr time_keeper_ptr) : enable_debug_info_(enable_debug_info), ego_nearest_param_(ego_nearest_param), @@ -1783,4 +1783,4 @@ std::optional MPTOptimizer::calcNormalizedAvoidanceCost( } return std::clamp(-negative_avoidance_cost / mpt_param_.max_avoidance_cost, 0.0, 1.0); } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 49d41e6b07884..d9df26938a9f8 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace { @@ -84,7 +84,7 @@ std::vector calcSegmentLengthVector(const std::vector & PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options) : Node("path_optimizer", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()), time_keeper_ptr_(std::make_shared()) { @@ -666,7 +666,7 @@ void PathOptimizer::publishDebugData(const Header & header) const time_keeper_ptr_->toc(__func__, " "); } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_path_optimizer::PathOptimizer) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_optimizer::PathOptimizer) diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index 797041ee75416..6745872f50b02 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -21,7 +21,7 @@ #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) : ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) @@ -212,4 +212,4 @@ bool ReplanChecker::isPathGoalChanged( return true; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 7712fbbf6c3cf..ec6a9e575be95 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -16,7 +16,7 @@ #include "autoware_path_optimizer/mpt_optimizer.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { // state equation: x = B u + W (u includes x_0) // NOTE: Originally, x_t+1 = Ad x_t + Bd u + Wd. @@ -69,4 +69,4 @@ Eigen::VectorXd StateEquationGenerator::predict( { return mat.B * U + mat.W; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 9d93cdc26a7ed..99eb85d27e054 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -36,7 +36,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace bg = boost::geometry; using tier4_autoware_utils::LinearRing2d; @@ -120,7 +120,7 @@ namespace geometry_utils bool isOutsideDrivableAreaFromRectangleFootprint( const geometry_msgs::msg::Pose & pose, const std::vector & left_bound, const std::vector & right_bound, - const vehicle_info_util::VehicleInfo & vehicle_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const bool use_footprint_polygon_for_outside_drivable_area_check) { if (left_bound.empty() || right_bound.empty()) { @@ -207,4 +207,4 @@ bool isOutsideDrivableAreaFromRectangleFootprint( return false; } } // namespace geometry_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 7983c5c2a3c2f..75e2b75b232e0 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -36,25 +36,25 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p) +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p) { return p.pose.position; } template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p) +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p) { return p.pose; } template <> -double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p) +double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p) { return p.longitudinal_velocity_mps; } } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace trajectory_utils { @@ -242,4 +242,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index 8ef099ba09f24..4af88539597be 100644 --- a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -42,7 +42,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", path_optimizer_dir + "/config/path_optimizer.param.yaml"}); - auto test_target_node = std::make_shared(node_options); + auto test_target_node = std::make_shared(node_options); // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry"); diff --git a/planning/path_smoother/CMakeLists.txt b/planning/autoware_path_smoother/CMakeLists.txt similarity index 70% rename from planning/path_smoother/CMakeLists.txt rename to planning/autoware_path_smoother/CMakeLists.txt index b2f42181f9a0f..a5873fe3735ef 100644 --- a/planning/path_smoother/CMakeLists.txt +++ b/planning/autoware_path_smoother/CMakeLists.txt @@ -1,23 +1,23 @@ cmake_minimum_required(VERSION 3.14) -project(path_smoother) +project(autoware_path_smoother) find_package(autoware_cmake REQUIRED) autoware_package() find_package(Eigen3 REQUIRED) -ament_auto_add_library(path_smoother SHARED +ament_auto_add_library(autoware_path_smoother SHARED DIRECTORY src ) -target_include_directories(path_smoother +target_include_directories(autoware_path_smoother SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) # register node -rclcpp_components_register_node(path_smoother - PLUGIN "path_smoother::ElasticBandSmoother" +rclcpp_components_register_node(autoware_path_smoother + PLUGIN "autoware::path_smoother::ElasticBandSmoother" EXECUTABLE elastic_band_smoother ) diff --git a/planning/path_smoother/README.md b/planning/autoware_path_smoother/README.md similarity index 100% rename from planning/path_smoother/README.md rename to planning/autoware_path_smoother/README.md diff --git a/planning/path_smoother/config/elastic_band_smoother.param.yaml b/planning/autoware_path_smoother/config/elastic_band_smoother.param.yaml similarity index 100% rename from planning/path_smoother/config/elastic_band_smoother.param.yaml rename to planning/autoware_path_smoother/config/elastic_band_smoother.param.yaml diff --git a/planning/path_smoother/docs/eb.md b/planning/autoware_path_smoother/docs/eb.md similarity index 100% rename from planning/path_smoother/docs/eb.md rename to planning/autoware_path_smoother/docs/eb.md diff --git a/planning/path_smoother/include/path_smoother/common_structs.hpp b/planning/autoware_path_smoother/include/autoware_path_smoother/common_structs.hpp similarity index 92% rename from planning/path_smoother/include/path_smoother/common_structs.hpp rename to planning/autoware_path_smoother/include/autoware_path_smoother/common_structs.hpp index d44c964cf634c..18f5ef82fd6d4 100644 --- a/planning/path_smoother/include/path_smoother/common_structs.hpp +++ b/planning/autoware_path_smoother/include/autoware_path_smoother/common_structs.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SMOOTHER__COMMON_STRUCTS_HPP_ -#define PATH_SMOOTHER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE_PATH_SMOOTHER__COMMON_STRUCTS_HPP_ +#define AUTOWARE_PATH_SMOOTHER__COMMON_STRUCTS_HPP_ -#include "path_smoother/type_alias.hpp" +#include "autoware_path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -25,7 +25,7 @@ #include #include -namespace path_smoother +namespace autoware::path_smoother { struct Bounds; @@ -131,6 +131,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace path_smoother +} // namespace autoware::path_smoother -#endif // PATH_SMOOTHER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE_PATH_SMOOTHER__COMMON_STRUCTS_HPP_ diff --git a/planning/path_smoother/include/path_smoother/elastic_band.hpp b/planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band.hpp similarity index 91% rename from planning/path_smoother/include/path_smoother/elastic_band.hpp rename to planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band.hpp index 1915757b360a4..65b9db9098561 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band.hpp +++ b/planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SMOOTHER__ELASTIC_BAND_HPP_ -#define PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#ifndef AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#define AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#include "autoware_path_smoother/common_structs.hpp" +#include "autoware_path_smoother/type_alias.hpp" #include "osqp_interface/osqp_interface.hpp" -#include "path_smoother/common_structs.hpp" -#include "path_smoother/type_alias.hpp" #include @@ -27,7 +27,7 @@ #include #include -namespace path_smoother +namespace autoware::path_smoother { class EBPathSmoother { @@ -128,6 +128,6 @@ class EBPathSmoother const std::vector & optimized_points, const std::vector & traj_points, const int pad_start_idx) const; }; -} // namespace path_smoother +} // namespace autoware::path_smoother -#endif // PATH_SMOOTHER__ELASTIC_BAND_HPP_ +#endif // AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_HPP_ diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band_smoother.hpp similarity index 89% rename from planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp rename to planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band_smoother.hpp index bc566fdfb96bf..16a4b2bb39db7 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/autoware_path_smoother/include/autoware_path_smoother/elastic_band_smoother.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ -#define PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ +#ifndef AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ +#define AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ +#include "autoware_path_smoother/common_structs.hpp" +#include "autoware_path_smoother/elastic_band.hpp" +#include "autoware_path_smoother/replan_checker.hpp" +#include "autoware_path_smoother/type_alias.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_smoother/common_structs.hpp" -#include "path_smoother/elastic_band.hpp" -#include "path_smoother/replan_checker.hpp" -#include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" @@ -32,7 +32,7 @@ #include #include -namespace path_smoother +namespace autoware::path_smoother { class ElasticBandSmoother : public rclcpp::Node { @@ -116,6 +116,6 @@ class ElasticBandSmoother : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace path_smoother +} // namespace autoware::path_smoother -#endif // PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ +#endif // AUTOWARE_PATH_SMOOTHER__ELASTIC_BAND_SMOOTHER_HPP_ diff --git a/planning/path_smoother/include/path_smoother/replan_checker.hpp b/planning/autoware_path_smoother/include/autoware_path_smoother/replan_checker.hpp similarity index 86% rename from planning/path_smoother/include/path_smoother/replan_checker.hpp rename to planning/autoware_path_smoother/include/autoware_path_smoother/replan_checker.hpp index d06cbc093a0c8..35b0e0102068b 100644 --- a/planning/path_smoother/include/path_smoother/replan_checker.hpp +++ b/planning/autoware_path_smoother/include/autoware_path_smoother/replan_checker.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SMOOTHER__REPLAN_CHECKER_HPP_ -#define PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#ifndef AUTOWARE_PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#define AUTOWARE_PATH_SMOOTHER__REPLAN_CHECKER_HPP_ -#include "path_smoother/common_structs.hpp" -#include "path_smoother/type_alias.hpp" +#include "autoware_path_smoother/common_structs.hpp" +#include "autoware_path_smoother/type_alias.hpp" #include #include #include -namespace path_smoother +namespace autoware::path_smoother { class ReplanChecker { @@ -66,6 +66,6 @@ class ReplanChecker bool isPathGoalChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const; }; -} // namespace path_smoother +} // namespace autoware::path_smoother -#endif // PATH_SMOOTHER__REPLAN_CHECKER_HPP_ +#endif // AUTOWARE_PATH_SMOOTHER__REPLAN_CHECKER_HPP_ diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/autoware_path_smoother/include/autoware_path_smoother/type_alias.hpp similarity index 86% rename from planning/path_smoother/include/path_smoother/type_alias.hpp rename to planning/autoware_path_smoother/include/autoware_path_smoother/type_alias.hpp index 21cbcf8e7eefc..dc565d69d3c4d 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/autoware_path_smoother/include/autoware_path_smoother/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SMOOTHER__TYPE_ALIAS_HPP_ -#define PATH_SMOOTHER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_PATH_SMOOTHER__TYPE_ALIAS_HPP_ +#define AUTOWARE_PATH_SMOOTHER__TYPE_ALIAS_HPP_ #include "autoware_planning_msgs/msg/path.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -25,7 +25,7 @@ #include "tier4_debug_msgs/msg/float64_stamped.hpp" #include "tier4_debug_msgs/msg/string_stamped.hpp" -namespace path_smoother +namespace autoware::path_smoother { // std_msgs using std_msgs::msg::Header; @@ -39,6 +39,6 @@ using nav_msgs::msg::Odometry; // debug using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; -} // namespace path_smoother +} // namespace autoware::path_smoother -#endif // PATH_SMOOTHER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_PATH_SMOOTHER__TYPE_ALIAS_HPP_ diff --git a/planning/path_smoother/include/path_smoother/utils/geometry_utils.hpp b/planning/autoware_path_smoother/include/autoware_path_smoother/utils/geometry_utils.hpp similarity index 80% rename from planning/path_smoother/include/path_smoother/utils/geometry_utils.hpp rename to planning/autoware_path_smoother/include/autoware_path_smoother/utils/geometry_utils.hpp index 39783b958fa61..6cdf34f319442 100644 --- a/planning/path_smoother/include/path_smoother/utils/geometry_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware_path_smoother/utils/geometry_utils.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ -#define PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ +#define AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ #include -namespace path_smoother +namespace autoware::path_smoother { namespace geometry_utils { @@ -31,5 +31,5 @@ bool isSamePoint(const T1 & t1, const T2 & t2) return (std::abs(p1.x - p2.x) <= epsilon && std::abs(p1.y - p2.y) <= epsilon); } } // namespace geometry_utils -} // namespace path_smoother -#endif // PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ +} // namespace autoware::path_smoother +#endif // AUTOWARE_PATH_SMOOTHER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp b/planning/autoware_path_smoother/include/autoware_path_smoother/utils/trajectory_utils.hpp similarity index 91% rename from planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp rename to planning/autoware_path_smoother/include/autoware_path_smoother/utils/trajectory_utils.hpp index 55a7829a5bea3..ea717fae20ef4 100644 --- a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp +++ b/planning/autoware_path_smoother/include/autoware_path_smoother/utils/trajectory_utils.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ -#define PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware_path_smoother/common_structs.hpp" +#include "autoware_path_smoother/type_alias.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_smoother/common_structs.hpp" -#include "path_smoother/type_alias.hpp" #include @@ -35,7 +35,7 @@ #include #include -namespace path_smoother +namespace autoware::path_smoother { namespace trajectory_utils { @@ -137,7 +137,7 @@ std::optional updateFrontPointForFix( motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_DEBUG( - rclcpp::get_logger("path_smoother.trajectory_utils"), + rclcpp::get_logger("autoware_path_smoother.trajectory_utils"), "Fixed point will not be inserted due to the error during calculation."); return std::nullopt; } @@ -148,7 +148,7 @@ std::optional updateFrontPointForFix( constexpr double max_lat_error = 3.0; if (max_lat_error < dist) { RCLCPP_DEBUG( - rclcpp::get_logger("path_smoother.trajectory_utils"), + rclcpp::get_logger("autoware_path_smoother.trajectory_utils"), "New Fixed point is too far from points %f [m]", dist); } @@ -170,5 +170,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace path_smoother -#endif // PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ +} // namespace autoware::path_smoother +#endif // AUTOWARE_PATH_SMOOTHER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/path_smoother/launch/elastic_band_smoother.launch.xml b/planning/autoware_path_smoother/launch/elastic_band_smoother.launch.xml similarity index 77% rename from planning/path_smoother/launch/elastic_band_smoother.launch.xml rename to planning/autoware_path_smoother/launch/elastic_band_smoother.launch.xml index db91e1e2dd97d..62658f88df3a4 100644 --- a/planning/path_smoother/launch/elastic_band_smoother.launch.xml +++ b/planning/autoware_path_smoother/launch/elastic_band_smoother.launch.xml @@ -3,9 +3,9 @@ - + - + diff --git a/planning/path_smoother/media/debug/eb_fixed_traj_visualization.png b/planning/autoware_path_smoother/media/debug/eb_fixed_traj_visualization.png similarity index 100% rename from planning/path_smoother/media/debug/eb_fixed_traj_visualization.png rename to planning/autoware_path_smoother/media/debug/eb_fixed_traj_visualization.png diff --git a/planning/path_smoother/media/debug/eb_traj_visualization.png b/planning/autoware_path_smoother/media/debug/eb_traj_visualization.png similarity index 100% rename from planning/path_smoother/media/debug/eb_traj_visualization.png rename to planning/autoware_path_smoother/media/debug/eb_traj_visualization.png diff --git a/planning/path_smoother/media/eb.svg b/planning/autoware_path_smoother/media/eb.svg similarity index 100% rename from planning/path_smoother/media/eb.svg rename to planning/autoware_path_smoother/media/eb.svg diff --git a/planning/path_smoother/media/eb_constraint.svg b/planning/autoware_path_smoother/media/eb_constraint.svg similarity index 100% rename from planning/path_smoother/media/eb_constraint.svg rename to planning/autoware_path_smoother/media/eb_constraint.svg diff --git a/planning/path_smoother/package.xml b/planning/autoware_path_smoother/package.xml similarity index 93% rename from planning/path_smoother/package.xml rename to planning/autoware_path_smoother/package.xml index a0e83fdf8a091..445f8a0e6e736 100644 --- a/planning/path_smoother/package.xml +++ b/planning/autoware_path_smoother/package.xml @@ -1,9 +1,9 @@ - path_smoother + autoware_path_smoother 0.1.0 - The path_smoother package + The autoware_path_smoother package Takayuki Murooka Maxime CLEMENT Apache License 2.0 diff --git a/planning/path_smoother/scripts/calculation_time_plotter.py b/planning/autoware_path_smoother/scripts/calculation_time_plotter.py similarity index 100% rename from planning/path_smoother/scripts/calculation_time_plotter.py rename to planning/autoware_path_smoother/scripts/calculation_time_plotter.py diff --git a/planning/path_smoother/src/elastic_band.cpp b/planning/autoware_path_smoother/src/elastic_band.cpp similarity index 98% rename from planning/path_smoother/src/elastic_band.cpp rename to planning/autoware_path_smoother/src/elastic_band.cpp index 8e538a0d6507f..a443cfd37caec 100644 --- a/planning/path_smoother/src/elastic_band.cpp +++ b/planning/autoware_path_smoother/src/elastic_band.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_smoother/elastic_band.hpp" +#include "autoware_path_smoother/elastic_band.hpp" +#include "autoware_path_smoother/type_alias.hpp" +#include "autoware_path_smoother/utils/geometry_utils.hpp" +#include "autoware_path_smoother/utils/trajectory_utils.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_smoother/type_alias.hpp" -#include "path_smoother/utils/geometry_utils.hpp" -#include "path_smoother/utils/trajectory_utils.hpp" #include "tf2/utils.h" #include @@ -84,7 +84,7 @@ std_msgs::msg::Header createHeader(const rclcpp::Time & now) } } // namespace -namespace path_smoother +namespace autoware::path_smoother { EBPathSmoother::EBParam::EBParam(rclcpp::Node * node) { @@ -453,4 +453,4 @@ std::optional> EBPathSmoother::convertOptimizedPoin time_keeper_ptr_->toc(__func__, " "); return eb_traj_points; } -} // namespace path_smoother +} // namespace autoware::path_smoother diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp similarity index 97% rename from planning/path_smoother/src/elastic_band_smoother.cpp rename to planning/autoware_path_smoother/src/elastic_band_smoother.cpp index 75300286ac9dc..2f507c8bae311 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/autoware_path_smoother/src/elastic_band_smoother.cpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_smoother/elastic_band_smoother.hpp" +#include "autoware_path_smoother/elastic_band_smoother.hpp" +#include "autoware_path_smoother/utils/geometry_utils.hpp" +#include "autoware_path_smoother/utils/trajectory_utils.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_smoother/utils/geometry_utils.hpp" -#include "path_smoother/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" #include #include -namespace path_smoother +namespace autoware::path_smoother { namespace { @@ -70,7 +70,7 @@ bool hasZeroVelocity(const TrajectoryPoint & traj_point) } // namespace ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_options) -: Node("path_smoother", node_options), time_keeper_ptr_(std::make_shared()) +: Node("autoware_path_smoother", node_options), time_keeper_ptr_(std::make_shared()) { // interface publisher traj_pub_ = create_publisher("~/output/traj", 1); @@ -384,7 +384,7 @@ std::vector ElasticBandSmoother::extendTrajectory( time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } -} // namespace path_smoother +} // namespace autoware::path_smoother #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(path_smoother::ElasticBandSmoother) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_smoother::ElasticBandSmoother) diff --git a/planning/path_smoother/src/replan_checker.cpp b/planning/autoware_path_smoother/src/replan_checker.cpp similarity index 97% rename from planning/path_smoother/src/replan_checker.cpp rename to planning/autoware_path_smoother/src/replan_checker.cpp index f451a05a8f835..4e5911ea5b42e 100644 --- a/planning/path_smoother/src/replan_checker.cpp +++ b/planning/autoware_path_smoother/src/replan_checker.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_smoother/replan_checker.hpp" +#include "autoware_path_smoother/replan_checker.hpp" +#include "autoware_path_smoother/utils/trajectory_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_smoother/utils/trajectory_utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include -namespace path_smoother +namespace autoware::path_smoother { ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) : ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) @@ -208,4 +208,4 @@ bool ReplanChecker::isPathGoalChanged( return true; } -} // namespace path_smoother +} // namespace autoware::path_smoother diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp similarity index 94% rename from planning/path_smoother/src/utils/trajectory_utils.cpp rename to planning/autoware_path_smoother/src/utils/trajectory_utils.cpp index b4d0950a593d5..5fea20c9a405f 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_smoother/src/utils/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_smoother/utils/trajectory_utils.hpp" +#include "autoware_path_smoother/utils/trajectory_utils.hpp" +#include "autoware_path_smoother/utils/geometry_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_smoother/utils/geometry_utils.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory_point.hpp" @@ -30,7 +30,7 @@ #include #include -namespace path_smoother +namespace autoware::path_smoother { namespace trajectory_utils { @@ -98,4 +98,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace path_smoother +} // namespace autoware::path_smoother diff --git a/planning/path_smoother/test/test_path_smoother_node_interface.cpp b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp similarity index 76% rename from planning/path_smoother/test/test_path_smoother_node_interface.cpp rename to planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp index 30e9fba1433cb..41c48630c8f8c 100644 --- a/planning/path_smoother/test/test_path_smoother_node_interface.cpp +++ b/planning/autoware_path_smoother/test/test_autoware_path_smoother_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_smoother/elastic_band_smoother.hpp" +#include "autoware_path_smoother/elastic_band_smoother.hpp" #include #include @@ -32,24 +32,26 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); - const auto path_smoothing_dir = ament_index_cpp::get_package_share_directory("path_smoother"); + const auto path_smoothing_dir = + ament_index_cpp::get_package_share_directory("autoware_path_smoother"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_common.param.yaml", "--params-file", planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", path_smoothing_dir + "/config/elastic_band_smoother.param.yaml"}); - auto test_target_node = std::make_shared(node_options); + auto test_target_node = + std::make_shared(node_options); // publish necessary topics from test_manager - test_manager->publishOdometry(test_target_node, "path_smoother/input/odometry"); + test_manager->publishOdometry(test_target_node, "autoware_path_smoother/input/odometry"); // set subscriber with topic name - test_manager->setTrajectorySubscriber("path_smoother/output/traj"); - test_manager->setPathSubscriber("path_smoother/output/path"); + test_manager->setTrajectorySubscriber("autoware_path_smoother/output/traj"); + test_manager->setPathSubscriber("autoware_path_smoother/output/path"); // set input topic name (this topic is changed to test node) - test_manager->setPathInputTopicName("path_smoother/input/path"); + test_manager->setPathInputTopicName("autoware_path_smoother/input/path"); // test with normal trajectory ASSERT_NO_THROW_WITH_ERROR_MSG(test_manager->testWithNominalPath(test_target_node)); diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index fe5484ca498ee..97eb2fd8d94fd 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -41,7 +41,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", - planning_validator_dir + "/config/planning_validator.param.yaml"}); + autoware_planning_validator_dir + "/config/planning_validator.param.yaml"}); // instantiate the TargetNode with node_options auto test_target_node = std::make_shared(node_options); @@ -70,18 +70,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) ## Implemented tests -| Node | Test name | exceptional input | output | Exceptional input pattern | -| ------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | -| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | -| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | -| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | -| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | +| Node | Test name | exceptional input | output | Exceptional input pattern | +| --------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | +| autoware_planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | +| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | +| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | +| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | ## Important Notes diff --git a/planning/rtc_interface/CMakeLists.txt b/planning/autoware_rtc_interface/CMakeLists.txt similarity index 85% rename from planning/rtc_interface/CMakeLists.txt rename to planning/autoware_rtc_interface/CMakeLists.txt index fda6025b84e86..cd36121e6aee8 100644 --- a/planning/rtc_interface/CMakeLists.txt +++ b/planning/autoware_rtc_interface/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(rtc_interface) +project(autoware_rtc_interface) ### Compile options if(NOT CMAKE_CXX_STANDARD) @@ -12,7 +12,7 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -ament_auto_add_library(rtc_interface SHARED +ament_auto_add_library(autoware_rtc_interface SHARED src/rtc_interface.cpp ) diff --git a/planning/rtc_interface/README.md b/planning/autoware_rtc_interface/README.md similarity index 79% rename from planning/rtc_interface/README.md rename to planning/autoware_rtc_interface/README.md index 7f8f73edcc6b2..19d200592bcf3 100644 --- a/planning/rtc_interface/README.md +++ b/planning/autoware_rtc_interface/README.md @@ -10,7 +10,7 @@ RTC Interface is an interface to publish the decision status of behavior plannin ```c++ // Generate instance (in this example, "intersection" is selected) -rtc_interface::RTCInterface rtc_interface(node, "intersection"); +autoware::rtc_interface::RTCInterface rtc_interface(node, "intersection"); // Generate UUID const unique_identifier_msgs::msg::UUID uuid = generateUUID(getModuleId()); @@ -51,12 +51,12 @@ rtc_interface.removeCooperateStatus(uuid); ### RTCInterface (Constructor) ```c++ -rtc_interface::RTCInterface(rclcpp::Node & node, const std::string & name); +autoware::rtc_interface::RTCInterface(rclcpp::Node & node, const std::string & name); ``` #### Description -A constructor for `rtc_interface::RTCInterface`. +A constructor for `autoware::rtc_interface::RTCInterface`. #### Input @@ -72,7 +72,7 @@ An instance of `RTCInterface` ### publishCooperateStatus ```c++ -rtc_interface::publishCooperateStatus(const rclcpp::Time & stamp) +autoware::rtc_interface::publishCooperateStatus(const rclcpp::Time & stamp) ``` #### Description @@ -90,7 +90,7 @@ Nothing ### updateCooperateStatus ```c++ -rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double start_distance, const double finish_distance, const rclcpp::Time & stamp) +autoware::rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double start_distance, const double finish_distance, const rclcpp::Time & stamp) ``` #### Description @@ -113,7 +113,7 @@ Nothing ### removeCooperateStatus ```c++ -rtc_interface::removeCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid) +autoware::rtc_interface::removeCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid) ``` #### Description @@ -131,7 +131,7 @@ Nothing ### clearCooperateStatus ```c++ -rtc_interface::clearCooperateStatus() +autoware::rtc_interface::clearCooperateStatus() ``` #### Description @@ -149,7 +149,7 @@ Nothing ### isActivated ```c++ -rtc_interface::isActivated(const unique_identifier_msgs::msg::UUID & uuid) +autoware::rtc_interface::isActivated(const unique_identifier_msgs::msg::UUID & uuid) ``` #### Description @@ -169,7 +169,7 @@ If not, return `false`. ### isRegistered ```c++ -rtc_interface::isRegistered(const unique_identifier_msgs::msg::UUID & uuid) +autoware::rtc_interface::isRegistered(const unique_identifier_msgs::msg::UUID & uuid) ``` #### Description diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware_rtc_interface/rtc_interface.hpp similarity index 94% rename from planning/rtc_interface/include/rtc_interface/rtc_interface.hpp rename to planning/autoware_rtc_interface/include/autoware_rtc_interface/rtc_interface.hpp index 7ab796321e041..8b435a6f8ed51 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/autoware_rtc_interface/include/autoware_rtc_interface/rtc_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RTC_INTERFACE__RTC_INTERFACE_HPP_ -#define RTC_INTERFACE__RTC_INTERFACE_HPP_ +#ifndef AUTOWARE_RTC_INTERFACE__RTC_INTERFACE_HPP_ +#define AUTOWARE_RTC_INTERFACE__RTC_INTERFACE_HPP_ #include "rclcpp/rclcpp.hpp" @@ -33,7 +33,7 @@ #include #include -namespace rtc_interface +namespace autoware::rtc_interface { using tier4_rtc_msgs::msg::AutoModeStatus; using tier4_rtc_msgs::msg::Command; @@ -101,6 +101,6 @@ class RTCInterface mutable std::mutex mutex_; }; -} // namespace rtc_interface +} // namespace autoware::rtc_interface -#endif // RTC_INTERFACE__RTC_INTERFACE_HPP_ +#endif // AUTOWARE_RTC_INTERFACE__RTC_INTERFACE_HPP_ diff --git a/planning/rtc_interface/package.xml b/planning/autoware_rtc_interface/package.xml similarity index 88% rename from planning/rtc_interface/package.xml rename to planning/autoware_rtc_interface/package.xml index 753e4df13908e..d08467d8f15a9 100644 --- a/planning/rtc_interface/package.xml +++ b/planning/autoware_rtc_interface/package.xml @@ -1,8 +1,8 @@ - rtc_interface + autoware_rtc_interface 0.1.0 - The rtc_interface package + The autoware_rtc_interface package Fumiya Watanabe Taiki Tanaka diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp similarity index 98% rename from planning/rtc_interface/src/rtc_interface.cpp rename to planning/autoware_rtc_interface/src/rtc_interface.cpp index 77fe29b41d893..3389d08b9015e 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "rtc_interface/rtc_interface.hpp" +#include "autoware_rtc_interface/rtc_interface.hpp" #include @@ -78,7 +78,7 @@ Module getModuleType(const std::string & module_name) } // namespace -namespace rtc_interface +namespace autoware::rtc_interface { RTCInterface::RTCInterface(rclcpp::Node * node, const std::string & name, const bool enable_rtc) : clock_{node->get_clock()}, @@ -363,4 +363,4 @@ bool RTCInterface::isLocked() const return is_locked_; } -} // namespace rtc_interface +} // namespace autoware::rtc_interface diff --git a/planning/scenario_selector/CMakeLists.txt b/planning/autoware_scenario_selector/CMakeLists.txt similarity index 54% rename from planning/scenario_selector/CMakeLists.txt rename to planning/autoware_scenario_selector/CMakeLists.txt index d2e9e051d721a..e615ed69d61bf 100644 --- a/planning/scenario_selector/CMakeLists.txt +++ b/planning/autoware_scenario_selector/CMakeLists.txt @@ -1,16 +1,16 @@ cmake_minimum_required(VERSION 3.14) -project(scenario_selector) +project(autoware_scenario_selector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(scenario_selector_node SHARED - src/scenario_selector_node/scenario_selector_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED + src/node.cpp ) -rclcpp_components_register_node(scenario_selector_node - PLUGIN "ScenarioSelectorNode" - EXECUTABLE scenario_selector +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::scenario_selector::ScenarioSelectorNode" + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) @@ -18,11 +18,10 @@ if(BUILD_TESTING) test/test_${PROJECT_NAME}_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} - ${PROJECT_NAME}_node + ${PROJECT_NAME} ) endif() - ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/planning/scenario_selector/README.md b/planning/autoware_scenario_selector/README.md similarity index 91% rename from planning/scenario_selector/README.md rename to planning/autoware_scenario_selector/README.md index 373ac164e5057..a0b78a572c734 100644 --- a/planning/scenario_selector/README.md +++ b/planning/autoware_scenario_selector/README.md @@ -1,4 +1,4 @@ -# scenario_selector +# autoware_scenario_selector ## scenario_selector_node @@ -29,12 +29,12 @@ None ### How to launch 1. Write your remapping info in `scenario_selector.launch` or add args when executing `roslaunch` -2. `roslaunch scenario_selector scenario_selector.launch` - - If you would like to use only a single scenario, `roslaunch scenario_selector dummy_scenario_selector_{scenario_name}.launch` +2. `roslaunch autoware_scenario_selector scenario_selector.launch` + - If you would like to use only a single scenario, `roslaunch autoware_scenario_selector dummy_scenario_selector_{scenario_name}.launch` ### Parameters -{{ json_to_markdown("planning/scenario_selector/schema/scenario_selector.schema.json") }} +{{ json_to_markdown("planning/autoware_scenario_selector/schema/scenario_selector.schema.json") }} ### Flowchart diff --git a/planning/scenario_selector/config/scenario_selector.param.yaml b/planning/autoware_scenario_selector/config/scenario_selector.param.yaml similarity index 100% rename from planning/scenario_selector/config/scenario_selector.param.yaml rename to planning/autoware_scenario_selector/config/scenario_selector.param.yaml diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/autoware_scenario_selector/include/autoware_scenario_selector/node.hpp similarity index 94% rename from planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp rename to planning/autoware_scenario_selector/include/autoware_scenario_selector/node.hpp index 7162a435d8852..5551db45bba37 100644 --- a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp +++ b/planning/autoware_scenario_selector/include/autoware_scenario_selector/node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_ -#define SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_ +#ifndef AUTOWARE_SCENARIO_SELECTOR__NODE_HPP_ +#define AUTOWARE_SCENARIO_SELECTOR__NODE_HPP_ #include #include @@ -43,6 +43,8 @@ #include #include +namespace autoware::scenario_selector +{ class ScenarioSelectorNode : public rclcpp::Node { public: @@ -100,5 +102,5 @@ class ScenarioSelectorNode : public rclcpp::Node double th_stopped_velocity_mps_; bool is_parking_completed_; }; - -#endif // SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_ +} // namespace autoware::scenario_selector +#endif // AUTOWARE_SCENARIO_SELECTOR__NODE_HPP_ diff --git a/planning/scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml b/planning/autoware_scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml similarity index 100% rename from planning/scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml rename to planning/autoware_scenario_selector/launch/dummy_scenario_selector_lane_driving.launch.xml diff --git a/planning/scenario_selector/launch/dummy_scenario_selector_parking.launch.xml b/planning/autoware_scenario_selector/launch/dummy_scenario_selector_parking.launch.xml similarity index 100% rename from planning/scenario_selector/launch/dummy_scenario_selector_parking.launch.xml rename to planning/autoware_scenario_selector/launch/dummy_scenario_selector_parking.launch.xml diff --git a/planning/scenario_selector/launch/scenario_selector.launch.xml b/planning/autoware_scenario_selector/launch/scenario_selector.launch.xml similarity index 80% rename from planning/scenario_selector/launch/scenario_selector.launch.xml rename to planning/autoware_scenario_selector/launch/scenario_selector.launch.xml index fdf176405d7b0..8c35451ee19dc 100644 --- a/planning/scenario_selector/launch/scenario_selector.launch.xml +++ b/planning/autoware_scenario_selector/launch/scenario_selector.launch.xml @@ -9,9 +9,9 @@ - + - + diff --git a/planning/scenario_selector/package.xml b/planning/autoware_scenario_selector/package.xml similarity index 93% rename from planning/scenario_selector/package.xml rename to planning/autoware_scenario_selector/package.xml index f910c09dbb070..ee58eac35c8ae 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/autoware_scenario_selector/package.xml @@ -1,9 +1,9 @@ - scenario_selector + autoware_scenario_selector 0.1.0 - The scenario_selector ROS 2 package + The autoware_scenario_selector ROS 2 package Taiki Tanaka Tomoya Kimura Shumpei Wakabayashi diff --git a/planning/scenario_selector/schema/scenario_selector.schema.json b/planning/autoware_scenario_selector/schema/scenario_selector.schema.json similarity index 94% rename from planning/scenario_selector/schema/scenario_selector.schema.json rename to planning/autoware_scenario_selector/schema/scenario_selector.schema.json index 8ccb75106c6a5..a9e6411084a88 100644 --- a/planning/scenario_selector/schema/scenario_selector.schema.json +++ b/planning/autoware_scenario_selector/schema/scenario_selector.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Scenario Selector Node", "type": "object", "definitions": { - "scenario_selector": { + "autoware_scenario_selector": { "type": "object", "properties": { "update_rate": { @@ -51,7 +51,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/scenario_selector" + "$ref": "#/definitions/autoware_scenario_selector" } }, "required": ["ros__parameters"] diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/autoware_scenario_selector/src/node.cpp similarity index 98% rename from planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp rename to planning/autoware_scenario_selector/src/node.cpp index 2ba56a911bbeb..1f1a22cf08deb 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/autoware_scenario_selector/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scenario_selector/scenario_selector_node.hpp" +#include "autoware_scenario_selector/node.hpp" #include #include @@ -28,7 +28,8 @@ #include #include #include - +namespace autoware::scenario_selector +{ namespace { template @@ -373,6 +374,7 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti this, get_clock(), period_ns, std::bind(&ScenarioSelectorNode::onTimer, this)); published_time_publisher_ = std::make_unique(this); } +} // namespace autoware::scenario_selector #include -RCLCPP_COMPONENTS_REGISTER_NODE(ScenarioSelectorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::scenario_selector::ScenarioSelectorNode) diff --git a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp similarity index 97% rename from planning/scenario_selector/test/test_scenario_selector_node_interface.cpp rename to planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp index 90995e4e2ae72..052c455c477a7 100644 --- a/planning/scenario_selector/test/test_scenario_selector_node_interface.cpp +++ b/planning/autoware_scenario_selector/test/test_autoware_scenario_selector_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "scenario_selector/scenario_selector_node.hpp" +#include "autoware_scenario_selector/node.hpp" #include #include @@ -22,7 +22,8 @@ #include #include - +namespace autoware::scenario_selector +{ using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -121,3 +122,4 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithOffTrackEgoPose) ASSERT_NO_THROW(test_manager->testTrajectoryWithInvalidEgoPose(test_target_node)); rclcpp::shutdown(); } +} // namespace autoware::scenario_selector 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 index 486ea547e3dc1..3d786ab995e5b 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -28,7 +28,7 @@ default="$(find-pkg-share autoware_launch)/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml" /> - + diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 4cf19c1da0e6a..a5e09ceb6270b 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -18,8 +18,10 @@ autoware_behavior_path_planner_common autoware_map_msgs autoware_path_optimizer + autoware_path_smoother autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils geography_utils geometry_msgs global_parameter_loader @@ -30,13 +32,11 @@ mission_planner motion_utils osqp_interface - path_smoother rclcpp rclcpp_components route_handler tier4_autoware_utils tier4_map_msgs - vehicle_info_util python3-flask-cors rosidl_default_runtime 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 index 9eb9a2b432a21..7dd3a20848aca 100644 --- 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 @@ -15,9 +15,9 @@ #include "centerline_source/optimization_trajectory_based_centerline.hpp" #include "autoware_path_optimizer/node.hpp" +#include "autoware_path_smoother/elastic_band_smoother.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_generator_node.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include "utils.hpp" @@ -128,9 +128,9 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // create an instance of elastic band and model predictive trajectory. const auto eb_path_smoother_ptr = - path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); + autoware::path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); const auto mpt_optimizer_ptr = - autoware_path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); + 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; @@ -152,13 +152,13 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra virtual_ego_pose_offset_idx) .pose; - // smooth trajectory by elastic band in the path_smoother package + // 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{ + 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); 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 index 24e95353eec93..b8a3a8baf67a3 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -77,8 +77,8 @@ lanelet::BasicPoint2d convert_to_lanelet_point(const geometry_msgs::msg::Point & } LinearRing2d create_vehicle_footprint( - const geometry_msgs::msg::Pose & pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double margin = 0.0) + const geometry_msgs::msg::Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0) { const auto & i = vehicle_info; @@ -105,7 +105,8 @@ LinearRing2d create_vehicle_footprint( } geometry_msgs::msg::Pose get_text_pose( - const geometry_msgs::msg::Pose & pose, const vehicle_info_util::VehicleInfo & vehicle_info) + const geometry_msgs::msg::Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const auto & i = vehicle_info; @@ -238,7 +239,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( rmw_qos_profile_services_default, callback_group_); // vehicle info - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); centerline_source_ = [&]() { const auto centerline_source_param = declare_parameter("centerline_source"); 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 index f8a65f2794809..2f25a064dca2a 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -18,10 +18,10 @@ #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 "vehicle_info_util/vehicle_info_util.hpp" #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" @@ -115,7 +115,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_; // vehicle info - vehicle_info_util::VehicleInfo 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/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py index 7a5cdfd905fe9..6316afc8cb3dc 100644 --- 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 @@ -63,7 +63,7 @@ def generate_test_description(): "config/behavior_velocity_planner.param.yaml", ), os.path.join( - get_package_share_directory("path_smoother"), + get_package_share_directory("autoware_path_smoother"), "config/elastic_band_smoother.param.yaml", ), os.path.join( diff --git a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp index d2e055bbccaa2..c7ec2e5b564e6 100644 --- a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ #define AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +#include #include -#include #include #include @@ -34,6 +34,7 @@ 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; @@ -41,7 +42,6 @@ using geometry_msgs::msg::PolygonStamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; -using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -56,7 +56,7 @@ class SurroundObstacleCheckerDebugNode { public: explicit SurroundObstacleCheckerDebugNode( - const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double base_link2front, const std::string & object_label, const double & surround_check_front_distance, const double & surround_check_side_distance, const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, @@ -79,7 +79,7 @@ class SurroundObstacleCheckerDebugNode rclcpp::Publisher::SharedPtr vehicle_footprint_offset_pub_; rclcpp::Publisher::SharedPtr vehicle_footprint_recover_offset_pub_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; double base_link2front_; std::string object_label_; double surround_check_front_distance_; diff --git a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp index 4ccd097b40837..4eb094e6617a4 100644 --- a/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp +++ b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp @@ -19,9 +19,9 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" +#include #include #include -#include #include #include @@ -46,12 +46,12 @@ namespace autoware::surround_obstacle_checker { +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using motion_utils::VehicleStopChecker; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; using Obstacle = std::pair; @@ -132,7 +132,7 @@ class SurroundObstacleCheckerNode : public rclcpp::Node // parameter NodeParam node_param_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; // data nav_msgs::msg::Odometry::ConstSharedPtr odometry_ptr_; diff --git a/planning/autoware_surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml index 3bb961766e9f3..530f850a3c34a 100644 --- a/planning/autoware_surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -17,6 +17,7 @@ autoware_adapi_v1_msgs autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils diagnostic_msgs eigen libpcl-all-dev @@ -32,7 +33,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index ba561a4b8cf35..b676673128269 100644 --- a/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -59,7 +59,7 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; SurroundObstacleCheckerDebugNode::SurroundObstacleCheckerDebugNode( - const vehicle_info_util::VehicleInfo & vehicle_info, const double base_link2front, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double base_link2front, const std::string & object_label, const double & surround_check_front_distance, const double & surround_check_side_distance, const double & surround_check_back_distance, const double & surround_check_hysteresis_distance, const geometry_msgs::msg::Pose & self_pose, diff --git a/planning/autoware_surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp index 0744ebecd370c..8d6279eeefb22 100644 --- a/planning/autoware_surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -196,7 +196,7 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio logger_configure_ = std::make_unique(this); } - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); // Publishers pub_stop_reason_ = diff --git a/planning/autoware_velocity_smoother/CMakeLists.txt b/planning/autoware_velocity_smoother/CMakeLists.txt index fee13513a7a97..9dd0c62c4537e 100644 --- a/planning/autoware_velocity_smoother/CMakeLists.txt +++ b/planning/autoware_velocity_smoother/CMakeLists.txt @@ -41,7 +41,7 @@ target_link_libraries(${PROJECT_NAME}_node ) rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "autoware_velocity_smoother::VelocitySmootherNode" + PLUGIN "autoware::velocity_smoother::VelocitySmootherNode" EXECUTABLE velocity_smoother_node ) 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 aecfab342e7e0..160ee183d8a0a 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -31,6 +31,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -53,7 +54,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -86,11 +87,15 @@ class VelocitySmootherNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_over_stop_velocity_; - rclcpp::Subscription::SharedPtr sub_current_odometry_; - rclcpp::Subscription::SharedPtr sub_current_acceleration_; rclcpp::Subscription::SharedPtr sub_current_trajectory_; - rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; - rclcpp::Subscription::SharedPtr sub_operation_mode_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_current_odometry_{ + this, "/localization/kinematic_state"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_current_acceleration_{this, "~/input/acceleration"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_external_velocity_limit_{ + this, "~/input/external_velocity_limit_mps"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/operation_mode_state"}; Odometry::ConstSharedPtr current_odometry_ptr_; // current odometry AccelWithCovarianceStamped::ConstSharedPtr current_acceleration_ptr_; @@ -180,12 +185,8 @@ class VelocitySmootherNode : public rclcpp::Node const std::vector & parameters); // topic callback - void onCurrentOdometry(const Odometry::ConstSharedPtr msg); - void onCurrentTrajectory(const Trajectory::ConstSharedPtr msg); - void onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg); - void calcExternalVelocityLimit(); // publish methods @@ -275,6 +276,6 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp index dc85c2b2f336f..24a20ca8a588f 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp @@ -20,7 +20,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace resampling { @@ -48,6 +48,6 @@ TrajectoryPoints resampleTrajectory( const double nearest_dist_threshold, const double nearest_yaw_threshold, const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); } // namespace resampling -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index e54a6e7b8afad..a5baa59f5d034 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -28,7 +28,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class AnalyticalJerkConstrainedSmoother : public SmootherBase { @@ -113,7 +113,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase std::string strTimes(const std::vector & times) const; std::string strStartIndices(const std::vector & start_indices) const; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother // clang-format off #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 3ab4fce11fd0b..0c18e102f6f66 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace analytical_velocity_planning_utils { @@ -51,7 +51,7 @@ double integ_x(double x0, double v0, double a0, double j0, double t); double integ_v(double v0, double a0, double j0, double t); double integ_a(double a0, double j0, double t); } // namespace analytical_velocity_planning_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother // clang-format off #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 09c1e7e96be7b..4e93fcd339140 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class JerkFilteredSmoother : public SmootherBase { @@ -69,6 +69,6 @@ class JerkFilteredSmoother : public SmootherBase const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 066e0acef67f6..44fa263e71fb9 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class L2PseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class L2PseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index b54de318e9702..f638e03658e05 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class LinfPseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class LinfPseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp index 43ccfcf193f14..0c17ef5bdbb9d 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -86,6 +86,6 @@ class SmootherBase protected: BaseParam base_param_; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp index d35f80fb7fad8..f7999d1c8b04c 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother::trajectory_utils +namespace autoware::velocity_smoother::trajectory_utils { using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -77,6 +77,6 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); -} // namespace autoware_velocity_smoother::trajectory_utils +} // namespace autoware::velocity_smoother::trajectory_utils #endif // AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index 4b5a46e495545..2bfc32ec90a5f 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -22,6 +22,7 @@ autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils geometry_msgs interpolation motion_utils @@ -33,7 +34,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util ament_cmake_ros ament_lint_auto diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 0ea10534eb126..be1f77fc951b1 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -20,7 +20,7 @@ #include "motion_utils/marker/marker_helper.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" -#include +#include #include #include @@ -32,7 +32,7 @@ #include // clang-format on -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) : Node("velocity_smoother", node_options) @@ -40,7 +40,7 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti using std::placeholders::_1; // set common params - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); wheelbase_ = vehicle_info.wheel_base_m; base_link2front_ = vehicle_info.max_longitudinal_offset_m; initCommonParam(); @@ -58,19 +58,6 @@ VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_opti pub_over_stop_velocity_ = create_publisher("~/stop_speed_exceeded", 1); sub_current_trajectory_ = create_subscription( "~/input/trajectory", 1, std::bind(&VelocitySmootherNode::onCurrentTrajectory, this, _1)); - sub_current_odometry_ = create_subscription( - "/localization/kinematic_state", 1, - std::bind(&VelocitySmootherNode::onCurrentOdometry, this, _1)); - sub_external_velocity_limit_ = create_subscription( - "~/input/external_velocity_limit_mps", 1, - std::bind(&VelocitySmootherNode::onExternalVelocityLimit, this, _1)); - sub_current_acceleration_ = create_subscription( - "~/input/acceleration", 1, [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { - current_acceleration_ptr_ = msg; - }); - sub_operation_mode_ = create_subscription( - "~/input/operation_mode_state", 1, - [this](const OperationModeState::ConstSharedPtr msg) { operation_mode_ = *msg; }); // parameter update set_param_res_ = @@ -319,16 +306,6 @@ void VelocitySmootherNode::publishTrajectory(const TrajectoryPoints & trajectory pub_trajectory_, publishing_trajectory.header.stamp); } -void VelocitySmootherNode::onCurrentOdometry(const Odometry::ConstSharedPtr msg) -{ - current_odometry_ptr_ = msg; -} - -void VelocitySmootherNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) -{ - external_velocity_limit_ptr_ = msg; -} - void VelocitySmootherNode::calcExternalVelocityLimit() { if (!external_velocity_limit_ptr_) { @@ -441,6 +418,15 @@ void VelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstSharedPtr base_traj_raw_ptr_ = msg; + // receive data + current_odometry_ptr_ = sub_current_odometry_.takeData(); + current_acceleration_ptr_ = sub_current_acceleration_.takeData(); + external_velocity_limit_ptr_ = sub_external_velocity_limit_.takeData(); + const auto operation_mode_ptr = sub_operation_mode_.takeData(); + if (operation_mode_ptr) { + operation_mode_ = *operation_mode_ptr; + } + // guard if (!checkData()) { return; @@ -1099,7 +1085,7 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_velocity_smoother::VelocitySmootherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::velocity_smoother::VelocitySmootherNode) diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp index 17ea1eb9fc9bd..af3b222a71ef9 100644 --- a/planning/autoware_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -22,7 +22,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace resampling { @@ -264,4 +264,4 @@ TrajectoryPoints resampleTrajectory( } } // namespace resampling -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 9cdfc515d51b7..dc5f4f19ce771 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -64,7 +64,7 @@ bool applyMaxVelocity( } // namespace -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) : SmootherBase(node) @@ -658,4 +658,4 @@ std::string AnalyticalJerkConstrainedSmoother::strStartIndices( return ss.str(); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 84ac29d14bdc5..e3edcfbbec681 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -19,7 +19,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace analytical_velocity_planning_utils { @@ -353,4 +353,4 @@ double integ_a(double a0, double j0, double t) } } // namespace analytical_velocity_planning_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index d212cfc0bcedf..d759930eb4320 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -27,7 +27,7 @@ #define VERBOSE_TRAJECTORY_VELOCITY false -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -480,4 +480,4 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( smoother_param_.jerk_filter_ds); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index 3c2872f2e58e3..fff1166567aee 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -242,4 +242,4 @@ TrajectoryPoints L2PseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index f434d8d4514ca..b00936f3092ad 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -254,4 +254,4 @@ TrajectoryPoints LinfPseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 5704113067244..e1d72889203cc 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -26,7 +26,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace @@ -270,4 +270,4 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( return output; } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index aff1a0b0e3213..6ebf246748666 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using geometry_msgs::msg::Point; namespace trajectory_utils @@ -610,4 +610,4 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes } } // namespace trajectory_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index 51ee84f535ca4..f1b7dbb197ea2 100644 --- a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -18,8 +18,8 @@ #include +using autoware::velocity_smoother::trajectory_utils::TrajectoryPoints; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_velocity_smoother::trajectory_utils::TrajectoryPoints; TrajectoryPoints genStraightTrajectory(const size_t size) { @@ -48,7 +48,7 @@ TEST(TestTrajectoryUtils, CalcTrajectoryCurvatureFrom3Points) const auto checkOutputSize = [](const size_t trajectory_size, const size_t idx_dist) { const auto trajectory_points = genStraightTrajectory(trajectory_size); const auto curvatures = - autoware_velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( + autoware::velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( trajectory_points, idx_dist); EXPECT_EQ(curvatures.size(), trajectory_size) << ", idx_dist = " << idx_dist; }; diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index f0145ea5a32a7..30a64955366e4 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -22,7 +22,7 @@ #include -using autoware_velocity_smoother::VelocitySmootherNode; +using autoware::velocity_smoother::VelocitySmootherNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() diff --git a/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml b/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml deleted file mode 100644 index 092d54c096ae9..0000000000000 --- a/planning/behavior_path_avoidance_by_lane_change_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp index ae1ee8c66a990..f05d0b9b3b973 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -17,9 +17,9 @@ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include -#include -#include +#include +#include +#include #include @@ -28,16 +28,16 @@ namespace behavior_path_planner { -using freespace_planning_algorithms::AbstractPlanningAlgorithm; -using freespace_planning_algorithms::AstarSearch; -using freespace_planning_algorithms::RRTStar; +using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::RRTStar; class FreespacePullOver : public PullOverPlannerBase { public: FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index 0009fbccdb5d8..903655b64e347 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -18,7 +18,7 @@ #include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include +#include #include @@ -27,7 +27,7 @@ namespace behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class GeometricPullOver : public PullOverPlannerBase { public: diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 8938e48e0ceb5..66f7274f888be 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -28,9 +28,9 @@ #include "behavior_path_goal_planner_module/goal_searcher.hpp" #include "behavior_path_goal_planner_module/shift_pull_over.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -50,19 +50,19 @@ namespace behavior_path_planner { +using autoware::lane_departure_checker::LaneDepartureChecker; using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; -using lane_departure_checker::LaneDepartureChecker; using nav_msgs::msg::OccupancyGrid; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; -using freespace_planning_algorithms::AbstractPlanningAlgorithm; -using freespace_planning_algorithms::AstarParam; -using freespace_planning_algorithms::AstarSearch; -using freespace_planning_algorithms::PlannerCommonParam; -using freespace_planning_algorithms::RRTStar; -using freespace_planning_algorithms::RRTStarParam; +using autoware::freespace_planning_algorithms::AbstractPlanningAlgorithm; +using autoware::freespace_planning_algorithms::AstarParam; +using autoware::freespace_planning_algorithms::AstarSearch; +using autoware::freespace_planning_algorithms::PlannerCommonParam; +using autoware::freespace_planning_algorithms::RRTStar; +using autoware::freespace_planning_algorithms::RRTStarParam; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; @@ -465,7 +465,7 @@ class GoalPlannerModule : public SceneModuleInterface mutable std::shared_ptr objects_filtering_params_; mutable std::shared_ptr safety_check_params_; - vehicle_info_util::VehicleInfo vehicle_info_{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; // planner std::vector> pull_over_planners_; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp index e5b284d03d9c4..dc992b7ed4967 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -19,9 +19,9 @@ #include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -29,9 +29,9 @@ namespace behavior_path_planner { -using freespace_planning_algorithms::AstarParam; -using freespace_planning_algorithms::PlannerCommonParam; -using freespace_planning_algorithms::RRTStarParam; +using autoware::freespace_planning_algorithms::AstarParam; +using autoware::freespace_planning_algorithms::PlannerCommonParam; +using autoware::freespace_planning_algorithms::RRTStarParam; enum class ParkingPolicy { LEFT_SIDE = 0, diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index ca84e7256a01f..c916e6be10d9f 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -112,7 +112,7 @@ class PullOverPlannerBase public: PullOverPlannerBase(rclcpp::Node & node, const GoalPlannerParameters & parameters) { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info_.createFootprint(); parameters_ = parameters; } @@ -133,7 +133,7 @@ class PullOverPlannerBase protected: std::shared_ptr planner_data_; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; LinearRing2d vehicle_footprint_; GoalPlannerParameters parameters_; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index 635816f17497f..e7b297548083a 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -17,7 +17,7 @@ #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include +#include #include @@ -26,7 +26,7 @@ namespace behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; class ShiftPullOver : public PullOverPlannerBase { diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index 83e817a8d5a45..3de0ba0c99a35 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -18,7 +18,7 @@ #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include +#include #include "visualization_msgs/msg/detail/marker_array__struct.hpp" #include @@ -58,8 +58,9 @@ lanelet::ConstLanelets generateExpandedPullOverLanes( lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, - const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double outer_road_offset, const double inner_road_offset); + const geometry_msgs::msg::Pose ego_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset, + const double inner_road_offset); PredictedObjects extractObjectsInExpandedPullOverLanes( const RouteHandler & route_handler, const bool left_side, const double backward_distance, const double forward_distance, double bound_offset, const PredictedObjects & objects); diff --git a/planning/behavior_path_goal_planner_module/package.xml b/planning/behavior_path_goal_planner_module/package.xml index c5201c1d2d9a2..75ca5c264b523 100644 --- a/planning/behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_goal_planner_module/package.xml @@ -21,11 +21,11 @@ eigen3_cmake_module autoware_behavior_path_planner_common + autoware_rtc_interface behavior_path_planner motion_utils pluginlib rclcpp - rtc_interface tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp index c17d7d38709e5..0a75e5ce94f79 100644 --- a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -27,12 +27,12 @@ namespace behavior_path_planner { FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const GoalPlannerParameters & parameters, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) : PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity}, left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { - freespace_planning_algorithms::VehicleShape vehicle_shape( + autoware::freespace_planning_algorithms::VehicleShape vehicle_shape( vehicle_info, parameters.vehicle_shape_margin); if (parameters.freespace_parking_algorithm == "astar") { use_back_ = parameters.astar_parameters.use_back; diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index d85205e001d0b..20fe808f51433 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -60,7 +60,7 @@ GoalPlannerModule::GoalPlannerModule( objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, thread_safe_data_{mutex_, clock_}, is_lane_parking_cb_running_{false}, is_freespace_parking_cb_running_{false}, @@ -95,7 +95,7 @@ GoalPlannerModule::GoalPlannerModule( // set selected goal searcher // currently there is only one goal_searcher_type - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); vehicle_footprint_ = vehicle_info.createFootprint(); goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_); diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index efd9eda322585..b56ae44dcf476 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -32,7 +32,7 @@ namespace behavior_path_planner { -using lane_departure_checker::LaneDepartureChecker; +using autoware::lane_departure_checker::LaneDepartureChecker; using lanelet::autoware::NoParkingArea; using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index 202eefe37bd83..6b538b6e9c7f5 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -105,8 +105,9 @@ static double getOffsetToLanesBoundary( lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes( const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, - const geometry_msgs::msg::Pose ego_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double outer_road_offset, const double inner_road_offset) + const geometry_msgs::msg::Pose ego_pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset, + const double inner_road_offset) { const double front_overhang = vehicle_info.front_overhang_m, wheel_base = vehicle_info.wheel_base_m, wheel_tread = vehicle_info.wheel_tread_m; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index e9a9ab15f6bc0..a7f65f82f4e9e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -241,7 +241,7 @@ rclcpp::Logger getLogger(const std::string & type); * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info); + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info); /** * @brief Checks if the given polygon is within an intersection area. @@ -304,7 +304,7 @@ namespace behavior_path_planner::utils::lane_change::debug geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose); geometry_msgs::msg::Polygon createExecutionArea( - const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, double additional_lat_offset); } // namespace behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_lane_change_module/package.xml b/planning/behavior_path_lane_change_module/package.xml index ee4089375376f..23fc8f61af2f3 100644 --- a/planning/behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_lane_change_module/package.xml @@ -20,11 +20,11 @@ eigen3_cmake_module autoware_behavior_path_planner_common + autoware_rtc_interface behavior_path_planner motion_utils pluginlib rclcpp - rtc_interface tier4_autoware_utils tier4_planning_msgs visualization_msgs diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 20b502886ccef..2fafd7c41d4a3 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -25,6 +25,7 @@ #include "object_recognition_utils/predicted_path_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" +#include #include #include #include @@ -33,7 +34,6 @@ #include #include #include -#include #include @@ -1159,7 +1159,7 @@ rclcpp::Logger getLogger(const std::string & type) } Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const vehicle_info_util::VehicleInfo & ego_info) + const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info) { const auto base_to_front = ego_info.max_longitudinal_offset_m; const auto base_to_rear = ego_info.rear_overhang_m; @@ -1240,7 +1240,7 @@ geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose }; geometry_msgs::msg::Polygon createExecutionArea( - const vehicle_info_util::VehicleInfo & vehicle_info, const Pose & pose, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset, double additional_lat_offset) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 08a3570b1a271..70190731ac1a7 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -33,7 +33,7 @@ Behavior Path Planner has following scene modules | Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../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](../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](../behavior_path_start_planner_module/README.md) | +| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../autoware_behavior_path_start_planner_module/README.md) | | Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../autoware_behavior_path_side_shift_module/README.md) | !!! Note diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index e4fe3d0e9d3e4..2c88a6d156f3d 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -38,23 +38,24 @@ autoware_adapi_v1_msgs autoware_behavior_path_planner_common + autoware_freespace_planning_algorithms + autoware_frenet_planner + autoware_lane_departure_checker + autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils autoware_vehicle_msgs behaviortree_cpp_v3 - freespace_planning_algorithms - frenet_planner geometry_msgs interpolation - lane_departure_checker lanelet2_extension libboost-dev libopencv-dev magic_enum motion_utils object_recognition_utils - path_sampler pluginlib rclcpp rclcpp_components @@ -66,7 +67,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index e6ebe66f3f440..d5b49f5efa4d2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -19,8 +19,8 @@ #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "motion_utils/trajectory/conversion.hpp" +#include #include -#include #include @@ -44,8 +44,8 @@ rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr) namespace behavior_path_planner { +using autoware::vehicle_info_utils::VehicleInfoUtils; using tier4_planning_msgs::msg::PathChangeModuleId; -using vehicle_info_util::VehicleInfoUtil; BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options) : Node("behavior_path_planner", node_options) @@ -212,7 +212,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); // vehicle info - const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = VehicleInfoUtils(*this).getVehicleInfo(); p.vehicle_info = vehicle_info; p.vehicle_width = vehicle_info.vehicle_width_m; p.vehicle_length = vehicle_info.vehicle_length_m; diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp index 3f3d144952d8e..5cf7b1f4e927d 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_module.hpp @@ -21,25 +21,25 @@ #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_frenet_planner/frenet_planner.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include "behavior_path_sampling_planner_module/util.hpp" -#include "bezier_sampler/bezier_sampling.hpp" -#include "frenet_planner/frenet_planner.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/constraints/footprint.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/soft_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" #include "tier4_planning_msgs/msg/path_with_lane_id.hpp" @@ -73,7 +73,7 @@ struct SamplingPlannerData struct SamplingPlannerDebugData { - std::vector sampled_candidates{}; + std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; std::vector obstacles{}; std::vector footprints{}; @@ -231,25 +231,25 @@ class SamplingPlannerModule : public SceneModuleInterface void updateDebugMarkers(); void prepareConstraints( - sampler_common::Constraints & constraints, + autoware::sampler_common::Constraints & constraints, const PredictedObjects::ConstSharedPtr & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) const; - frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, + autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const SamplingPlannerInternalParameters & internal_params_); PathWithLaneId convertFrenetPathToPathWithLaneID( - const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const autoware::frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, const double path_z); // member // std::shared_ptr params_; std::shared_ptr internal_params_; - vehicle_info_util::VehicleInfo vehicle_info_{}; - std::optional prev_sampling_path_ = std::nullopt; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; + std::optional prev_sampling_path_ = std::nullopt; // move to utils void extendOutputDrivableArea( diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp index 6e00fefe89574..4ddfbf436ffaf 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/sampling_planner_parameters.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ #define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_PARAMETERS_HPP_ -#include "bezier_sampler/bezier_sampling.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_sampler_common/structures.hpp" #include namespace behavior_path_planner @@ -78,12 +78,12 @@ struct Sampling std::vector target_lateral_positions{}; int nb_target_lateral_positions{}; Frenet frenet; - bezier_sampler::SamplingParameters bezier{}; + autoware::bezier_sampler::SamplingParameters bezier{}; }; struct SamplingPlannerInternalParameters { - sampler_common::Constraints constraints; + autoware::sampler_common::Constraints constraints; Sampling sampling; Preprocessing preprocessing{}; }; diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp index ac8ba788cb9bd..2825ead2a9fee 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/util.hpp @@ -14,8 +14,8 @@ #ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ #define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__UTIL_HPP_ -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include @@ -40,18 +40,20 @@ struct SoftConstraintsInputs lanelet::ConstLanelets closest_lanelets_to_goal; behavior_path_planner::PlanResult reference_path; behavior_path_planner::PlanResult prev_module_path; - std::optional prev_path; + std::optional prev_path; lanelet::ConstLanelets current_lanes; }; using SoftConstraintsFunctionVector = std::vector>; + autoware::sampler_common::Path &, const autoware::sampler_common::Constraints &, + const SoftConstraintsInputs &)>>; using HardConstraintsFunctionVector = std::vector>; + autoware::sampler_common::Path &, const autoware::sampler_common::Constraints &, + const MultiPoint2d &)>>; inline std::vector evaluateSoftConstraints( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, const autoware::sampler_common::Constraints & constraints, const SoftConstraintsFunctionVector & soft_constraints_functions, const SoftConstraintsInputs & input_data) { @@ -71,7 +73,7 @@ inline std::vector evaluateSoftConstraints( } inline std::vector evaluateHardConstraints( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint, const HardConstraintsFunctionVector & hard_constraints) { std::vector constraints_results; @@ -86,11 +88,11 @@ inline std::vector evaluateHardConstraints( return constraints_results; } -inline sampler_common::State getInitialState( +inline autoware::sampler_common::State getInitialState( const geometry_msgs::msg::Pose & pose, - const sampler_common::transform::Spline2D & reference_spline) + const autoware::sampler_common::transform::Spline2D & reference_spline) { - sampler_common::State initial_state; + autoware::sampler_common::State initial_state; Point2d initial_state_pose{pose.position.x, pose.position.y}; const auto rpy = tier4_autoware_utils::getRPY(pose.orientation); initial_state.pose = initial_state_pose; diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index e6fb0d4d0f573..c48f3240ab84d 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -14,17 +14,18 @@ autoware_cmake autoware_behavior_path_planner_common + autoware_bezier_sampler + autoware_frenet_planner + autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils autoware_vehicle_msgs - bezier_sampler - frenet_planner geometry_msgs interpolation lanelet2_extension motion_utils - path_sampler pluginlib rclcpp rclcpp_components @@ -37,7 +38,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp index 4b0ecdcd24e14..d2cc57d46de30 100644 --- a/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp +++ b/planning/behavior_path_sampling_planner_module/src/sampling_planner_module.cpp @@ -35,23 +35,24 @@ SamplingPlannerModule::SamplingPlannerModule( std::unordered_map> & objects_of_interest_marker_interface_ptr_map) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT - vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} + vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()} { - internal_params_ = - std::shared_ptr(new SamplingPlannerInternalParameters{}); + internal_params_ = std::make_shared(); updateModuleParams(parameters); // check if the path is empty hard_constraints_.emplace_back( []( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const MultiPoint2d & footprint) -> bool { return !path.points.empty() && !path.poses.empty(); }); hard_constraints_.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint) -> bool { if (!footprint.empty()) { path.constraint_results.inside_drivable_area = @@ -71,7 +72,8 @@ SamplingPlannerModule::SamplingPlannerModule( hard_constraints_.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const MultiPoint2d & footprint) -> bool { if (path.curvatures.empty()) { path.constraint_results.valid_curvature = false; @@ -96,8 +98,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Yaw difference soft constraint cost -> Considering implementation // soft_constraints_.emplace_back( // [&]( - // sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & - // constraints, + // autoware::sampler_common::Path & path, [[maybe_unused]] const + // autoware::sampler_common::Constraints & constraints, // [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { // if (path.points.empty()) return 0.0; // const auto & goal_pose_yaw = @@ -109,7 +111,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Remaining path length soft_constraints_.emplace_back( [&]( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.points.empty()) return 0.0; if (path.poses.empty()) return 0.0; @@ -142,8 +145,8 @@ SamplingPlannerModule::SamplingPlannerModule( // Distance to centerline soft_constraints_.emplace_back( [&]( - [[maybe_unused]] sampler_common::Path & path, - [[maybe_unused]] const sampler_common::Constraints & constraints, + [[maybe_unused]] autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.poses.empty()) return 0.0; const auto & last_pose = path.poses.back(); @@ -159,7 +162,8 @@ SamplingPlannerModule::SamplingPlannerModule( // // Curvature cost soft_constraints_.emplace_back( []( - sampler_common::Path & path, [[maybe_unused]] const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + [[maybe_unused]] const autoware::sampler_common::Constraints & constraints, [[maybe_unused]] const SoftConstraintsInputs & input_data) -> double { if (path.curvatures.empty()) return std::numeric_limits::max(); @@ -244,7 +248,7 @@ bool SamplingPlannerModule::isReferencePathSafe() const std::vector hard_constraints_results; auto transform_to_sampling_path = [](const PlanResult plan) { - sampler_common::Path path; + autoware::sampler_common::Path path; for (size_t i = 0; i < plan->points.size(); ++i) { const auto x = plan->points[i].point.pose.position.x; const auto y = plan->points[i].point.pose.position.y; @@ -258,17 +262,20 @@ bool SamplingPlannerModule::isReferencePathSafe() const } return path; }; - sampler_common::Path reference_path = transform_to_sampling_path(prev_module_reference_path); - const auto footprint = sampler_common::constraints::buildFootprintPoints( + autoware::sampler_common::Path reference_path = + transform_to_sampling_path(prev_module_reference_path); + const auto footprint = autoware::sampler_common::constraints::buildFootprintPoints( reference_path, internal_params_->constraints); behavior_path_planner::HardConstraintsFunctionVector hard_constraints_reference_path; hard_constraints_reference_path.emplace_back( []( - sampler_common::Path & path, const sampler_common::Constraints & constraints, + autoware::sampler_common::Path & path, + const autoware::sampler_common::Constraints & constraints, const MultiPoint2d & footprint) -> bool { path.constraint_results.collision_free = - !sampler_common::constraints::has_collision(footprint, constraints.obstacle_polygons); + !autoware::sampler_common::constraints::has_collision( + footprint, constraints.obstacle_polygons); return path.constraint_results.collision_free; }); evaluateHardConstraints( @@ -295,7 +302,7 @@ SamplingPlannerData SamplingPlannerModule::createPlannerData( } PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( - const frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, + const autoware::frenet_planner::Path frenet_path, const lanelet::ConstLanelets & lanelets, const double path_z) { auto quaternion_from_rpy = [](double roll, double pitch, double yaw) -> tf2::Quaternion { @@ -348,12 +355,12 @@ PathWithLaneId SamplingPlannerModule::convertFrenetPathToPathWithLaneID( } void SamplingPlannerModule::prepareConstraints( - sampler_common::Constraints & constraints, + autoware::sampler_common::Constraints & constraints, const PredictedObjects::ConstSharedPtr & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) const { - constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); constraints.rtree.clear(); size_t i = 0; for (const auto & o : predicted_objects->objects) { @@ -370,7 +377,7 @@ void SamplingPlannerModule::prepareConstraints( // TODO(Maxime): directly use lines instead of polygon - sampler_common::Polygon2d drivable_area_polygon; + autoware::sampler_common::Polygon2d drivable_area_polygon; for (const auto & p : right_bound) { drivable_area_polygon.outer().emplace_back(p.x, p.y); } @@ -392,7 +399,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() if (reference_path_ptr->points.empty()) { return {}; } - auto reference_spline = [&]() -> sampler_common::transform::Spline2D { + auto reference_spline = [&]() -> autoware::sampler_common::transform::Spline2D { std::vector x; std::vector y; x.reserve(reference_path_ptr->points.size()); @@ -404,19 +411,19 @@ BehaviorModuleOutput SamplingPlannerModule::plan() return {x, y}; }(); - frenet_planner::FrenetState frenet_initial_state; - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::FrenetState frenet_initial_state; + autoware::frenet_planner::SamplingParameters sampling_parameters; const auto & pose = planner_data_->self_odometry->pose.pose; - sampler_common::State initial_state = + autoware::sampler_common::State initial_state = behavior_path_planner::getInitialState(pose, reference_spline); sampling_parameters = prepareSamplingParameters(initial_state, reference_spline, *internal_params_); auto set_frenet_state = []( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & reference_spline, - frenet_planner::FrenetState & frenet_initial_state) + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & reference_spline, + autoware::frenet_planner::FrenetState & frenet_initial_state) { frenet_initial_state.position = initial_state.frenet; @@ -496,14 +503,16 @@ BehaviorModuleOutput SamplingPlannerModule::plan() const int path_divisions = internal_params_->sampling.previous_path_reuse_points_nb; const bool is_extend_previous_path = path_divisions > 0; - std::vector frenet_paths; + std::vector frenet_paths; // Extend prev path if (prev_path_is_valid && is_extend_previous_path) { - frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); + autoware::frenet_planner::Path prev_path_frenet = prev_sampling_path_.value(); frenet_paths.push_back(prev_path_frenet); - auto get_subset = [](const frenet_planner::Path & path, size_t offset) -> frenet_planner::Path { - frenet_planner::Path s; + auto get_subset = []( + const autoware::frenet_planner::Path & path, + size_t offset) -> autoware::frenet_planner::Path { + autoware::frenet_planner::Path s; s.points = {path.points.begin(), path.points.begin() + offset}; s.curvatures = {path.curvatures.begin(), path.curvatures.begin() + offset}; s.yaws = {path.yaws.begin(), path.yaws.begin() + offset}; @@ -512,7 +521,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() return s; }; - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {ego_pose.position.x, ego_pose.position.y}; const auto closest_iter = std::min_element( @@ -537,22 +546,22 @@ BehaviorModuleOutput SamplingPlannerModule::plan() const auto reused_path = get_subset(prev_path_frenet, reuse_idx); geometry_msgs::msg::Pose future_pose = reused_path.poses.back(); - sampler_common::State future_state = + autoware::sampler_common::State future_state = behavior_path_planner::getInitialState(future_pose, reference_spline); - frenet_planner::FrenetState frenet_reuse_state; + autoware::frenet_planner::FrenetState frenet_reuse_state; set_frenet_state(future_state, reference_spline, frenet_reuse_state); - frenet_planner::SamplingParameters extension_sampling_parameters = + autoware::frenet_planner::SamplingParameters extension_sampling_parameters = prepareSamplingParameters(future_state, reference_spline, *internal_params_); - auto extension_frenet_paths = frenet_planner::generatePaths( + auto extension_frenet_paths = autoware::frenet_planner::generatePaths( reference_spline, frenet_reuse_state, extension_sampling_parameters); for (auto & p : extension_frenet_paths) { if (!p.points.empty()) frenet_paths.push_back(reused_path.extend(p)); } } } else { - frenet_paths = - frenet_planner::generatePaths(reference_spline, frenet_initial_state, sampling_parameters); + frenet_paths = autoware::frenet_planner::generatePaths( + reference_spline, frenet_initial_state, sampling_parameters); } const auto path_for_calculating_bounds = getPreviousModuleOutput().reference_path; @@ -587,8 +596,8 @@ BehaviorModuleOutput SamplingPlannerModule::plan() std::vector> hard_constraints_results_full; std::vector> soft_constraints_results_full; for (auto & path : frenet_paths) { - const auto footprint = - sampler_common::constraints::buildFootprintPoints(path, internal_params_->constraints); + const auto footprint = autoware::sampler_common::constraints::buildFootprintPoints( + path, internal_params_->constraints); std::vector hard_constraints_results = evaluateHardConstraints(path, internal_params_->constraints, footprint, hard_constraints_); path.constraint_results.valid_curvature = true; @@ -598,7 +607,7 @@ BehaviorModuleOutput SamplingPlannerModule::plan() soft_constraints_results_full.push_back(soft_constraints_results); } - std::vector candidate_paths; + std::vector candidate_paths; const auto move_to_paths = [&candidate_paths](auto & paths_to_move) { candidate_paths.insert( candidate_paths.end(), std::make_move_iterator(paths_to_move.begin()), @@ -917,9 +926,9 @@ DrivableLanes SamplingPlannerModule::generateExpandDrivableLanes( return current_drivable_lanes; } -frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, +autoware::frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const SamplingPlannerInternalParameters & params_) { // calculate target lateral positions @@ -953,10 +962,10 @@ frenet_planner::SamplingParameters SamplingPlannerModule::prepareSamplingParamet } else { target_lateral_positions = params_.sampling.target_lateral_positions; } - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::SamplingParameters sampling_parameters; sampling_parameters.resolution = params_.sampling.resolution; const auto max_s = path_spline.lastS(); - frenet_planner::SamplingParameter p; + autoware::frenet_planner::SamplingParameter p; for (const auto target_length : params_.sampling.target_lengths) { p.target_state.position.s = std::min(max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length)); diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index ebf760405a256..67947398afb01 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -16,9 +16,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs lanelet2_extension motion_utils diff --git a/planning/behavior_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_blind_spot_module/plugins.xml index 7dda59ea2fdbe..fb5019de5df08 100644 --- a/planning/behavior_velocity_blind_spot_module/plugins.xml +++ b/planning/behavior_velocity_blind_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index a30836ab9f6a9..c357df481bca8 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include @@ -25,7 +25,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -107,4 +107,4 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp index 6e4135c50b253..6b963d86c9bbf 100644 --- a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp @@ -16,7 +16,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /* @@ -150,4 +150,4 @@ void BlindSpotModule::reactRTCApprovalByDecision( return; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index f1b0a56018d9b..7f2f7ebd89baa 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include -#include +#include +#include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) @@ -95,8 +95,9 @@ BlindSpotModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::BlindSpotModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::BlindSpotModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp index 7d6f936cc9d5b..12dccd846affc 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class BlindSpotModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 14ebe70216d5a..eed767fb891f9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -14,9 +14,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include #include @@ -40,7 +40,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -704,4 +704,4 @@ bool BlindSpotModule::isTargetObjectType( return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index b3bf90f5b91d1..cfb6bfdfeeded 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -15,9 +15,9 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -34,7 +34,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** * @brief wrapper class of interpolated path with lane id @@ -237,6 +237,6 @@ class BlindSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 690eca196d536..ca07636c2f60e 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -33,12 +33,12 @@ #define EIGEN_MPL2_ONLY #include #include -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathPointWithLaneId; @@ -108,6 +108,6 @@ std::vector getLinestringIntersects( std::optional getStopLineFromMap( const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 2fd5e2bb514c3..87ccdebd02fcf 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,9 +21,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common + autoware_vehicle_info_utils eigen geometry_msgs grid_map_core @@ -42,7 +43,6 @@ tier4_api_msgs tier4_autoware_utils tier4_debug_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml index 3d1d720857c17..e38ab0d85a63a 100644 --- a/planning/behavior_velocity_crosswalk_module/plugins.xml +++ b/planning/behavior_velocity_crosswalk_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index 4bee98db2fa56..1418d46081fcd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -14,14 +14,14 @@ #include "scene_crosswalk.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; @@ -207,4 +207,4 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index fe4282893674c..428ec2510c7f4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::Crosswalk; @@ -229,8 +229,9 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::CrosswalkModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::CrosswalkModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index ba7e75193deab..520dab49b347f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_crosswalk.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -56,6 +56,6 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC class CrosswalkModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index b6adb32f642eb..619e389f3fcf8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -22,11 +22,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::Index idx_offset; for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { @@ -140,7 +140,7 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, const std::vector & dynamic_objects, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); @@ -170,4 +170,4 @@ double calculate_detection_range( const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity); return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 4aab9d3bfc888..c4f2c1a23c57e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index /// @param [in] grid_map input grid map @@ -40,7 +40,7 @@ namespace behavior_velocity_planner /// @return true if the index is occluded bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief interpolate a point beyond the end of the given segment /// @param [in] segment input segment @@ -62,7 +62,7 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, const std::vector & dynamic_objects, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief calculate the distance away from the crosswalk that should be checked for occlusions /// @param occluded_objects_velocity assumed velocity of the objects coming out of occlusions @@ -89,6 +89,6 @@ std::vector select_and_inflate_o void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, const std::vector & objects); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 191eea8feabed..0db87508975a0 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -16,8 +16,8 @@ #include "occluded_crosswalk.hpp" -#include -#include +#include +#include #include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcArcLength; @@ -1207,7 +1207,7 @@ geometry_msgs::msg::Polygon CrosswalkModule::createObjectPolygon( } geometry_msgs::msg::Polygon CrosswalkModule::createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const auto & i = vehicle_info; const auto & front_m = i.max_longitudinal_offset_m; @@ -1281,4 +1281,4 @@ void CrosswalkModule::planStop( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 107a904dd076b..75f75846d4f4b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -17,7 +17,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using autoware_perception_msgs::msg::ObjectClassification; @@ -419,7 +419,7 @@ class CrosswalkModule : public SceneModuleInterface const double width_m, const double length_m); static geometry_msgs::msg::Polygon createVehiclePolygon( - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); void recordTime(const int step_num) { @@ -459,6 +459,6 @@ class CrosswalkModule : public SceneModuleInterface std::optional current_initial_occlusion_time_; std::optional most_recent_occlusion_time_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index d276ae95e06b6..883b40b89730f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -14,7 +14,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -45,7 +45,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcSignedArcLength; @@ -62,16 +62,19 @@ std::vector> getCrosswalksOnPath( // Add current lane id const auto nearest_lane_id = - behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); + autoware::behavior_velocity_planner::planning_utils::getNearestLaneId( + path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } for (const auto lane_id : unique_lane_ids) { @@ -229,4 +232,4 @@ std::optional getStopLineFromMap( return stop_line.front(); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_detection_area_module/package.xml index 6bcca43edc0cb..b2d370380829a 100644 --- a/planning/behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_detection_area_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_detection_area_module/plugins.xml b/planning/behavior_velocity_detection_area_module/plugins.xml index 73497c8bfdf2a..9356c863ec26e 100644 --- a/planning/behavior_velocity_detection_area_module/plugins.xml +++ b/planning/behavior_velocity_detection_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp index ee2af54e5ea2a..1ce982cd8df52 100644 --- a/planning/behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include #include @@ -29,7 +29,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::appendMarkerArray; @@ -191,4 +191,4 @@ motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index feb5bf6bb50ef..336a1710ed593 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::DetectionArea; using tier4_autoware_utils::getOrDeclareParameter; @@ -84,8 +84,9 @@ DetectionAreaModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::DetectionAreaModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::DetectionAreaModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp index 71cfa0a5eef96..64a76171814b9 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class DetectionAreaModulePlugin : public PluginWrapper -#include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcLongitudinalOffsetPose; @@ -339,4 +339,4 @@ bool DetectionAreaModule::hasEnoughBrakingDistance( return arc_lane_utils::calcSignedDistance(self_pose, line_pose.position) > pass_judge_line_distance; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_detection_area_module/src/scene.hpp index f135c3b2660cb..f577cd92dea6e 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.hpp @@ -23,15 +23,15 @@ #define EIGEN_MPL2_ONLY #include -#include -#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d @@ -101,6 +101,6 @@ class DetectionAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml index 84d01d04a09a4..f25ded5e1823c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -12,9 +12,10 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common + autoware_vehicle_info_utils geometry_msgs libboost-dev motion_utils @@ -24,7 +25,6 @@ tf2 tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml index 2f5662c7998ac..ba2d2ba61ad50 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp index 8497369917232..10843fd3aef2b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { std::optional find_closest_collision_point( @@ -80,4 +80,4 @@ std::vector find_collisions( return collisions; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp index 27a48afa032b1..8431221a27d6b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -20,7 +20,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief find the collision point closest to ego along an object footprint /// @param [in] ego_data ego data including its path and footprints used for finding a collision @@ -41,6 +41,6 @@ std::vector find_collisions( const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // COLLISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp index 367a1c2be1d5a..5a457d651cd9e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug { std::vector make_delete_markers( @@ -110,4 +110,4 @@ std::vector make_polygon_markers( } return markers; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp index 7397f63ca079c..c567948927089 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug { std::vector make_delete_markers( const size_t from, const size_t to, const std::string & ns); @@ -40,6 +40,6 @@ std::vector make_collision_markers( const rclcpp::Time & now); std::vector make_polygon_markers( const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug #endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index a58e4e9b88c75..5f005348c4cea 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { tier4_autoware_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, @@ -81,4 +81,4 @@ void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) } } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index 0050a4e2c9259..c75d7bb9cc49e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief create the footprint of the given obstacles and their projection over a fixed time /// horizon @@ -50,6 +50,6 @@ tier4_autoware_utils::Polygon2d project_to_pose( /// @param [inout] ego_data ego data with its path and the rtree to populate /// @param [in] params parameters void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index 63f1f3025f94a..f594432c9e452 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -20,7 +20,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -43,7 +43,7 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node pp.ignore_unavoidable_collisions = getOrDeclareParameter(node, ns + ".ignore_unavoidable_collisions"); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.ego_lateral_offset = std::max(std::abs(vehicle_info.min_lateral_offset_m), vehicle_info.max_lateral_offset_m); pp.ego_longitudinal_offset = vehicle_info.max_longitudinal_offset_m; @@ -68,9 +68,9 @@ DynamicObstacleStopModuleManager::getModuleExpiredFunction( return false; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::DynamicObstacleStopModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::DynamicObstacleStopModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp index eb7bf4c6dbc98..d5c403e26792c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_dynamic_obstacle_stop.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface { @@ -53,6 +53,6 @@ class DynamicObstacleStopModulePlugin : public PluginWrapper #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief filter the given predicted objects @@ -91,4 +91,4 @@ std::vector filter_predicted_obj } return filtered_objects; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index 5daa0cda44203..71da351d2f10b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief filter the given predicted objects @@ -34,6 +34,6 @@ std::vector filter_predicted_obj const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // OBJECT_FILTERING_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp index 1df8a1ed6a4a3..0f6fdbfadbf86 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp @@ -18,7 +18,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { void update_object_map( ObjectStopDecisionMap & object_map, const std::vector & collisions, @@ -66,4 +66,4 @@ std::optional find_earliest_collision( return earliest_collision; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp index 5756c8589fb73..cc13614e80782 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief representation of a decision to stop for a dynamic object struct ObjectStopDecision @@ -71,6 +71,6 @@ void update_object_map( std::optional find_earliest_collision( const ObjectStopDecisionMap & object_map, const EgoData & ego_data); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // OBJECT_STOP_DECISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp index 11ec540e3980f..9c5eced187aa8 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -21,8 +21,8 @@ #include "object_stop_decision.hpp" #include "types.hpp" -#include -#include +#include +#include #include #include #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { using visualization_msgs::msg::Marker; @@ -163,4 +163,4 @@ motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp index 79911c40a195d..8018412ba2b4b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -18,7 +18,7 @@ #include "object_stop_decision.hpp" #include "types.hpp" -#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { class DynamicObstacleStopModule : public SceneModuleInterface { @@ -57,6 +57,6 @@ class DynamicObstacleStopModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp index 532d770608dd3..32ef89413b2f8 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { using BoxIndexPair = std::pair; using Rtree = boost::geometry::index::rtree>; @@ -84,6 +84,6 @@ struct Collision geometry_msgs::msg::Point point; std::string object_uuid; }; -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_intersection_module/plugins.xml b/planning/behavior_velocity_intersection_module/plugins.xml deleted file mode 100644 index 206f0324231ec..0000000000000 --- a/planning/behavior_velocity_intersection_module/plugins.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_no_drivable_lane_module/package.xml index 79eea68142cc8..4c3d6dbf78153 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/package.xml @@ -19,8 +19,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml index 33b0d5d6e7469..e3e9efb254fba 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp index 0c6bb747a854b..0908fe712dbf5 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -14,14 +14,14 @@ #include "scene.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -102,4 +102,4 @@ visualization_msgs::msg::MarkerArray NoDrivableLaneModule::createDebugMarkerArra return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 0b7a8a8c28dd5..8c3fc5ef0f3ce 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -14,12 +14,12 @@ #include "manager.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -66,8 +66,9 @@ NoDrivableLaneModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoDrivableLaneModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp index 90455bd4b1c62..4afa498a5679b 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class NoDrivableLaneModuleManager : public SceneModuleManagerInterface { @@ -49,6 +49,6 @@ class NoDrivableLaneModulePlugin : public PluginWrapper +#include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::createPoint; @@ -283,4 +283,4 @@ void NoDrivableLaneModule::initialize_debug_data( } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp index fb90e023fedcd..6910e972a8685 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -17,14 +17,14 @@ #include "util.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -85,6 +85,6 @@ class NoDrivableLaneModule : public SceneModuleInterface void initialize_debug_data( const lanelet::Lanelet & no_drivable_lane, const geometry_msgs::msg::Point & ego_pos); }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp index 4c5efad94dfc8..c9a6ce1ac1510 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -17,14 +17,14 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -110,4 +110,4 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP } return path_no_drivable_lane_polygon_intersection; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp index 11953fd5dd177..289b04a2ce96d 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -51,6 +51,6 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, const geometry_msgs::msg::Point & ego_pos, const size_t max_num); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index 0991b37120a6f..97b3b01ee31e1 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,9 +17,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common + autoware_vehicle_info_utils eigen geometry_msgs interpolation @@ -34,7 +35,6 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_lint_auto diff --git a/planning/behavior_velocity_no_stopping_area_module/plugins.xml b/planning/behavior_velocity_no_stopping_area_module/plugins.xml index a9b07297cfa30..23d2a800c3cf5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/plugins.xml +++ b/planning/behavior_velocity_no_stopping_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index 9e51afca2d475..e90273d4ac5e4 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene_no_stopping_area.hpp" -#include -#include +#include +#include #include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -175,4 +175,4 @@ motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index faa265afe1559..31682fe0419d2 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::getOrDeclareParameter; @@ -39,7 +39,7 @@ NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) { const std::string ns(getModuleName()); auto & pp = planner_param_; - const auto & vi = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto & vi = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.state_clear_time = getOrDeclareParameter(node, ns + ".state_clear_time"); pp.stuck_vehicle_vel_thr = getOrDeclareParameter(node, ns + ".stuck_vehicle_vel_thr"); pp.stop_margin = getOrDeclareParameter(node, ns + ".stop_margin"); @@ -86,8 +86,9 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoStoppingAreaModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index baf5901ccc124..696115d4bda6d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_no_stopping_area.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class NoStoppingAreaModulePlugin : public PluginWrapper -#include -#include +#include +#include +#include #include #include #include @@ -36,7 +36,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -416,4 +416,4 @@ void NoStoppingAreaModule::insertStopPoint( // Insert stop point or replace with zero velocity planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 62ec0b88b328e..e7e66775f368e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -18,9 +18,9 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include +#include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d @@ -172,6 +172,6 @@ class NoStoppingAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_NO_STOPPING_AREA_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml index a38900d1a893b..53a2c460c2e44 100644 --- a/planning/behavior_velocity_occlusion_spot_module/plugins.xml +++ b/planning/behavior_velocity_occlusion_spot_module/plugins.xml @@ -1,3 +1,3 @@ - - + + diff --git a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt b/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt deleted file mode 100644 index e8191f9b632d5..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/CMakeLists.txt +++ /dev/null @@ -1,12 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_out_of_lane_module) - -find_package(autoware_cmake REQUIRED) -autoware_package() -pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) - -ament_auto_add_library(${PROJECT_NAME} SHARED - DIRECTORY src -) - -ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_out_of_lane_module/README.md b/planning/behavior_velocity_out_of_lane_module/README.md deleted file mode 100644 index 99396eb0edf22..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/README.md +++ /dev/null @@ -1,165 +0,0 @@ -## Out of Lane - -### Role - -`out_of_lane` is the module that decelerates and stops to prevent the ego vehicle from entering another lane with incoming dynamic objects. - -### Activation Timing - -This module is activated if `launch_out_of_lane` is set to true. - -### Inner-workings / Algorithms - -The algorithm is made of the following steps. - -1. Calculate the ego path footprints (red). -2. Calculate the other lanes (magenta). -3. Calculate the overlapping ranges between the ego path footprints and the other lanes (green). -4. For each overlapping range, decide if a stop or slow down action must be taken. -5. For each action, insert the corresponding stop or slow down point in the path. - -![overview](./docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png) - -#### 1. Ego Path Footprints - -In this first step, the ego footprint is projected at each path point and are eventually inflated based on the `extra_..._offset` parameters. - -#### 2. Other lanes - -In the second step, the set of lanes to consider for overlaps is generated. -This set is built by selecting all lanelets within some distance from the ego vehicle, and then removing non-relevant lanelets. -The selection distance is chosen as the maximum between the `slowdown.distance_threshold` and the `stop.distance_threshold`. - -A lanelet is deemed non-relevant if it meets one of the following conditions. - -- It is part of the lanelets followed by the ego path. -- It contains the rear point of the ego footprint. -- It follows one of the ego path lanelets. - -#### 3. Overlapping ranges - -In the third step, overlaps between the ego path footprints and the other lanes are calculated. -For each pair of other lane $l$ and ego path footprint $f$, we calculate the overlapping polygons using `boost::geometry::intersection`. -For each overlapping polygon found, if the distance inside the other lane $l$ is above the `overlap.minimum_distance` threshold, then the overlap is ignored. -Otherwise, the arc length range (relative to the ego path) and corresponding points of the overlapping polygons are stored. -Ultimately, for each other lane $l$, overlapping ranges of successive overlaps are built with the following information: - -- overlapped other lane $l$. -- start and end ego path indexes. -- start and end ego path arc lengths. -- start and end overlap points. - -#### 4. Decisions - -In the fourth step, a decision to either slow down or stop before each overlapping range is taken based on the dynamic objects. -The conditions for the decision depend on the value of the `mode` parameter. - -Whether it is decided to slow down or stop is determined by the distance between the ego vehicle and the start of the overlapping range (in arc length along the ego path). -If this distance is bellow the `actions.slowdown.threshold`, a velocity of `actions.slowdown.velocity` will be used. -If the distance is bellow the `actions.stop.threshold`, a velocity of `0`m/s will be used. - - - - - - -
- - - -
- -##### Threshold - -With the `mode` set to `"threshold"`, -a decision to stop or slow down before a range is made if -an incoming dynamic object is estimated to reach the overlap within `threshold.time_threshold`. - -##### TTC (time to collision) - -With the `mode` set to `"ttc"`, -estimates for the times when ego and the dynamic objects reach the start and end of the overlapping range are calculated. -This is then used to calculate the time to collision over the period where ego crosses the overlap. -If the time to collision is predicted to go bellow the `ttc.threshold`, the decision to stop or slow down is made. - -##### Intervals - -With the `mode` set to `"intervals"`, -the estimated times when ego and the dynamic objects reach the start and end points of -the overlapping range are used to create time intervals. -These intervals can be made shorter or longer using the -`intervals.ego_time_buffer` and `intervals.objects_time_buffer` parameters. -If the time interval of ego overlaps with the time interval of an object, the decision to stop or slow down is made. - -##### Time estimates - -###### Ego - -To estimate the times when ego will reach an overlap, it is assumed that ego travels along its path -at its current velocity or at half the velocity of the path points, whichever is higher. - -###### Dynamic objects - -Two methods are used to estimate the time when a dynamic objects with reach some point. -If `objects.use_predicted_paths` is set to `true`, the predicted paths of the dynamic object are used if their confidence value is higher than the value set by the `objects.predicted_path_min_confidence` parameter. -Otherwise, the lanelet map is used to estimate the distance between the object and the point and the time is calculated assuming the object keeps its current velocity. - -#### 5. Path update - -Finally, for each decision to stop or slow down before an overlapping range, -a point is inserted in the path. -For a decision taken for an overlapping range with a lane $l$ starting at ego path point index $i$, -a point is inserted in the path between index $i$ and $i-1$ such that the ego footprint projected at the inserted point does not overlap $l$. -Such point with no overlap must exist since, by definition of the overlapping range, -we know that there is no overlap at $i-1$. - -If the point would cause a higher deceleration than allowed by the `max_accel` parameter (node parameter), -it is skipped. - -Moreover, parameter `action.distance_buffer` adds an extra distance between the ego footprint and the overlap when possible. - -### Module Parameters - -| Parameter | Type | Description | -| ----------------------------- | ------ | --------------------------------------------------------------------------------- | -| `mode` | string | [-] mode used to consider a dynamic object. Candidates: threshold, intervals, ttc | -| `skip_if_already_overlapping` | bool | [-] if true, do not run this module when ego already overlaps another lane | - -| Parameter /threshold | Type | Description | -| -------------------- | ------ | ---------------------------------------------------------------- | -| `time_threshold` | double | [s] consider objects that will reach an overlap within this time | - -| Parameter /intervals | Type | Description | -| --------------------- | ------ | ------------------------------------------------------- | -| `ego_time_buffer` | double | [s] extend the ego time interval by this buffer | -| `objects_time_buffer` | double | [s] extend the time intervals of objects by this buffer | - -| Parameter /ttc | Type | Description | -| -------------- | ------ | ------------------------------------------------------------------------------------------------------ | -| `threshold` | double | [s] consider objects with an estimated time to collision bellow this value while ego is on the overlap | - -| Parameter /objects | Type | Description | -| ------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `minimum_velocity` | double | [m/s] ignore objects with a velocity lower than this value | -| `predicted_path_min_confidence` | double | [-] minimum confidence required for a predicted path to be considered | -| `use_predicted_paths` | bool | [-] if true, use the predicted paths to estimate future positions; if false, assume the object moves at constant velocity along _all_ lanelets it currently is located in | - -| Parameter /overlap | Type | Description | -| ------------------ | ------ | ---------------------------------------------------------------------------------------------------- | -| `minimum_distance` | double | [m] minimum distance inside a lanelet for an overlap to be considered | -| `extra_length` | double | [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) | - -| Parameter /action | Type | Description | -| ----------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `skip_if_over_max_decel` | bool | [-] if true, do not take an action that would cause more deceleration than the maximum allowed | -| `distance_buffer` | double | [m] buffer distance to try to keep between the ego footprint and lane | -| `slowdown.distance_threshold` | double | [m] insert a slow down when closer than this distance from an overlap | -| `slowdown.velocity` | double | [m] slow down velocity | -| `stop.distance_threshold` | double | [m] insert a stop when closer than this distance from an overlap | - -| Parameter /ego | Type | Description | -| -------------------- | ------ | ---------------------------------------------------- | -| `extra_front_offset` | double | [m] extra front distance to add to the ego footprint | -| `extra_rear_offset` | double | [m] extra rear distance to add to the ego footprint | -| `extra_left_offset` | double | [m] extra left distance to add to the ego footprint | -| `extra_right_offset` | double | [m] extra right distance to add to the ego footprint | diff --git a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml b/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml deleted file mode 100644 index e4d08367ee4f7..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/config/out_of_lane.param.yaml +++ /dev/null @@ -1,44 +0,0 @@ -/**: - ros__parameters: - out_of_lane: # module to stop or slowdown before overlapping another lane with other objects - mode: ttc # mode used to consider a conflict with an object. "threshold", "intervals", or "ttc" - skip_if_already_overlapping: true # do not run this module when ego already overlaps another lane - ignore_overlaps_over_lane_changeable_lanelets: true # if true, overlaps on lane changeable lanelets are ignored - - threshold: - time_threshold: 5.0 # [s] consider objects that will reach an overlap within this time - intervals: # consider objects if their estimated time interval spent on the overlap intersect the estimated time interval of ego - ego_time_buffer: 0.5 # [s] extend the ego time interval by this buffer - objects_time_buffer: 0.5 # [s] extend the time intervals of objects by this buffer - ttc: - threshold: 3.0 # [s] consider objects with an estimated time to collision bellow this value while on the overlap - - objects: - minimum_velocity: 0.5 # [m/s] objects lower than this velocity will be ignored - use_predicted_paths: true # if true, use the predicted paths to estimate future positions. - # if false, assume the object moves at constant velocity along *all* lanelets it currently is located in. - predicted_path_min_confidence : 0.1 # when using predicted paths, ignore the ones whose confidence is lower than this value. - distance_buffer: 1.0 # [m] distance buffer used to determine if a collision will occur in the other lane - cut_predicted_paths_beyond_red_lights: true # if true, predicted paths are cut beyond the stop line of red traffic lights - - overlap: - minimum_distance: 0.0 # [m] minimum distance inside a lanelet for an overlap to be considered - extra_length: 0.0 # [m] extra arc length to add to the front and back of an overlap (used to calculate enter/exit times) - - action: # action to insert in the path if an object causes a conflict at an overlap - skip_if_over_max_decel: true # if true, do not take an action that would cause more deceleration than the maximum allowed - precision: 0.1 # [m] precision when inserting a stop pose in the path - distance_buffer: 1.5 # [m] buffer distance to try to keep between the ego footprint and lane - min_duration: 1.0 # [s] minimum duration needed before a decision can be canceled - slowdown: - distance_threshold: 30.0 # [m] insert a slowdown when closer than this distance from an overlap - velocity: 2.0 # [m/s] slowdown velocity - stop: - distance_threshold: 15.0 # [m] insert a stop when closer than this distance from an overlap - - ego: - min_assumed_velocity: 2.0 # [m/s] minimum velocity used to calculate the enter and exit times of ego - extra_front_offset: 0.0 # [m] extra front distance - extra_rear_offset: 0.0 # [m] extra rear distance - extra_right_offset: 0.0 # [m] extra right distance - extra_left_offset: 0.0 # [m] extra left distance diff --git a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png b/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png deleted file mode 100644 index f095467b3b0ac..0000000000000 Binary files a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane-footprints_other_lanes_overlaps_ranges.png and /dev/null differ diff --git a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_slow.png b/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_slow.png deleted file mode 100644 index 2f358975b9491..0000000000000 Binary files a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_slow.png and /dev/null differ diff --git a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_stop.png b/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_stop.png deleted file mode 100644 index fdb75ac0c6eb8..0000000000000 Binary files a/planning/behavior_velocity_out_of_lane_module/docs/out_of_lane_stop.png and /dev/null differ diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml deleted file mode 100644 index d3742ea7d305f..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ /dev/null @@ -1,40 +0,0 @@ - - - - behavior_velocity_out_of_lane_module - 0.1.0 - The behavior_velocity_out_of_lane_module package - - Maxime Clement - Tomoya Kimura - Shumpei Wakabayashi - Takayuki Murooka - - Apache License 2.0 - - ament_cmake_auto - autoware_cmake - autoware_perception_msgs - autoware_planning_msgs - behavior_velocity_planner_common - geometry_msgs - lanelet2_extension - libboost-dev - motion_utils - pluginlib - rclcpp - route_handler - tf2 - tier4_autoware_utils - tier4_planning_msgs - traffic_light_utils - vehicle_info_util - visualization_msgs - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/planning/behavior_velocity_out_of_lane_module/plugins.xml b/planning/behavior_velocity_out_of_lane_module/plugins.xml deleted file mode 100644 index 8c18fdce79480..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp deleted file mode 100644 index 0c9e6448bb374..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ /dev/null @@ -1,106 +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 CALCULATE_SLOWDOWN_POINTS_HPP_ -#define CALCULATE_SLOWDOWN_POINTS_HPP_ - -#include "footprint.hpp" -#include "types.hpp" - -#include -#include - -#include - -#include -#include - -namespace behavior_velocity_planner::out_of_lane -{ - -/// @brief estimate whether ego can decelerate without breaking the deceleration limit -/// @details assume ego wants to reach the target point at the target velocity -/// @param [in] ego_data ego data -/// @param [in] point target point -/// @param [in] target_vel target_velocity -bool can_decelerate( - const EgoData & ego_data, const PathPointWithLaneId & point, const double target_vel) -{ - if (ego_data.velocity < 0.5) return true; - const auto dist_ahead_of_ego = motion_utils::calcSignedArcLength( - ego_data.path.points, ego_data.pose.position, point.point.pose.position); - const auto acc_to_target_vel = - (ego_data.velocity * ego_data.velocity - target_vel * target_vel) / (2 * dist_ahead_of_ego); - return acc_to_target_vel < std::abs(ego_data.max_decel); -} - -/// @brief calculate the last pose along the path where ego does not overlap the lane to avoid -/// @param [in] ego_data ego data -/// @param [in] decision the input decision (i.e., which lane to avoid and at what speed) -/// @param [in] footprint the ego footprint -/// @param [in] prev_slowdown_point previous slowdown point. If set, ignore deceleration limits -/// @param [in] params parameters -/// @return the last pose that is not out of lane (if found) -std::optional calculate_last_in_lane_pose( - const EgoData & ego_data, const Slowdown & decision, const Polygon2d & footprint, - const std::optional & prev_slowdown_point, const PlannerParam & params) -{ - const auto from_arc_length = - motion_utils::calcSignedArcLength(ego_data.path.points, 0, ego_data.first_path_idx); - const auto to_arc_length = - motion_utils::calcSignedArcLength(ego_data.path.points, 0, decision.target_path_idx); - PathPointWithLaneId interpolated_point; - for (auto l = to_arc_length - params.precision; l > from_arc_length; l -= params.precision) { - // TODO(Maxime): binary search - interpolated_point.point.pose = motion_utils::calcInterpolatedPose(ego_data.path.points, l); - const auto respect_decel_limit = - !params.skip_if_over_max_decel || prev_slowdown_point || - can_decelerate(ego_data, interpolated_point, decision.velocity); - const auto interpolated_footprint = project_to_pose(footprint, interpolated_point.point.pose); - const auto is_overlap_lane = boost::geometry::overlaps( - interpolated_footprint, decision.lane_to_avoid.polygon2d().basicPolygon()); - const auto is_overlap_extra_lane = - prev_slowdown_point && - boost::geometry::overlaps( - interpolated_footprint, - prev_slowdown_point->slowdown.lane_to_avoid.polygon2d().basicPolygon()); - if (respect_decel_limit && !is_overlap_lane && !is_overlap_extra_lane) - return interpolated_point; - } - return std::nullopt; -} - -/// @brief calculate the slowdown point to insert in the path -/// @param ego_data ego data (path, velocity, etc) -/// @param decisions decision (before which point to stop, what lane to avoid entering, etc) -/// @param prev_slowdown_point previously calculated slowdown point -/// @param params parameters -/// @return optional slowdown point to insert in the path -std::optional calculate_slowdown_point( - const EgoData & ego_data, const std::vector & decisions, - const std::optional prev_slowdown_point, PlannerParam params) -{ - params.extra_front_offset += params.dist_buffer; - const auto base_footprint = make_base_footprint(params); - - // search for the first slowdown decision for which a stop point can be inserted - for (const auto & decision : decisions) { - const auto last_in_lane_pose = - calculate_last_in_lane_pose(ego_data, decision, base_footprint, prev_slowdown_point, params); - if (last_in_lane_pose) return SlowdownToInsert{decision, *last_in_lane_pose}; - } - return std::nullopt; -} -} // namespace behavior_velocity_planner::out_of_lane -#endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp deleted file mode 100644 index 862ca351a118a..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ /dev/null @@ -1,187 +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 "debug.hpp" - -#include - -#include - -namespace behavior_velocity_planner::out_of_lane::debug -{ -namespace -{ -visualization_msgs::msg::Marker get_base_marker() -{ - visualization_msgs::msg::Marker base_marker; - base_marker.header.frame_id = "map"; - base_marker.header.stamp = rclcpp::Time(0); - base_marker.id = 0; - base_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; - base_marker.action = visualization_msgs::msg::Marker::ADD; - base_marker.pose.position = tier4_autoware_utils::createMarkerPosition(0.0, 0.0, 0); - base_marker.pose.orientation = tier4_autoware_utils::createMarkerOrientation(0, 0, 0, 1.0); - base_marker.scale = tier4_autoware_utils::createMarkerScale(0.1, 0.1, 0.1); - base_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); - return base_marker; -} -} // namespace -void add_footprint_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = "footprints"; - for (const auto & f : footprints) { - debug_marker.points.clear(); - for (const auto & p : f) - debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - debug_marker.points.clear(); - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_current_overlap_marker( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = "current_overlap"; - debug_marker.points.clear(); - for (const auto & p : current_footprint) - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), z)); - if (!debug_marker.points.empty()) debug_marker.points.push_back(debug_marker.points.front()); - if (current_overlapped_lanelets.empty()) - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.1, 1.0, 0.1, 0.5); - else - debug_marker.color = tier4_autoware_utils::createMarkerColor(1.0, 0.1, 0.1, 0.5); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - for (const auto & ll : current_overlapped_lanelets) { - debug_marker.points.clear(); - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.5)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_lanelet_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color, const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = ns; - debug_marker.color = color; - for (const auto & ll : lanelets) { - debug_marker.points.clear(); - - // add a small z offset to draw above the lanelet map - for (const auto & p : ll.polygon3d()) - debug_marker.points.push_back( - tier4_autoware_utils::createMarkerPosition(p.x(), p.y(), p.z() + 0.1)); - debug_marker.points.push_back(debug_marker.points.front()); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -void add_range_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, const double z, - const size_t prev_nb) -{ - auto debug_marker = get_base_marker(); - debug_marker.ns = "ranges"; - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.2, 0.9, 0.1, 0.5); - for (const auto & range : ranges) { - debug_marker.points.clear(); - debug_marker.points.push_back( - path.points[first_ego_idx + range.entering_path_idx].point.pose.position); - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - for (const auto & overlap : range.debug.overlaps) { - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - overlap.min_overlap_point.x(), overlap.min_overlap_point.y(), z)); - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - overlap.max_overlap_point.x(), overlap.max_overlap_point.y(), z)); - } - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - range.exiting_point.x(), range.exiting_point.y(), z)); - debug_marker.points.push_back( - path.points[first_ego_idx + range.exiting_path_idx].point.pose.position); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - debug_marker.action = debug_marker.DELETE; - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); - debug_marker.action = debug_marker.ADD; - debug_marker.id = 0; - debug_marker.ns = "decisions"; - debug_marker.color = tier4_autoware_utils::createMarkerColor(0.9, 0.1, 0.1, 1.0); - debug_marker.points.clear(); - for (const auto & range : ranges) { - debug_marker.type = debug_marker.LINE_STRIP; - if (range.debug.decision) { - debug_marker.points.push_back(tier4_autoware_utils::createMarkerPosition( - range.entering_point.x(), range.entering_point.y(), z)); - debug_marker.points.push_back( - range.debug.object->kinematics.initial_pose_with_covariance.pose.position); - } - debug_marker_array.markers.push_back(debug_marker); - debug_marker.points.clear(); - debug_marker.id++; - - debug_marker.type = debug_marker.TEXT_VIEW_FACING; - debug_marker.pose.position.x = range.entering_point.x(); - debug_marker.pose.position.y = range.entering_point.y(); - debug_marker.pose.position.z = z; - std::stringstream ss; - ss << "Ego: " << range.debug.times.ego.enter_time << " - " << range.debug.times.ego.exit_time - << "\n"; - if (range.debug.object) { - debug_marker.pose.position.x += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.x; - debug_marker.pose.position.y += - range.debug.object->kinematics.initial_pose_with_covariance.pose.position.y; - debug_marker.pose.position.x /= 2; - debug_marker.pose.position.y /= 2; - ss << "Obj: " << range.debug.times.object.enter_time << " - " - << range.debug.times.object.exit_time << "\n"; - } - debug_marker.scale.z = 1.0; - debug_marker.text = ss.str(); - debug_marker_array.markers.push_back(debug_marker); - debug_marker.id++; - } - for (; debug_marker.id < static_cast(prev_nb); ++debug_marker.id) - debug_marker_array.markers.push_back(debug_marker); -} - -} // namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp deleted file mode 100644 index 0802ae78d7a26..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ /dev/null @@ -1,69 +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 DEBUG_HPP_ -#define DEBUG_HPP_ - -#include "types.hpp" - -#include - -#include - -#include - -namespace behavior_velocity_planner::out_of_lane::debug -{ -/// @brief add footprint markers to the given marker array -/// @param [inout] debug_marker_array marker array -/// @param [in] footprints footprints to turn into markers -/// @param [in] z z value to use for the markers -/// @param [in] prev_nb previous number of markers (used to delete the extra ones) -void add_footprint_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygons2d & footprints, const double z, const size_t prev_nb); -/// @brief add footprint markers to the given marker array -/// @param [inout] debug_marker_array marker array -/// @param [in] current_footprint footprint to turn into a marker -/// @param [in] current_overlapped_lanelets lanelets to turn into markers -/// @param [in] z z value to use for the markers -/// @param [in] prev_nb previous number of markers (used to delete the extra ones) -void add_current_overlap_marker( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::BasicPolygon2d & current_footprint, - const lanelet::ConstLanelets & current_overlapped_lanelets, const double z, const size_t prev_nb); -/// @brief add footprint markers to the given marker array -/// @param [inout] debug_marker_array marker array -/// @param [in] lanelets lanelets to turn into markers -/// @param [in] ns namespace of the markers -/// @param [in] color namespace of the markers -/// @param [in] prev_nb previous number of markers (used to delete the extra ones) -void add_lanelet_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, - const lanelet::ConstLanelets & lanelets, const std::string & ns, - const std_msgs::msg::ColorRGBA & color, const size_t prev_nb); -/// @brief add ranges markers to the given marker array -/// @param [inout] debug_marker_array marker array -/// @param [in] ranges ranges to turn into markers -/// @param [in] path modified ego path that was used to calculate the ranges -/// @param [in] first_path_idx first idx of ego on the path -/// @param [in] z z value to use for the markers -/// @param [in] prev_nb previous number of markers (used to delete the extra ones) -void add_range_markers( - visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, - const double z, const size_t prev_nb); -} // namespace behavior_velocity_planner::out_of_lane::debug - -#endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp deleted file mode 100644 index 126c75c2f80b9..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ /dev/null @@ -1,383 +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 "decisions.hpp" - -#include -#include -#include -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -namespace behavior_velocity_planner::out_of_lane -{ -double distance_along_path(const EgoData & ego_data, const size_t target_idx) -{ - return motion_utils::calcSignedArcLength( - ego_data.path.points, ego_data.pose.position, ego_data.first_path_idx + target_idx); -} - -double time_along_path(const EgoData & ego_data, const size_t target_idx, const double min_velocity) -{ - const auto dist = distance_along_path(ego_data, target_idx); - const auto v = std::max( - std::max(ego_data.velocity, min_velocity), - ego_data.path.points[ego_data.first_path_idx + target_idx].point.longitudinal_velocity_mps * - 0.5); - return dist / v; -} - -bool object_is_incoming( - const lanelet::BasicPoint2d & object_position, - const std::shared_ptr route_handler, - const lanelet::ConstLanelet & lane) -{ - const auto lanelets = route_handler->getPrecedingLaneletSequence(lane, 50.0); - if (boost::geometry::within(object_position, lane.polygon2d().basicPolygon())) return true; - for (const auto & lls : lanelets) - for (const auto & ll : lls) - if (boost::geometry::within(object_position, ll.polygon2d().basicPolygon())) return true; - return false; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger) -{ - // skip the dynamic object if it is not in a lane preceding the overlapped lane - // lane changes are intentionally not considered - const auto object_point = lanelet::BasicPoint2d( - object.kinematics.initial_pose_with_covariance.pose.position.x, - object.kinematics.initial_pose_with_covariance.pose.position.y); - if (!object_is_incoming(object_point, route_handler, range.lane)) return {}; - - const auto max_deviation = object.shape.dimensions.y + range.inside_distance + dist_buffer; - const auto max_lon_deviation = object.shape.dimensions.x / 2.0; - auto worst_enter_time = std::optional(); - auto worst_exit_time = std::optional(); - - for (const auto & predicted_path : object.kinematics.predicted_paths) { - const auto unique_path_points = motion_utils::removeOverlapPoints(predicted_path.path); - if (unique_path_points.size() < 2) continue; - const auto time_step = rclcpp::Duration(predicted_path.time_step).seconds(); - const auto enter_point = - geometry_msgs::msg::Point().set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_segment_idx = - motion_utils::findNearestSegmentIndex(unique_path_points, enter_point); - const auto enter_offset = motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, enter_segment_idx, enter_point); - const auto enter_lat_dist = - std::abs(motion_utils::calcLateralOffset(unique_path_points, enter_point, enter_segment_idx)); - const auto enter_segment_length = tier4_autoware_utils::calcDistance2d( - unique_path_points[enter_segment_idx], unique_path_points[enter_segment_idx + 1]); - const auto enter_offset_ratio = enter_offset / enter_segment_length; - const auto enter_time = - static_cast(enter_segment_idx) * time_step + enter_offset_ratio * time_step; - - const auto exit_point = - geometry_msgs::msg::Point().set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_segment_idx = - motion_utils::findNearestSegmentIndex(unique_path_points, exit_point); - const auto exit_offset = motion_utils::calcLongitudinalOffsetToSegment( - unique_path_points, exit_segment_idx, exit_point); - const auto exit_lat_dist = - std::abs(motion_utils::calcLateralOffset(unique_path_points, exit_point, exit_segment_idx)); - const auto exit_segment_length = tier4_autoware_utils::calcDistance2d( - unique_path_points[exit_segment_idx], unique_path_points[exit_segment_idx + 1]); - const auto exit_offset_ratio = exit_offset / static_cast(exit_segment_length); - const auto exit_time = - static_cast(exit_segment_idx) * time_step + exit_offset_ratio * time_step; - - RCLCPP_DEBUG( - logger, "\t\t\tPredicted path (time step = %2.2fs): enter @ %2.2fs, exit @ %2.2fs", time_step, - enter_time, exit_time); - // predicted path is too far from the overlapping range to be relevant - const auto is_far_from_entering_point = enter_lat_dist > max_deviation; - const auto is_far_from_exiting_point = exit_lat_dist > max_deviation; - if (is_far_from_entering_point && is_far_from_exiting_point) { - RCLCPP_DEBUG( - logger, - " * far_from_enter (%d) = %2.2fm | far_from_exit (%d) = %2.2fm | max_dev = %2.2fm\n", - is_far_from_entering_point, enter_lat_dist, is_far_from_exiting_point, exit_lat_dist, - max_deviation); - continue; - } - const auto is_last_predicted_path_point = - (exit_segment_idx + 2 == unique_path_points.size() || - enter_segment_idx + 2 == unique_path_points.size()); - const auto does_not_reach_overlap = - is_last_predicted_path_point && (std::min(exit_offset, enter_offset) > max_lon_deviation); - if (does_not_reach_overlap) { - RCLCPP_DEBUG( - logger, " * does not reach the overlap = %2.2fm | max_dev = %2.2fm\n", - std::min(exit_offset, enter_offset), max_lon_deviation); - continue; - } - - const auto same_driving_direction_as_ego = enter_time < exit_time; - if (same_driving_direction_as_ego) { - worst_enter_time = worst_enter_time ? std::min(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::max(*worst_exit_time, exit_time) : exit_time; - } else { - worst_enter_time = worst_enter_time ? std::max(*worst_enter_time, enter_time) : enter_time; - worst_exit_time = worst_exit_time ? std::min(*worst_exit_time, exit_time) : exit_time; - } - } - if (worst_enter_time && worst_exit_time) { - RCLCPP_DEBUG( - logger, "\t\t\t * found enter/exit time [%2.2f, %2.2f]\n", *worst_enter_time, - *worst_exit_time); - return std::make_pair(*worst_enter_time, *worst_exit_time); - } - RCLCPP_DEBUG(logger, "\t\t\t * enter/exit time not found\n"); - return {}; -} - -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger) -{ - const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; - const auto object_point = lanelet::BasicPoint2d(p.x, p.y); - const auto half_size = object.shape.dimensions.x / 2.0; - lanelet::ConstLanelets object_lanelets; - for (const auto & ll : inputs.lanelets) - if (boost::geometry::within(object_point, ll.polygon2d().basicPolygon())) - object_lanelets.push_back(ll); - - geometry_msgs::msg::Pose pose; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto range_enter_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto range_exit_length = lanelet::utils::getArcCoordinates({range.lane}, pose).length; - const auto range_size = std::abs(range_enter_length - range_exit_length); - auto worst_enter_dist = std::optional(); - auto worst_exit_dist = std::optional(); - for (const auto & lane : object_lanelets) { - const auto path = inputs.route_handler->getRoutingGraphPtr()->shortestPath(lane, range.lane); - RCLCPP_DEBUG( - logger, "\t\t\tPath ? %d [from %ld to %ld]\n", path.has_value(), lane.id(), range.lane.id()); - if (path) { - lanelet::ConstLanelets lls; - for (const auto & ll : *path) lls.push_back(ll); - pose.position.set__x(object_point.x()).set__y(object_point.y()); - const auto object_curr_length = lanelet::utils::getArcCoordinates(lls, pose).length; - pose.position.set__x(range.entering_point.x()).set__y(range.entering_point.y()); - const auto enter_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - pose.position.set__x(range.exiting_point.x()).set__y(range.exiting_point.y()); - const auto exit_dist = - lanelet::utils::getArcCoordinates(lls, pose).length - object_curr_length; - RCLCPP_DEBUG( - logger, "\t\t\t%2.2f -> [%2.2f(%2.2f, %2.2f) - %2.2f(%2.2f, %2.2f)]\n", object_curr_length, - enter_dist, range.entering_point.x(), range.entering_point.y(), exit_dist, - range.exiting_point.x(), range.exiting_point.y()); - const auto already_entered_range = std::abs(enter_dist - exit_dist) > range_size * 2.0; - if (already_entered_range) continue; - // multiple paths to the overlap -> be conservative and use the "worst" case - // "worst" = min/max arc length depending on if the lane is running opposite to the ego path - const auto is_opposite = enter_dist > exit_dist; - if (!worst_enter_dist) - worst_enter_dist = enter_dist; - else if (is_opposite) - worst_enter_dist = std::max(*worst_enter_dist, enter_dist); - else - worst_enter_dist = std::min(*worst_enter_dist, enter_dist); - if (!worst_exit_dist) - worst_exit_dist = exit_dist; - else if (is_opposite) - worst_exit_dist = std::max(*worst_exit_dist, exit_dist); - else - worst_exit_dist = std::min(*worst_exit_dist, exit_dist); - } - } - if (worst_enter_dist && worst_exit_dist) { - const auto v = object.kinematics.initial_twist_with_covariance.twist.linear.x; - return std::make_pair((*worst_enter_dist - half_size) / v, (*worst_exit_dist + half_size) / v); - } - return {}; -} - -bool threshold_condition(const RangeTimes & range_times, const PlannerParam & params) -{ - const auto enter_within_threshold = - range_times.object.enter_time > 0.0 && range_times.object.enter_time < params.time_threshold; - const auto exit_within_threshold = - range_times.object.exit_time > 0.0 && range_times.object.exit_time < params.time_threshold; - return enter_within_threshold || exit_within_threshold; -} - -bool intervals_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto opposite_way_condition = [&]() -> bool { - const auto ego_exits_before_object_enters = - range_times.ego.exit_time + params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] (opposite way) ego exit %2.2fs < obj enter %2.2fs ? -> should not " - "enter = %d\n", - range_times.ego.exit_time + params.intervals_ego_buffer, - range_times.object.enter_time + params.intervals_obj_buffer, ego_exits_before_object_enters); - return ego_exits_before_object_enters; - }; - const auto same_way_condition = [&]() -> bool { - const auto object_enters_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.enter_time + params.intervals_obj_buffer && - range_times.object.enter_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - const auto object_exits_during_overlap = - range_times.ego.enter_time - params.intervals_ego_buffer < - range_times.object.exit_time + params.intervals_obj_buffer && - range_times.object.exit_time - params.intervals_obj_buffer - range_times.ego.exit_time < - range_times.ego.exit_time + params.intervals_ego_buffer; - RCLCPP_DEBUG( - logger, - "\t\t\t[Intervals] obj enters during overlap ? %d OR obj exits during overlap %d ? -> should " - "not " - "enter = %d\n", - object_enters_during_overlap, object_exits_during_overlap, - object_enters_during_overlap || object_exits_during_overlap); - return object_enters_during_overlap || object_exits_during_overlap; - }; - - const auto object_is_going_same_way = - range_times.object.enter_time < range_times.object.exit_time; - return (object_is_going_same_way && same_way_condition()) || - (!object_is_going_same_way && opposite_way_condition()); -} - -bool ttc_condition( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - const auto ttc_at_enter = range_times.ego.enter_time - range_times.object.enter_time; - const auto ttc_at_exit = range_times.ego.exit_time - range_times.object.exit_time; - const auto collision_during_overlap = (ttc_at_enter < 0.0) != (ttc_at_exit < 0.0); - const auto ttc_is_bellow_threshold = - std::min(std::abs(ttc_at_enter), std::abs(ttc_at_exit)) <= params.ttc_threshold; - RCLCPP_DEBUG( - logger, "\t\t\t[TTC] (%2.2fs - %2.2fs) -> %d\n", ttc_at_enter, ttc_at_exit, - (collision_during_overlap || ttc_is_bellow_threshold)); - return collision_during_overlap || ttc_is_bellow_threshold; -} - -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger) -{ - RCLCPP_DEBUG( - logger, " enter at %2.2fs, exits at %2.2fs\n", range_times.object.enter_time, - range_times.object.exit_time); - return (params.mode == "threshold" && threshold_condition(range_times, params)) || - (params.mode == "intervals" && intervals_condition(range_times, params, logger)) || - (params.mode == "ttc" && ttc_condition(range_times, params, logger)); -} - -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - RangeTimes range_times{}; - range_times.ego.enter_time = - time_along_path(inputs.ego_data, range.entering_path_idx, params.ego_min_velocity); - range_times.ego.exit_time = - time_along_path(inputs.ego_data, range.exiting_path_idx, params.ego_min_velocity); - RCLCPP_DEBUG( - logger, "\t[%lu -> %lu] %ld | ego enters at %2.2f, exits at %2.2f\n", range.entering_path_idx, - range.exiting_path_idx, range.lane.id(), range_times.ego.enter_time, range_times.ego.exit_time); - for (const auto & object : inputs.objects.objects) { - RCLCPP_DEBUG( - logger, "\t\t[%s] going at %2.2fm/s", - tier4_autoware_utils::toHexString(object.object_id).c_str(), - object.kinematics.initial_twist_with_covariance.twist.linear.x); - if (object.kinematics.initial_twist_with_covariance.twist.linear.x < params.objects_min_vel) { - RCLCPP_DEBUG(logger, " SKIP (velocity bellow threshold %2.2fm/s)\n", params.objects_min_vel); - continue; // skip objects with velocity bellow a threshold - } - // skip objects that are already on the interval - const auto enter_exit_time = - params.objects_use_predicted_paths - ? object_time_to_range( - object, range, inputs.route_handler, params.objects_dist_buffer, logger) - : object_time_to_range(object, range, inputs, logger); - if (!enter_exit_time) { - RCLCPP_DEBUG(logger, " SKIP (no enter/exit times found)\n"); - continue; // skip objects that are not driving towards the overlapping range - } - - range_times.object.enter_time = enter_exit_time->first; - range_times.object.exit_time = enter_exit_time->second; - if (will_collide_on_range(range_times, params, logger)) { - range.debug.times = range_times; - range.debug.object = object; - return true; - } - } - range.debug.times = range_times; - return false; -} - -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params) -{ - if (distance < params.stop_dist_threshold) { - decision->velocity = 0.0; - } else if (distance < params.slow_dist_threshold) { - decision->velocity = params.slow_velocity; - } else { - decision.reset(); - } -} - -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger) -{ - std::optional decision; - if (should_not_enter(range, inputs, params, logger)) { - decision.emplace(); - decision->target_path_idx = - inputs.ego_data.first_path_idx + range.entering_path_idx; // add offset from curr pose - decision->lane_to_avoid = range.lane; - const auto ego_dist_to_range = distance_along_path(inputs.ego_data, range.entering_path_idx); - set_decision_velocity(decision, ego_dist_to_range, params); - } - return decision; -} - -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger) -{ - std::vector decisions; - for (const auto & range : inputs.ranges) { - if (range.entering_path_idx == 0UL) continue; // skip if we already entered the range - const auto optional_decision = calculate_decision(range, inputs, params, logger); - range.debug.decision = optional_decision; - if (optional_decision) decisions.push_back(*optional_decision); - } - return decisions; -} - -} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp deleted file mode 100644 index 4f2b0a6dad89b..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ /dev/null @@ -1,115 +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 DECISIONS_HPP_ -#define DECISIONS_HPP_ - -#include "types.hpp" - -#include -#include - -#include - -#include - -#include -#include -#include -#include - -namespace behavior_velocity_planner::out_of_lane -{ -/// @brief calculate the distance along the ego path between ego and some target path index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego path index -/// @return distance between ego and the target [m] -double distance_along_path(const EgoData & ego_data, const size_t target_idx); -/// @brief estimate the time when ego will reach some target path index -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] target_idx target ego path index -/// @param [in] min_velocity minimum ego velocity used to estimate the time -/// @return time taken by ego to reach the target [s] -double time_along_path(const EgoData & ego_data, const size_t target_idx); -/// @brief use an object's predicted paths to estimate the times it will reach the enter and exit -/// points of an overlapping range -/// @details times when the predicted paths of the object enters/exits the range are calculated -/// but may not exist (e.g,, predicted path ends before reaching the end of the range) -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] route_handler route handler used to estimate the path of the dynamic object -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const std::shared_ptr route_handler, const double dist_buffer, - const rclcpp::Logger & logger); -/// @brief use the lanelet map to estimate the times when an object will reach the enter and exit -/// points of an overlapping range -/// @param [in] object dynamic object -/// @param [in] range overlapping range -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] dist_buffer extra distance used to estimate if a collision will occur on the range -/// @param [in] logger ros logger -/// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in -/// the opposite direction, time at enter > time at exit. -std::optional> object_time_to_range( - const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, - const DecisionInputs & inputs, const rclcpp::Logger & logger); -/// @brief decide whether an object is coming in the range at the same time as ego -/// @details the condition depends on the mode (threshold, intervals, ttc) -/// @param [in] range_times times when ego and the object enter/exit the range -/// @param [in] params parameters -/// @param [in] logger ros logger -bool will_collide_on_range( - const RangeTimes & range_times, const PlannerParam & params, const rclcpp::Logger & logger); -/// @brief check whether we should avoid entering the given range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return true if we should avoid entering the range -bool should_not_enter( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief set the velocity of a decision (or unset it) based on the distance away from the range -/// @param [out] decision decision to update (either set its velocity or unset the decision) -/// @param [in] distance distance between ego and the range corresponding to the decision -/// @param [in] params parameters -void set_decision_velocity( - std::optional & decision, const double distance, const PlannerParam & params); -/// @brief calculate the decision to slowdown or stop before an overlapping range -/// @param [in] range the range to check -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return an optional decision to slowdown or stop -std::optional calculate_decision( - const OverlapRange & range, const DecisionInputs & inputs, const PlannerParam & params, - const rclcpp::Logger & logger); -/// @brief calculate decisions to slowdown or stop before some overlapping ranges -/// @param [in] inputs information used to take decisions (ranges, ego and objects data, route -/// handler, lanelets) -/// @param [in] params parameters -/// @param [in] logger ros logger -/// @return return the calculated decisions to slowdown or stop -std::vector calculate_decisions( - const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -} // namespace behavior_velocity_planner::out_of_lane - -#endif // DECISIONS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp deleted file mode 100644 index 80cd106bf52ab..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ /dev/null @@ -1,146 +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 "filter_predicted_objects.hpp" - -#include -#include - -#include - -#include -#include - -#include - -namespace behavior_velocity_planner::out_of_lane -{ -void cut_predicted_path_beyond_line( - autoware_perception_msgs::msg::PredictedPath & predicted_path, - const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) -{ - auto stop_line_idx = 0UL; - bool found = false; - lanelet::BasicSegment2d path_segment; - path_segment.first.x() = predicted_path.path.front().position.x; - path_segment.first.y() = predicted_path.path.front().position.y; - for (stop_line_idx = 1; stop_line_idx < predicted_path.path.size(); ++stop_line_idx) { - path_segment.second.x() = predicted_path.path[stop_line_idx].position.x; - path_segment.second.y() = predicted_path.path[stop_line_idx].position.y; - if (boost::geometry::intersects(stop_line, path_segment)) { - found = true; - break; - } - path_segment.first = path_segment.second; - } - if (found) { - auto cut_idx = stop_line_idx; - double arc_length = 0; - while (cut_idx > 0 && arc_length < object_front_overhang) { - arc_length += tier4_autoware_utils::calcDistance2d( - predicted_path.path[cut_idx], predicted_path.path[cut_idx - 1]); - --cut_idx; - } - predicted_path.path.resize(cut_idx); - } -} - -std::optional find_next_stop_line( - const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) -{ - lanelet::ConstLanelets lanelets; - lanelet::BasicLineString2d query_line; - for (const auto & p : path.path) query_line.emplace_back(p.position.x, p.position.y); - const auto query_results = lanelet::geometry::findWithin2d( - planner_data.route_handler_->getLaneletMapPtr()->laneletLayer, query_line); - for (const auto & r : query_results) lanelets.push_back(r.second); - for (const auto & ll : lanelets) { - for (const auto & element : ll.regulatoryElementsAs()) { - const auto traffic_signal_stamped = planner_data.getTrafficSignal(element->id()); - if ( - traffic_signal_stamped.has_value() && element->stopLine().has_value() && - traffic_light_utils::isTrafficSignalStop(ll, traffic_signal_stamped.value().signal)) { - lanelet::BasicLineString2d stop_line; - for (const auto & p : *(element->stopLine())) stop_line.emplace_back(p.x(), p.y()); - return stop_line; - } - } - } - return std::nullopt; -} - -void cut_predicted_path_beyond_red_lights( - autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, - const double object_front_overhang) -{ - const auto stop_line = find_next_stop_line(predicted_path, planner_data); - if (stop_line) { - // we use a longer stop line to also cut predicted paths that slightly go around the stop line - auto longer_stop_line = *stop_line; - const auto diff = stop_line->back() - stop_line->front(); - longer_stop_line.front() -= diff * 0.5; - longer_stop_line.back() += diff * 0.5; - cut_predicted_path_beyond_line(predicted_path, longer_stop_line, object_front_overhang); - } -} - -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) { - 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(); - if (is_pedestrian) continue; - - auto filtered_object = object; - const auto is_invalid_predicted_path = [&](const auto & predicted_path) { - const auto is_low_confidence = predicted_path.confidence < params.objects_min_confidence; - const auto no_overlap_path = motion_utils::removeOverlapPoints(predicted_path.path); - if (no_overlap_path.size() <= 1) return true; - 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); - return is_low_confidence || is_crossing_ego; - }; - if (params.objects_use_predicted_paths) { - auto & predicted_paths = filtered_object.kinematics.predicted_paths; - const auto new_end = - std::remove_if(predicted_paths.begin(), predicted_paths.end(), is_invalid_predicted_path); - predicted_paths.erase(new_end, predicted_paths.end()); - if (params.objects_cut_predicted_paths_beyond_red_lights) - for (auto & predicted_path : predicted_paths) - cut_predicted_path_beyond_red_lights( - predicted_path, planner_data, filtered_object.shape.dimensions.x); - predicted_paths.erase( - std::remove_if( - predicted_paths.begin(), predicted_paths.end(), - [](const auto & p) { return p.path.empty(); }), - predicted_paths.end()); - } - - if (!params.objects_use_predicted_paths || !filtered_object.kinematics.predicted_paths.empty()) - filtered_objects.objects.push_back(filtered_object); - } - return filtered_objects; -} - -} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp deleted file mode 100644 index be3e8809d2e3d..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ /dev/null @@ -1,57 +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. - -#ifndef FILTER_PREDICTED_OBJECTS_HPP_ -#define FILTER_PREDICTED_OBJECTS_HPP_ - -#include "types.hpp" - -#include - -#include - -namespace behavior_velocity_planner::out_of_lane -{ -/// @brief cut a predicted path beyond the given stop line -/// @param [inout] predicted_path predicted path to cut -/// @param [in] stop_line stop line used for cutting -/// @param [in] object_front_overhang extra distance to cut ahead of the stop line -void cut_predicted_path_beyond_line( - autoware_perception_msgs::msg::PredictedPath & predicted_path, - const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); - -/// @brief find the next red light stop line along the given path -/// @param [in] path predicted path to check for a stop line -/// @param [in] planner_data planner data with stop line information -/// @return the first red light stop line found along the path (if any) -std::optional find_next_stop_line( - const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); - -/// @brief cut predicted path beyond stop lines of red lights -/// @param [inout] predicted_path predicted path to cut -/// @param [in] planner_data planner data to get the map and traffic light information -void cut_predicted_path_beyond_red_lights( - autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, - const double object_front_overhang); - -/// @brief filter predicted objects and their predicted paths -/// @param [in] planner_data planner data -/// @param [in] ego_data ego data -/// @param [in] params parameters -/// @return filtered predicted objects -autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane - -#endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp deleted file mode 100644 index d136f4a8598f3..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ /dev/null @@ -1,90 +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 "footprint.hpp" - -#include - -#include - -#include -#include - -#include -#include - -namespace behavior_velocity_planner::out_of_lane -{ -tier4_autoware_utils::Polygon2d make_base_footprint( - const PlannerParam & p, const bool ignore_offset) -{ - tier4_autoware_utils::Polygon2d base_footprint; - const auto front_offset = ignore_offset ? 0.0 : p.extra_front_offset; - const auto rear_offset = ignore_offset ? 0.0 : p.extra_rear_offset; - const auto right_offset = ignore_offset ? 0.0 : p.extra_right_offset; - const auto left_offset = ignore_offset ? 0.0 : p.extra_left_offset; - base_footprint.outer() = { - {p.front_offset + front_offset, p.left_offset + left_offset}, - {p.front_offset + front_offset, p.right_offset - right_offset}, - {p.rear_offset - rear_offset, p.right_offset - right_offset}, - {p.rear_offset - rear_offset, p.left_offset + left_offset}}; - return base_footprint; -} - -lanelet::BasicPolygon2d project_to_pose( - const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose) -{ - const auto angle = tf2::getYaw(pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); - lanelet::BasicPolygon2d footprint; - for (const auto & p : rotated_footprint.outer()) - footprint.emplace_back(p.x() + pose.position.x, p.y() + pose.position.y); - return footprint; -} - -std::vector calculate_path_footprints( - const EgoData & ego_data, const PlannerParam & params) -{ - const auto base_footprint = make_base_footprint(params); - std::vector path_footprints; - path_footprints.reserve(ego_data.path.points.size()); - double length = 0.0; - const auto range = std::max(params.slow_dist_threshold, params.stop_dist_threshold) + - params.front_offset + params.extra_front_offset; - for (auto i = ego_data.first_path_idx; i < ego_data.path.points.size() && length < range; ++i) { - const auto & path_pose = ego_data.path.points[i].point.pose; - const auto angle = tf2::getYaw(path_pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); - lanelet::BasicPolygon2d footprint; - for (const auto & p : rotated_footprint.outer()) - footprint.emplace_back(p.x() + path_pose.position.x, p.y() + path_pose.position.y); - path_footprints.push_back(footprint); - if (i + 1 < ego_data.path.points.size()) - length += tier4_autoware_utils::calcDistance2d(path_pose, ego_data.path.points[i + 1].point); - } - return path_footprints; -} - -lanelet::BasicPolygon2d calculate_current_ego_footprint( - const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset) -{ - const auto base_footprint = make_base_footprint(params, ignore_offset); - const auto angle = tf2::getYaw(ego_data.pose.orientation); - const auto rotated_footprint = tier4_autoware_utils::rotatePolygon(base_footprint, angle); - lanelet::BasicPolygon2d footprint; - for (const auto & p : rotated_footprint.outer()) - footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); - return footprint; -} -} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp deleted file mode 100644 index a573b6ff3981c..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp +++ /dev/null @@ -1,59 +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 FOOTPRINT_HPP_ -#define FOOTPRINT_HPP_ - -#include "types.hpp" - -#include - -#include - -namespace behavior_velocity_planner -{ -namespace out_of_lane -{ -/// @brief create the base footprint of ego -/// @param [in] p parameters used to create the footprint -/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the -/// footprint -/// @return base ego footprint -tier4_autoware_utils::Polygon2d make_base_footprint( - const PlannerParam & p, const bool ignore_offset = false); -/// @brief project a footprint to the given pose -/// @param [in] base_footprint footprint to project -/// @param [in] pose projection pose -/// @return footprint projected to the given pose -lanelet::BasicPolygon2d project_to_pose( - const tier4_autoware_utils::Polygon2d & base_footprint, const geometry_msgs::msg::Pose & pose); -/// @brief calculate the path footprints -/// @details the resulting polygon follows the format used by the lanelet library: clockwise order -/// and implicit closing edge -/// @param [in] ego_data data related to the ego vehicle (includes its path) -/// @param [in] params parameters -/// @return polygon footprints for each path point starting from ego's current position -std::vector calculate_path_footprints( - const EgoData & ego_data, const PlannerParam & params); -/// @brief calculate the current ego footprint -/// @param [in] ego_data data related to the ego vehicle -/// @param [in] params parameters -/// @param [in] ignore_offset optional parameter, if true, ignore the "extra offsets" to build the -/// footprint -lanelet::BasicPolygon2d calculate_current_ego_footprint( - const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); -} // namespace out_of_lane -} // namespace behavior_velocity_planner - -#endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp deleted file mode 100644 index 67d8a79a63f03..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ /dev/null @@ -1,130 +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 "lanelets_selection.hpp" - -#include - -#include -#include - -#include -#include - -namespace behavior_velocity_planner::out_of_lane -{ - -lanelet::ConstLanelets consecutive_lanelets( - const route_handler::RouteHandler & route_handler, const lanelet::ConstLanelet & lanelet) -{ - lanelet::ConstLanelets consecutives = route_handler.getRoutingGraphPtr()->following(lanelet); - const auto previous = route_handler.getRoutingGraphPtr()->previous(lanelet); - consecutives.insert(consecutives.end(), previous.begin(), previous.end()); - return consecutives; -} - -lanelet::ConstLanelets get_missing_lane_change_lanelets( - lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler) -{ - lanelet::ConstLanelets missing_lane_change_lanelets; - const auto & routing_graph = *route_handler.getRoutingGraphPtr(); - lanelet::ConstLanelets adjacents; - lanelet::ConstLanelets consecutives; - for (const auto & ll : path_lanelets) { - const auto consecutives_of_ll = consecutive_lanelets(route_handler, ll); - std::copy_if( - consecutives_of_ll.begin(), consecutives_of_ll.end(), std::back_inserter(consecutives), - [&](const auto & l) { return !contains_lanelet(consecutives, l.id()); }); - const auto adjacents_of_ll = routing_graph.besides(ll); - std::copy_if( - adjacents_of_ll.begin(), adjacents_of_ll.end(), std::back_inserter(adjacents), - [&](const auto & l) { return !contains_lanelet(adjacents, l.id()); }); - } - std::copy_if( - adjacents.begin(), adjacents.end(), std::back_inserter(missing_lane_change_lanelets), - [&](const auto & l) { - return !contains_lanelet(missing_lane_change_lanelets, l.id()) && - !contains_lanelet(path_lanelets, l.id()) && contains_lanelet(consecutives, l.id()); - }); - return missing_lane_change_lanelets; -} - -lanelet::ConstLanelets calculate_path_lanelets( - const EgoData & ego_data, const route_handler::RouteHandler & route_handler) -{ - const auto lanelet_map_ptr = route_handler.getLaneletMapPtr(); - lanelet::ConstLanelets path_lanelets; - lanelet::BasicLineString2d path_ls; - for (const auto & p : ego_data.path.points) - path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y); - for (const auto & dist_lanelet : - lanelet::geometry::findWithin2d(lanelet_map_ptr->laneletLayer, path_ls)) { - if (!contains_lanelet(path_lanelets, dist_lanelet.second.id())) - path_lanelets.push_back(dist_lanelet.second); - } - const auto missing_lanelets = get_missing_lane_change_lanelets(path_lanelets, route_handler); - path_lanelets.insert(path_lanelets.end(), missing_lanelets.begin(), missing_lanelets.end()); - return path_lanelets; -} - -void add_lane_changeable_lanelets( - lanelet::ConstLanelets & lanelets_to_ignore, const lanelet::ConstLanelets & path_lanelets, - const route_handler::RouteHandler & route_handler) -{ - for (const auto & path_lanelet : path_lanelets) - for (const auto & ll : route_handler.getLaneChangeableNeighbors(path_lanelet)) - if (!contains_lanelet(lanelets_to_ignore, ll.id())) lanelets_to_ignore.push_back(ll); -} - -lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, - const route_handler::RouteHandler & route_handler, const PlannerParam & params) -{ - lanelet::ConstLanelets ignored_lanelets; - // ignore lanelets directly behind ego - const auto behind = - planning_utils::calculateOffsetPoint2d(ego_data.pose, params.rear_offset, 0.0); - const lanelet::BasicPoint2d behind_point(behind.x(), behind.y()); - const auto behind_lanelets = lanelet::geometry::findWithin2d( - route_handler.getLaneletMapPtr()->laneletLayer, behind_point, 0.0); - for (const auto & l : behind_lanelets) { - const auto is_path_lanelet = contains_lanelet(path_lanelets, l.second.id()); - if (!is_path_lanelet) ignored_lanelets.push_back(l.second); - } - if (params.ignore_overlaps_over_lane_changeable_lanelets) - add_lane_changeable_lanelets(ignored_lanelets, path_lanelets, route_handler); - return ignored_lanelets; -} - -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const route_handler::RouteHandler & route_handler, const PlannerParam & params) -{ - lanelet::ConstLanelets other_lanelets; - const lanelet::BasicPoint2d ego_point(ego_data.pose.position.x, ego_data.pose.position.y); - const auto lanelets_within_range = lanelet::geometry::findWithin2d( - route_handler.getLaneletMapPtr()->laneletLayer, ego_point, - std::max(params.slow_dist_threshold, params.stop_dist_threshold) + params.front_offset + - params.extra_front_offset); - for (const auto & ll : lanelets_within_range) { - if (std::string(ll.second.attributeOr(lanelet::AttributeName::Subtype, "none")) != "road") - continue; - const auto is_path_lanelet = contains_lanelet(path_lanelets, ll.second.id()); - const auto is_ignored_lanelet = contains_lanelet(ignored_lanelets, ll.second.id()); - if (!is_path_lanelet && !is_ignored_lanelet) other_lanelets.push_back(ll.second); - } - return other_lanelets; -} -} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp deleted file mode 100644 index 87757a0cb2230..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.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 LANELETS_SELECTION_HPP_ -#define LANELETS_SELECTION_HPP_ - -#include "types.hpp" - -#include - -#include - -namespace behavior_velocity_planner::out_of_lane -{ -/// @brief checks if a lanelet is already contained in a vector of lanelets -/// @param [in] lanelets vector to check -/// @param [in] id lanelet id to check -/// @return true if the given vector contains a lanelet of the given id -inline bool contains_lanelet(const lanelet::ConstLanelets & lanelets, const lanelet::Id id) -{ - return std::find_if(lanelets.begin(), lanelets.end(), [&](const auto & l) { - return l.id() == id; - }) != lanelets.end(); -}; - -/// @brief calculate lanelets crossed by the ego path -/// @details calculated from the ids of the path msg, the lanelets containing path points -/// @param [in] ego_data data about the ego vehicle -/// @param [in] route_handler route handler -/// @return lanelets crossed by the ego vehicle -lanelet::ConstLanelets calculate_path_lanelets( - const EgoData & ego_data, const route_handler::RouteHandler & route_handler); -/// @brief calculate lanelets that may not be crossed by the path but may be overlapped during a -/// lane change -/// @param [in] path_lanelets lanelets driven by the ego vehicle -/// @param [in] route_handler route handler -/// @return lanelets that may be overlapped by a lane change (and are not already in path_lanelets) -lanelet::ConstLanelets get_missing_lane_change_lanelets( - lanelet::ConstLanelets & path_lanelets, const route_handler::RouteHandler & route_handler); -/// @brief calculate lanelets that should be ignored -/// @param [in] ego_data data about the ego vehicle -/// @param [in] path_lanelets lanelets driven by the ego vehicle -/// @param [in] route_handler route handler -/// @param [in] params parameters -/// @return lanelets to ignore -lanelet::ConstLanelets calculate_ignored_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, - const route_handler::RouteHandler & route_handler, const PlannerParam & params); -/// @brief calculate lanelets that should be checked by the module -/// @param [in] ego_data data about the ego vehicle -/// @param [in] path_lanelets lanelets driven by the ego vehicle -/// @param [in] ignored_lanelets lanelets to ignore -/// @param [in] route_handler route handler -/// @param [in] params parameters -/// @return lanelets to check for overlaps -lanelet::ConstLanelets calculate_other_lanelets( - const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, - const lanelet::ConstLanelets & ignored_lanelets, - const route_handler::RouteHandler & route_handler, const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane - -#endif // LANELETS_SELECTION_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp deleted file mode 100644 index 840054d92252f..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ /dev/null @@ -1,107 +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 "manager.hpp" - -#include "scene_out_of_lane.hpp" - -#include -#include - -#include - -#include -#include -#include - -namespace behavior_velocity_planner -{ -using tier4_autoware_utils::getOrDeclareParameter; - -OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) -: SceneModuleManagerInterface(node, getModuleName()), module_id_(0UL) -{ - const std::string ns(getModuleName()); - auto & pp = planner_param_; - - pp.mode = getOrDeclareParameter(node, ns + ".mode"); - pp.skip_if_already_overlapping = - getOrDeclareParameter(node, ns + ".skip_if_already_overlapping"); - pp.ignore_overlaps_over_lane_changeable_lanelets = - getOrDeclareParameter(node, ns + ".ignore_overlaps_over_lane_changeable_lanelets"); - - pp.time_threshold = getOrDeclareParameter(node, ns + ".threshold.time_threshold"); - pp.intervals_ego_buffer = getOrDeclareParameter(node, ns + ".intervals.ego_time_buffer"); - pp.intervals_obj_buffer = - getOrDeclareParameter(node, ns + ".intervals.objects_time_buffer"); - pp.ttc_threshold = getOrDeclareParameter(node, ns + ".ttc.threshold"); - - pp.objects_min_vel = getOrDeclareParameter(node, ns + ".objects.minimum_velocity"); - pp.objects_use_predicted_paths = - getOrDeclareParameter(node, ns + ".objects.use_predicted_paths"); - pp.objects_min_confidence = - getOrDeclareParameter(node, ns + ".objects.predicted_path_min_confidence"); - pp.objects_dist_buffer = getOrDeclareParameter(node, ns + ".objects.distance_buffer"); - pp.objects_cut_predicted_paths_beyond_red_lights = - getOrDeclareParameter(node, ns + ".objects.cut_predicted_paths_beyond_red_lights"); - - pp.overlap_min_dist = getOrDeclareParameter(node, ns + ".overlap.minimum_distance"); - pp.overlap_extra_length = getOrDeclareParameter(node, ns + ".overlap.extra_length"); - - pp.skip_if_over_max_decel = - getOrDeclareParameter(node, ns + ".action.skip_if_over_max_decel"); - pp.precision = getOrDeclareParameter(node, ns + ".action.precision"); - pp.min_decision_duration = getOrDeclareParameter(node, ns + ".action.min_duration"); - pp.dist_buffer = getOrDeclareParameter(node, ns + ".action.distance_buffer"); - pp.slow_velocity = getOrDeclareParameter(node, ns + ".action.slowdown.velocity"); - pp.slow_dist_threshold = - getOrDeclareParameter(node, ns + ".action.slowdown.distance_threshold"); - pp.stop_dist_threshold = - getOrDeclareParameter(node, ns + ".action.stop.distance_threshold"); - - pp.ego_min_velocity = getOrDeclareParameter(node, ns + ".ego.min_assumed_velocity"); - pp.extra_front_offset = getOrDeclareParameter(node, ns + ".ego.extra_front_offset"); - pp.extra_rear_offset = getOrDeclareParameter(node, ns + ".ego.extra_rear_offset"); - pp.extra_left_offset = getOrDeclareParameter(node, ns + ".ego.extra_left_offset"); - pp.extra_right_offset = getOrDeclareParameter(node, ns + ".ego.extra_right_offset"); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); - pp.front_offset = vehicle_info.max_longitudinal_offset_m; - pp.rear_offset = vehicle_info.min_longitudinal_offset_m; - pp.left_offset = vehicle_info.max_lateral_offset_m; - pp.right_offset = vehicle_info.min_lateral_offset_m; -} - -void OutOfLaneModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - if (path.points.empty()) return; - // general - if (!isModuleRegistered(module_id_)) { - registerModule(std::make_shared( - module_id_, planner_param_, logger_.get_child("out_of_lane_module"), clock_)); - } -} - -std::function &)> -OutOfLaneModuleManager::getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) -{ - return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { - return false; - }; -} -} // namespace behavior_velocity_planner - -#include -PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OutOfLaneModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp deleted file mode 100644 index 9da1751576a7f..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ /dev/null @@ -1,66 +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 MANAGER_HPP_ -#define MANAGER_HPP_ - -#include "scene_out_of_lane.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include - -namespace behavior_velocity_planner -{ -class OutOfLaneModuleManager : public SceneModuleManagerInterface -{ -public: - explicit OutOfLaneModuleManager(rclcpp::Node & node); - - const char * getModuleName() override { return "out_of_lane"; } - -private: - using PlannerParam = out_of_lane::PlannerParam; - - PlannerParam planner_param_; - int64_t module_id_; - - void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; - - std::function &)> getModuleExpiredFunction( - const tier4_planning_msgs::msg::PathWithLaneId & path) override; -}; - -class OutOfLaneModulePlugin : public PluginWrapper -{ -}; - -} // namespace behavior_velocity_planner - -#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp deleted file mode 100644 index 6dfb41ccfbfcb..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp +++ /dev/null @@ -1,126 +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 "overlapping_range.hpp" - -#include -#include - -#include - -#include -#include - -#include - -namespace behavior_velocity_planner::out_of_lane -{ - -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & path_footprint, const lanelet::ConstLanelets & path_lanelets, - const lanelet::ConstLanelet & lanelet) -{ - Overlap overlap; - const auto & left_bound = lanelet.leftBound2d().basicLineString(); - const auto & right_bound = lanelet.rightBound2d().basicLineString(); - // TODO(Maxime): these intersects (for each path footprint, for each lanelet) are too expensive - const auto overlap_left = boost::geometry::intersects(path_footprint, left_bound); - const auto overlap_right = boost::geometry::intersects(path_footprint, right_bound); - - lanelet::BasicPolygons2d overlapping_polygons; - if (overlap_left || overlap_right) - boost::geometry::intersection( - path_footprint, lanelet.polygon2d().basicPolygon(), overlapping_polygons); - for (const auto & overlapping_polygon : overlapping_polygons) { - for (const auto & point : overlapping_polygon) { - if (overlap_left && overlap_right) - overlap.inside_distance = boost::geometry::distance(left_bound, right_bound); - else if (overlap_left) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, left_bound)); - else if (overlap_right) - overlap.inside_distance = - std::max(overlap.inside_distance, boost::geometry::distance(point, right_bound)); - geometry_msgs::msg::Pose p; - p.position.x = point.x(); - p.position.y = point.y(); - const auto length = lanelet::utils::getArcCoordinates(path_lanelets, p).length; - if (length > overlap.max_arc_length) { - overlap.max_arc_length = length; - overlap.max_overlap_point = point; - } - if (length < overlap.min_arc_length) { - overlap.min_arc_length = length; - overlap.min_overlap_point = point; - } - } - } - return overlap; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & path_footprints, - const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params) -{ - OverlapRanges ranges; - OtherLane other_lane(lanelet); - std::vector overlaps; - for (auto i = 0UL; i < path_footprints.size(); ++i) { - const auto overlap = calculate_overlap(path_footprints[i], path_lanelets, lanelet); - const auto has_overlap = overlap.inside_distance > params.overlap_min_dist; - if (has_overlap) { // open/update the range - overlaps.push_back(overlap); - if (!other_lane.range_is_open) { - other_lane.first_range_bound.index = i; - other_lane.first_range_bound.point = overlap.min_overlap_point; - other_lane.first_range_bound.arc_length = - overlap.min_arc_length - params.overlap_extra_length; - other_lane.first_range_bound.inside_distance = overlap.inside_distance; - other_lane.range_is_open = true; - } - other_lane.last_range_bound.index = i; - other_lane.last_range_bound.point = overlap.max_overlap_point; - other_lane.last_range_bound.arc_length = overlap.max_arc_length + params.overlap_extra_length; - other_lane.last_range_bound.inside_distance = overlap.inside_distance; - } else if (other_lane.range_is_open) { // !has_overlap: close the range if it is open - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - } - // close the range if it is still open - if (other_lane.range_is_open) { - ranges.push_back(other_lane.close_range()); - ranges.back().debug.overlaps = overlaps; - overlaps.clear(); - } - return ranges; -} - -OverlapRanges calculate_overlapping_ranges( - const std::vector & path_footprints, - const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params) -{ - OverlapRanges ranges; - for (auto & lanelet : lanelets) { - const auto lanelet_ranges = - calculate_overlapping_ranges(path_footprints, path_lanelets, lanelet, params); - ranges.insert(ranges.end(), lanelet_ranges.begin(), lanelet_ranges.end()); - } - return ranges; -} - -} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp deleted file mode 100644 index 2b0830acc28cc..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp +++ /dev/null @@ -1,60 +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 OVERLAPPING_RANGE_HPP_ -#define OVERLAPPING_RANGE_HPP_ - -#include "types.hpp" - -#include - -#include - -#include - -namespace behavior_velocity_planner::out_of_lane -{ - -/// @brief calculate the overlap between the given footprint and lanelet -/// @param [in] path_footprint footprint used to calculate the overlap -/// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path -/// @param [in] lanelet lanelet used to calculate the overlap -/// @return the found overlap between the footprint and the lanelet -Overlap calculate_overlap( - const lanelet::BasicPolygon2d & path_footprint, const lanelet::ConstLanelets & path_lanelets, - const lanelet::ConstLanelet & lanelet); -/// @brief calculate the overlapping ranges between the path footprints and a lanelet -/// @param [in] path_footprints footprints used to calculate the overlaps -/// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path -/// @param [in] lanelet lanelet used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelet -OverlapRanges calculate_overlapping_ranges( - const std::vector & path_footprints, - const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelet & lanelet, - const PlannerParam & params); -/// @brief calculate the overlapping ranges between the path footprints and some lanelets -/// @param [in] path_footprints footprints used to calculate the overlaps -/// @param [in] path_lanelets path lanelets used to calculate arc length along the ego path -/// @param [in] lanelets lanelets used to calculate the overlaps -/// @param [in] params parameters -/// @return the overlapping ranges found between the footprints and the lanelets, sorted by -/// increasing arc length along the path -OverlapRanges calculate_overlapping_ranges( - const std::vector & path_footprints, - const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets, - const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane - -#endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp deleted file mode 100644 index 067a5e3dc7698..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ /dev/null @@ -1,244 +0,0 @@ -// Copyright 2023 TIER IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "scene_out_of_lane.hpp" - -#include "calculate_slowdown_points.hpp" -#include "debug.hpp" -#include "decisions.hpp" -#include "filter_predicted_objects.hpp" -#include "footprint.hpp" -#include "lanelets_selection.hpp" -#include "overlapping_range.hpp" -#include "types.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include - -#include -#include -#include -#include - -namespace behavior_velocity_planner::out_of_lane -{ - -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; - -OutOfLaneModule::OutOfLaneModule( - const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) -{ - velocity_factor_.init(PlanningBehavior::ROUTE_OBSTACLE); -} - -bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) -{ - debug_data_.reset_data(); - *stop_reason = planning_utils::initializeStopReason(StopReason::OUT_OF_LANE); - if (!path || path->points.size() < 2) return true; - tier4_autoware_utils::StopWatch stopwatch; - stopwatch.tic(); - EgoData ego_data; - ego_data.pose = planner_data_->current_odometry->pose; - ego_data.path.points = path->points; - ego_data.first_path_idx = - motion_utils::findNearestSegmentIndex(path->points, ego_data.pose.position); - motion_utils::removeOverlapPoints(ego_data.path.points); - ego_data.velocity = planner_data_->current_velocity->twist.linear.x; - ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold; - stopwatch.tic("calculate_path_footprints"); - const auto current_ego_footprint = calculate_current_ego_footprint(ego_data, params_, true); - const auto path_footprints = calculate_path_footprints(ego_data, params_); - const auto calculate_path_footprints_us = stopwatch.toc("calculate_path_footprints"); - // Calculate lanelets to ignore and consider - stopwatch.tic("calculate_lanelets"); - const auto path_lanelets = calculate_path_lanelets(ego_data, *planner_data_->route_handler_); - const auto ignored_lanelets = - calculate_ignored_lanelets(ego_data, path_lanelets, *planner_data_->route_handler_, params_); - const auto other_lanelets = calculate_other_lanelets( - ego_data, path_lanelets, ignored_lanelets, *planner_data_->route_handler_, params_); - const auto calculate_lanelets_us = stopwatch.toc("calculate_lanelets"); - - debug_data_.footprints = path_footprints; - debug_data_.path_lanelets = path_lanelets; - debug_data_.ignored_lanelets = ignored_lanelets; - debug_data_.other_lanelets = other_lanelets; - debug_data_.path = ego_data.path; - debug_data_.first_path_idx = ego_data.first_path_idx; - - if (params_.skip_if_already_overlapping) { - debug_data_.current_footprint = current_ego_footprint; - const auto overlapped_lanelet_it = - std::find_if(other_lanelets.begin(), other_lanelets.end(), [&](const auto & ll) { - return boost::geometry::intersects(ll.polygon2d().basicPolygon(), current_ego_footprint); - }); - if (overlapped_lanelet_it != other_lanelets.end()) { - debug_data_.current_overlapped_lanelets.push_back(*overlapped_lanelet_it); - // TODO(Maxime): we may want to just add the overlapped lane to the ignored lanelets - RCLCPP_DEBUG(logger_, "Ego is already overlapping a lane, skipping the module ()\n"); - return true; - } - } - // Calculate overlapping ranges - stopwatch.tic("calculate_overlapping_ranges"); - const auto ranges = - calculate_overlapping_ranges(path_footprints, path_lanelets, other_lanelets, params_); - const auto calculate_overlapping_ranges_us = stopwatch.toc("calculate_overlapping_ranges"); - // Calculate stop and slowdown points - DecisionInputs inputs; - inputs.ranges = ranges; - inputs.ego_data = ego_data; - stopwatch.tic("filter_predicted_objects"); - inputs.objects = filter_predicted_objects(*planner_data_, ego_data, params_); - const auto filter_predicted_objects_ms = stopwatch.toc("filter_predicted_objects"); - inputs.route_handler = planner_data_->route_handler_; - inputs.lanelets = other_lanelets; - stopwatch.tic("calculate_decisions"); - const auto decisions = calculate_decisions(inputs, params_, logger_); - const auto calculate_decisions_us = stopwatch.toc("calculate_decisions"); - stopwatch.tic("calc_slowdown_points"); - if ( // reset the previous inserted point if the timer expired - prev_inserted_point_ && - (clock_->now() - prev_inserted_point_time_).seconds() > params_.min_decision_duration) - prev_inserted_point_.reset(); - auto point_to_insert = - calculate_slowdown_point(ego_data, decisions, prev_inserted_point_, params_); - const auto calc_slowdown_points_us = stopwatch.toc("calc_slowdown_points"); - stopwatch.tic("insert_slowdown_points"); - debug_data_.slowdowns.clear(); - if ( // reset the timer if there is no previous inserted point or if we avoid the same lane - point_to_insert && - (!prev_inserted_point_ || prev_inserted_point_->slowdown.lane_to_avoid.id() == - point_to_insert->slowdown.lane_to_avoid.id())) - prev_inserted_point_time_ = clock_->now(); - // reuse previous stop point if there is no new one or if its velocity is not higher than the new - // one and its arc length is lower - const auto should_use_prev_inserted_point = [&]() { - if ( - point_to_insert && prev_inserted_point_ && - prev_inserted_point_->slowdown.velocity <= point_to_insert->slowdown.velocity) { - const auto arc_length = motion_utils::calcSignedArcLength( - path->points, 0LU, point_to_insert->point.point.pose.position); - const auto prev_arc_length = motion_utils::calcSignedArcLength( - path->points, 0LU, prev_inserted_point_->point.point.pose.position); - return prev_arc_length < arc_length; - } - return !point_to_insert && prev_inserted_point_; - }(); - if (should_use_prev_inserted_point) { - // if the path changed the prev point is no longer on the path so we project it - const auto insert_arc_length = motion_utils::calcSignedArcLength( - path->points, 0LU, prev_inserted_point_->point.point.pose.position); - prev_inserted_point_->point.point.pose = - motion_utils::calcInterpolatedPose(path->points, insert_arc_length); - point_to_insert = prev_inserted_point_; - } - if (point_to_insert) { - prev_inserted_point_ = point_to_insert; - RCLCPP_DEBUG(logger_, "Avoiding lane %lu", point_to_insert->slowdown.lane_to_avoid.id()); - debug_data_.slowdowns = {*point_to_insert}; - auto path_idx = motion_utils::findNearestSegmentIndex( - path->points, point_to_insert->point.point.pose.position) + - 1; - planning_utils::insertVelocity( - *path, point_to_insert->point, point_to_insert->slowdown.velocity, path_idx); - auto stop_pose_reached = false; - if (point_to_insert->slowdown.velocity == 0.0) { - const auto dist_to_stop_pose = motion_utils::calcSignedArcLength( - path->points, ego_data.pose.position, point_to_insert->point.point.pose.position); - if (ego_data.velocity < 1e-3 && dist_to_stop_pose < 1e-3) stop_pose_reached = true; - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = point_to_insert->point.point.pose; - stop_factor.dist_to_stop_pose = dist_to_stop_pose; - planning_utils::appendStopReason(stop_factor, stop_reason); - } - velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, point_to_insert->point.point.pose, - stop_pose_reached ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING, "out_of_lane"); - } else if (!decisions.empty()) { - RCLCPP_WARN(logger_, "Could not insert stop point (would violate max deceleration limits)"); - } - const auto insert_slowdown_points_us = stopwatch.toc("insert_slowdown_points"); - debug_data_.ranges = inputs.ranges; - - const auto total_time_us = stopwatch.toc(); - RCLCPP_DEBUG( - logger_, - "Total time = %2.2fus\n" - "\tcalculate_lanelets = %2.0fus\n" - "\tcalculate_path_footprints = %2.0fus\n" - "\tcalculate_overlapping_ranges = %2.0fus\n" - "\tfilter_pred_objects = %2.0fus\n" - "\tcalculate_decisions = %2.0fus\n" - "\tcalc_slowdown_points = %2.0fus\n" - "\tinsert_slowdown_points = %2.0fus\n", - total_time_us, calculate_lanelets_us, calculate_path_footprints_us, - calculate_overlapping_ranges_us, filter_predicted_objects_ms, calculate_decisions_us, - calc_slowdown_points_us, insert_slowdown_points_us); - return true; -} - -MarkerArray OutOfLaneModule::createDebugMarkerArray() -{ - constexpr auto z = 0.0; - MarkerArray debug_marker_array; - - debug::add_footprint_markers( - debug_marker_array, debug_data_.footprints, z, debug_data_.prev_footprints); - debug::add_current_overlap_marker( - debug_marker_array, debug_data_.current_footprint, debug_data_.current_overlapped_lanelets, z, - debug_data_.prev_current_overlapped_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data_.path_lanelets, "path_lanelets", - tier4_autoware_utils::createMarkerColor(0.1, 0.1, 1.0, 0.5), debug_data_.prev_path_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data_.ignored_lanelets, "ignored_lanelets", - tier4_autoware_utils::createMarkerColor(0.7, 0.7, 0.2, 0.5), debug_data_.prev_ignored_lanelets); - debug::add_lanelet_markers( - debug_marker_array, debug_data_.other_lanelets, "other_lanelets", - tier4_autoware_utils::createMarkerColor(0.4, 0.4, 0.7, 0.5), debug_data_.prev_other_lanelets); - debug::add_range_markers( - debug_marker_array, debug_data_.ranges, debug_data_.path, debug_data_.first_path_idx, z, - debug_data_.prev_ranges); - return debug_marker_array; -} - -motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() -{ - motion_utils::VirtualWalls virtual_walls; - motion_utils::VirtualWall wall; - wall.text = "out_of_lane"; - wall.longitudinal_offset = params_.front_offset; - wall.style = motion_utils::VirtualWallType::slowdown; - for (const auto & slowdown : debug_data_.slowdowns) { - wall.pose = slowdown.point.point.pose; - virtual_walls.push_back(wall); - } - return virtual_walls; -} - -} // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp deleted file mode 100644 index 1d51c45c6afd1..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ /dev/null @@ -1,65 +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 SCENE_OUT_OF_LANE_HPP_ -#define SCENE_OUT_OF_LANE_HPP_ - -#include "types.hpp" - -#include -#include -#include - -#include -#include -#include - -#include - -#include -#include -#include - -namespace behavior_velocity_planner::out_of_lane -{ -class OutOfLaneModule : public SceneModuleInterface -{ -public: - OutOfLaneModule( - const int64_t module_id, PlannerParam planner_param, const rclcpp::Logger & logger, - const rclcpp::Clock::SharedPtr clock); - - /// @brief insert stop or slow down points to prevent dangerously entering another lane - /// @param [inout] path the path to update - /// @param [inout] stop_reason reason for stopping - bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; - - visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; - motion_utils::VirtualWalls createVirtualWalls() override; - -private: - PlannerParam params_; - - std::optional prev_inserted_point_{}; - rclcpp::Time prev_inserted_point_time_{}; - -protected: - int64_t module_id_{}; - - // Debug - mutable DebugData debug_data_; -}; -} // namespace behavior_velocity_planner::out_of_lane - -#endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp deleted file mode 100644 index cb2945f8b32b8..0000000000000 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ /dev/null @@ -1,235 +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 TYPES_HPP_ -#define TYPES_HPP_ - -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include - -namespace behavior_velocity_planner::out_of_lane -{ -/// @brief parameters for the "out of lane" module -struct PlannerParam -{ - std::string mode; // mode used to consider a conflict with an object - bool skip_if_already_overlapping; // if true, do not run the module when ego already overlaps - // another lane - bool ignore_overlaps_over_lane_changeable_lanelets; // if true, overlaps on lane changeable - // lanelets are ignored - - double time_threshold; // [s](mode="threshold") objects time threshold - double intervals_ego_buffer; // [s](mode="intervals") buffer to extend the ego time range - double intervals_obj_buffer; // [s](mode="intervals") buffer to extend the objects time range - double ttc_threshold; // [s](mode="ttc") threshold on time to collision between ego and an object - double ego_min_velocity; // [m/s] minimum velocity of ego used to calculate its ttc or time range - - bool objects_use_predicted_paths; // whether to use the objects' predicted paths - bool objects_cut_predicted_paths_beyond_red_lights; // whether to cut predicted paths beyond red - // lights' stop lines - double objects_min_vel; // [m/s] objects lower than this velocity will be ignored - double objects_min_confidence; // minimum confidence to consider a predicted path - double objects_dist_buffer; // [m] distance buffer used to determine if a collision will occur in - // the other lane - - double overlap_extra_length; // [m] extra length to add around an overlap range - double overlap_min_dist; // [m] min distance inside another lane to consider an overlap - // action to insert in the path if an object causes a conflict at an overlap - bool skip_if_over_max_decel; // if true, skip the action if it causes more than the max decel - double dist_buffer; - double slow_velocity; - double slow_dist_threshold; - double stop_dist_threshold; - double precision; // [m] precision when inserting a stop pose in the path - double min_decision_duration; // [s] minimum duration needed a decision can be canceled - // ego dimensions used to create its polygon footprint - double front_offset; // [m] front offset (from vehicle info) - double rear_offset; // [m] rear offset (from vehicle info) - double right_offset; // [m] right offset (from vehicle info) - double left_offset; // [m] left offset (from vehicle info) - double extra_front_offset; // [m] extra front distance - double extra_rear_offset; // [m] extra rear distance - double extra_right_offset; // [m] extra right distance - double extra_left_offset; // [m] extra left distance -}; - -struct EnterExitTimes -{ - double enter_time{}; - double exit_time{}; -}; -struct RangeTimes -{ - EnterExitTimes ego{}; - EnterExitTimes object{}; -}; - -/// @brief action taken by the "out of lane" module -struct Slowdown -{ - size_t target_path_idx{}; // we want to slowdown before this path index - double velocity{}; // desired slow down velocity - lanelet::ConstLanelet lane_to_avoid; // we want to slowdown before entering this lane -}; -/// @brief slowdown to insert in a path -struct SlowdownToInsert -{ - Slowdown slowdown; - tier4_planning_msgs::msg::PathWithLaneId::_points_type::value_type point; -}; - -/// @brief bound of an overlap range (either the first, or last bound) -struct RangeBound -{ - size_t index; - lanelet::BasicPoint2d point; - double arc_length; - double inside_distance; -}; - -/// @brief representation of an overlap between the ego footprint and some other lane -struct Overlap -{ - double inside_distance = 0.0; ///!< distance inside the overlap - double min_arc_length = std::numeric_limits::infinity(); - double max_arc_length = 0.0; - lanelet::BasicPoint2d min_overlap_point{}; ///!< point with min arc length - lanelet::BasicPoint2d max_overlap_point{}; ///!< point with max arc length -}; - -/// @brief range along the path where ego overlaps another lane -struct OverlapRange -{ - lanelet::ConstLanelet lane; - size_t entering_path_idx{}; - size_t exiting_path_idx{}; - lanelet::BasicPoint2d entering_point; // pose of the overlapping point closest along the lane - lanelet::BasicPoint2d exiting_point; // pose of the overlapping point furthest along the lane - double inside_distance{}; // [m] how much ego footprint enters the lane - mutable struct - { - std::vector overlaps; - std::optional decision; - RangeTimes times; - std::optional object{}; - } debug; -}; -using OverlapRanges = std::vector; -/// @brief representation of a lane and its current overlap range -struct OtherLane -{ - bool range_is_open = false; - RangeBound first_range_bound{}; - RangeBound last_range_bound{}; - lanelet::ConstLanelet lanelet; - lanelet::BasicPolygon2d polygon; - - explicit OtherLane(lanelet::ConstLanelet ll) : lanelet(std::move(ll)) - { - polygon = lanelet.polygon2d().basicPolygon(); - } - - [[nodiscard]] OverlapRange close_range() - { - OverlapRange range; - range.lane = lanelet; - range.entering_path_idx = first_range_bound.index; - range.entering_point = first_range_bound.point; - range.exiting_path_idx = last_range_bound.index; - range.exiting_point = last_range_bound.point; - range.inside_distance = - std::max(first_range_bound.inside_distance, last_range_bound.inside_distance); - range_is_open = false; - last_range_bound = {}; - return range; - } -}; - -/// @brief data related to the ego vehicle -struct EgoData -{ - tier4_planning_msgs::msg::PathWithLaneId path{}; - size_t first_path_idx{}; - double velocity{}; // [m/s] - double max_decel{}; // [m/s²] - geometry_msgs::msg::Pose pose; -}; - -/// @brief data needed to make decisions -struct DecisionInputs -{ - OverlapRanges ranges{}; - EgoData ego_data; - autoware_perception_msgs::msg::PredictedObjects objects{}; - std::shared_ptr route_handler{}; - lanelet::ConstLanelets lanelets{}; -}; - -/// @brief debug data -struct DebugData -{ - std::vector footprints; - std::vector slowdowns; - geometry_msgs::msg::Pose ego_pose; - OverlapRanges ranges; - lanelet::BasicPolygon2d current_footprint; - lanelet::ConstLanelets current_overlapped_lanelets; - lanelet::ConstLanelets path_lanelets; - lanelet::ConstLanelets ignored_lanelets; - lanelet::ConstLanelets other_lanelets; - tier4_planning_msgs::msg::PathWithLaneId path; - size_t first_path_idx; - - size_t prev_footprints = 0; - size_t prev_slowdowns = 0; - size_t prev_ranges = 0; - size_t prev_current_overlapped_lanelets = 0; - size_t prev_ignored_lanelets = 0; - size_t prev_path_lanelets = 0; - size_t prev_other_lanelets = 0; - void reset_data() - { - prev_footprints = footprints.size(); - footprints.clear(); - prev_slowdowns = slowdowns.size(); - slowdowns.clear(); - prev_ranges = ranges.size(); - ranges.clear(); - prev_current_overlapped_lanelets = current_overlapped_lanelets.size(); - current_overlapped_lanelets.clear(); - prev_ignored_lanelets = ignored_lanelets.size(); - ignored_lanelets.clear(); - prev_path_lanelets = path_lanelets.size(); - path_lanelets.clear(); - prev_other_lanelets = other_lanelets.size(); - other_lanelets.clear(); - } -}; - -} // namespace behavior_velocity_planner::out_of_lane - -#endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml index 9b1de66c95c92..62e215951757d 100644 --- a/planning/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -15,8 +15,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_speed_bump_module/plugins.xml b/planning/behavior_velocity_speed_bump_module/plugins.xml index 506d25669f8cf..48287a7f94b4b 100644 --- a/planning/behavior_velocity_speed_bump_module/plugins.xml +++ b/planning/behavior_velocity_speed_bump_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp index 7409c24e7e8bf..acbe3e05debaa 100644 --- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -23,7 +23,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; @@ -120,4 +120,4 @@ visualization_msgs::msg::MarkerArray SpeedBumpModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index fcd000699b7ff..42f10586d8fec 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::SpeedBump; using tier4_autoware_utils::getOrDeclareParameter; @@ -80,8 +80,9 @@ SpeedBumpModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::SpeedBumpModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::SpeedBumpModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp index 40fcfdd081c2e..41b9a593dde95 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class SpeedBumpModuleManager : public SceneModuleManagerInterface { @@ -49,6 +49,6 @@ class SpeedBumpModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_speed_bump_module/src/scene.cpp index dc944cc0c292b..0ca4e0731b155 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.cpp @@ -20,7 +20,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; @@ -169,4 +169,4 @@ bool SpeedBumpModule::applySlowDownSpeed( output.points, slow_start_point_idx, slow_end_point_idx, speed_bump_speed); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp index f227366127046..ffbcf5fdf9325 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -17,14 +17,14 @@ #include "util.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -81,6 +81,6 @@ class SpeedBumpModule : public SceneModuleInterface float speed_bump_slow_down_speed_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp index d1533d7028dcf..ca4e7e379d1ea 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -17,7 +17,7 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -172,4 +172,4 @@ float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float spee return std::clamp(speed, p2.y, p1.y); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_speed_bump_module/src/util.hpp index 2cd408735fd61..72bad3db86f73 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.hpp @@ -35,7 +35,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -70,6 +70,6 @@ std::optional insertPointWithOffset( // returns y (speed) for y=mx+b float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float speed_bump_height); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_stop_line_module/plugins.xml deleted file mode 100644 index 51fb225fbebad..0000000000000 --- a/planning/behavior_velocity_stop_line_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index ff7b5a269db32..067dcaca6a638 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -19,9 +19,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_traffic_light_module/plugins.xml b/planning/behavior_velocity_traffic_light_module/plugins.xml index b65cc66c5c232..9b5e84fff982d 100644 --- a/planning/behavior_velocity_traffic_light_module/plugins.xml +++ b/planning/behavior_velocity_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index b3dedaa29d6ad..7ceded9c91934 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -24,7 +24,7 @@ #include #endif -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray() { @@ -54,4 +54,4 @@ motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 01c3acf84cd63..ef5f8b2b36016 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -24,7 +24,7 @@ #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::TrafficLight; using tier4_autoware_utils::getOrDeclareParameter; @@ -188,8 +188,9 @@ bool TrafficLightModuleManager::hasSameTrafficLight( return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::TrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::TrafficLightModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 97340f8592a7d..c40056a0ba10c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -65,6 +65,6 @@ class TrafficLightModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index c886578dc65e4..3ec597f1de249 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -432,4 +432,4 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( return modified_path; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 8385a1210d421..220145a2c7163 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -23,15 +23,15 @@ #define EIGEN_MPL2_ONLY #include #include -#include -#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class TrafficLightModule : public SceneModuleInterface { @@ -129,6 +129,6 @@ class TrafficLightModule : public SceneModuleInterface // Traffic Light State TrafficSignal looking_tl_state_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index f878953d50420..6782736c3e173 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -20,6 +20,7 @@ autoware_adapi_v1_msgs autoware_map_msgs autoware_planning_msgs + autoware_vehicle_info_utils geometry_msgs lanelet2_extension motion_utils @@ -31,7 +32,6 @@ tf2_ros tier4_autoware_utils tier4_planning_msgs - vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 5ba152af6611d..a7f3cfaa7ee34 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -16,6 +16,7 @@ #include "utility_functions.hpp" +#include #include #include #include @@ -25,7 +26,6 @@ #include #include #include -#include #include #include @@ -108,7 +108,7 @@ double project_goal_to_map( geometry_msgs::msg::Pose get_closest_centerline_pose( const lanelet::ConstLanelets & road_lanelets, const geometry_msgs::msg::Pose & point, - vehicle_info_util::VehicleInfo vehicle_info) + autoware::vehicle_info_utils::VehicleInfo vehicle_info) { lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLaneletWithConstrains( @@ -152,7 +152,7 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) pub_goal_footprint_marker_ = node_->create_publisher("~/debug/goal_footprint", durable_qos); - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); param_.enable_correct_goal_pose = node_->declare_parameter("enable_correct_goal_pose"); param_.consider_no_drivable_lanes = node_->declare_parameter("consider_no_drivable_lanes"); diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 60c232162991d..49000a750a9a1 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -15,10 +15,10 @@ #ifndef LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ #define LANELET2_PLUGINS__DEFAULT_PLANNER_HPP_ +#include #include #include #include -#include #include #include @@ -52,7 +52,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin void clearRoute() override; MarkerArray visualize(const LaneletRoute & route) const override; MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; private: using RouteSections = std::vector; diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp index 428cd979a2526..ed8dbfed63bfe 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -15,10 +15,10 @@ #ifndef LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ #define LANELET2_PLUGINS__UTILITY_FUNCTIONS_HPP_ +#include #include #include #include -#include #include #include 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 051415e9856ed..a6176555ae7af 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 @@ -17,6 +17,7 @@ autoware_motion_velocity_planner_common autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_info_utils geometry_msgs lanelet2_extension libboost-dev @@ -28,7 +29,6 @@ tier4_autoware_utils tier4_planning_msgs traffic_light_utils - vehicle_info_util visualization_msgs ament_cmake_ros 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 ce993dc1cedb0..77292a41b9cf1 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 @@ -103,8 +103,8 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( 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->predicted_objects.objects) { 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; 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 2cd050d15dd9b..11640f9ad2c8e 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 @@ -100,7 +100,7 @@ void OutOfLaneModule::init_parameters(rclcpp::Node & node) pp.extra_rear_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_rear_offset"); pp.extra_left_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_left_offset"); pp.extra_right_offset = getOrDeclareParameter(node, ns_ + ".ego.extra_right_offset"); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); pp.front_offset = vehicle_info.max_longitudinal_offset_m; pp.rear_offset = vehicle_info.min_longitudinal_offset_m; pp.left_offset = vehicle_info.max_lateral_offset_m; @@ -154,11 +154,11 @@ VelocityPlanningResult OutOfLaneModule::plan( tier4_autoware_utils::StopWatch stopwatch; stopwatch.tic(); out_of_lane::EgoData ego_data; - ego_data.pose = planner_data->current_odometry->pose; + ego_data.pose = planner_data->current_odometry.pose.pose; ego_data.trajectory_points = ego_trajectory_points; ego_data.first_trajectory_idx = motion_utils::findNearestSegmentIndex(ego_trajectory_points, ego_data.pose.position); - ego_data.velocity = planner_data->current_velocity->twist.linear.x; + ego_data.velocity = planner_data->current_odometry.twist.twist.linear.x; ego_data.max_decel = planner_data->velocity_smoother_->getMinDecel(); stopwatch.tic("calculate_trajectory_footprints"); const auto current_ego_footprint = 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/include/autoware_motion_velocity_planner_common/planner_data.hpp index add6b5ef392ea..a25ab5d55a910 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/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -15,18 +15,17 @@ #ifndef AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #define AUTOWARE_MOTION_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#include #include #include -#include #include #include #include #include #include -#include -#include #include +#include #include #include #include @@ -55,17 +54,16 @@ struct TrafficSignalStamped struct PlannerData { explicit PlannerData(rclcpp::Node & node) - : vehicle_info_(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()) + : vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()) { } // msgs from callbacks that are used for data-ready - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; - geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; - geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; - autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; - pcl::PointCloud::ConstPtr no_ground_pointcloud; - nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; + nav_msgs::msg::Odometry current_odometry{}; + geometry_msgs::msg::AccelWithCovarianceStamped current_acceleration{}; + autoware_perception_msgs::msg::PredictedObjects predicted_objects{}; + pcl::PointCloud no_ground_pointcloud{}; + nav_msgs::msg::OccupancyGrid occupancy_grid{}; std::shared_ptr route_handler; // nearest search @@ -78,12 +76,12 @@ 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; + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_{}; // parameters - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; /** *@fn diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index 2f4e7241d60ee..a115d28ee6d86 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -15,10 +15,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs autoware_velocity_smoother - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension 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/src/node.cpp index 4d152afa250b7..c53de15d67ebb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -63,38 +63,10 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & sub_trajectory_ = this->create_subscription( "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1), create_subscription_options(this)); - sub_predicted_objects_ = - this->create_subscription( - "~/input/dynamic_objects", 1, - std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1), - create_subscription_options(this)); - sub_no_ground_pointcloud_ = this->create_subscription( - "~/input/no_ground_pointcloud", rclcpp::SensorDataQoS(), - std::bind(&MotionVelocityPlannerNode::on_no_ground_pointcloud, this, _1), - create_subscription_options(this)); - sub_vehicle_odometry_ = this->create_subscription( - "~/input/vehicle_odometry", 1, std::bind(&MotionVelocityPlannerNode::on_odometry, this, _1), - create_subscription_options(this)); - sub_acceleration_ = this->create_subscription( - "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), - create_subscription_options(this)); sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1), create_subscription_options(this)); - sub_traffic_signals_ = - this->create_subscription( - "~/input/traffic_signals", 1, - std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1), - create_subscription_options(this)); - sub_virtual_traffic_light_states_ = - this->create_subscription( - "~/input/virtual_traffic_light_states", 1, - std::bind(&MotionVelocityPlannerNode::on_virtual_traffic_light_states, this, _1), - create_subscription_options(this)); - sub_occupancy_grid_ = this->create_subscription( - "~/input/occupancy_grid", 1, std::bind(&MotionVelocityPlannerNode::on_occupancy_grid, this, _1), - create_subscription_options(this)); srv_load_plugin_ = create_service( "~/service/load_plugin", std::bind(&MotionVelocityPlannerNode::on_load_plugin, this, _1, _2)); @@ -151,45 +123,59 @@ void MotionVelocityPlannerNode::on_unload_plugin( } // NOTE: argument planner_data must not be referenced for multithreading -bool MotionVelocityPlannerNode::is_data_ready() const +bool MotionVelocityPlannerNode::update_planner_data() { - const auto & d = planner_data_; auto clock = *get_clock(); - const auto check_with_msg = [&](const auto ptr, const auto & msg) { + auto is_ready = true; + const auto check_with_log = [&](const auto ptr, const auto & log) { constexpr auto throttle_duration = 3000; // [ms] if (!ptr) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, msg); + RCLCPP_INFO_THROTTLE(get_logger(), clock, throttle_duration, log); + is_ready = false; return false; } return true; }; - return check_with_msg(d.current_odometry, "Waiting for current odometry") && - check_with_msg(d.current_velocity, "Waiting for current velocity") && - check_with_msg(d.current_acceleration, "Waiting for current acceleration") && - check_with_msg(d.predicted_objects, "Waiting for predicted objects") && - check_with_msg(d.no_ground_pointcloud, "Waiting for pointcloud") && - check_with_msg(map_ptr_, "Waiting for the map") && - check_with_msg( - d.velocity_smoother_, "Waiting for the initialization of the velocity smoother") && - check_with_msg(d.occupancy_grid, "Waiting for the occupancy grid"); -} + const auto ego_state_ptr = sub_vehicle_odometry_.takeData(); + if (check_with_log(ego_state_ptr, "Waiting for current odometry")) + planner_data_.current_odometry = *ego_state_ptr; -void MotionVelocityPlannerNode::on_occupancy_grid( - const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - planner_data_.occupancy_grid = msg; -} + const auto ego_accel_ptr = sub_acceleration_.takeData(); + if (check_with_log(ego_accel_ptr, "Waiting for current acceleration")) + planner_data_.current_acceleration = *ego_accel_ptr; -void MotionVelocityPlannerNode::on_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - planner_data_.predicted_objects = msg; + 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; + + 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; + } + + const auto occupancy_grid_ptr = sub_occupancy_grid_.takeData(); + if (check_with_log(occupancy_grid_ptr, "Waiting for the occupancy grid")) + planner_data_.occupancy_grid = *occupancy_grid_ptr; + + // here we use bitwise operator to not short-circuit the logging messages + is_ready &= check_with_log(map_ptr_, "Waiting for the map"); + is_ready &= check_with_log( + planner_data_.velocity_smoother_, "Waiting for the initialization of the velocity smoother"); + + // 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; + + return is_ready; } -void MotionVelocityPlannerNode::on_no_ground_pointcloud( +std::optional> +MotionVelocityPlannerNode::process_no_ground_pointcloud( const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { geometry_msgs::msg::TransformStamped transform; @@ -198,7 +184,7 @@ void MotionVelocityPlannerNode::on_no_ground_pointcloud( "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); } catch (tf2::TransformException & e) { RCLCPP_WARN(get_logger(), "no transform found for no_ground_pointcloud: %s", e.what()); - return; + return {}; } pcl::PointCloud pc; @@ -206,42 +192,14 @@ void MotionVelocityPlannerNode::on_no_ground_pointcloud( Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); - if (!pc.empty()) { - tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); - } - - { - std::lock_guard lock(mutex_); - planner_data_.no_ground_pointcloud = pc_transformed; - } -} - -void MotionVelocityPlannerNode::on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - - auto current_odometry = std::make_shared(); - current_odometry->header = msg->header; - current_odometry->pose = msg->pose.pose; - planner_data_.current_odometry = current_odometry; - - auto current_velocity = std::make_shared(); - current_velocity->header = msg->header; - current_velocity->twist = msg->twist.twist; - planner_data_.current_velocity = current_velocity; -} - -void MotionVelocityPlannerNode::on_acceleration( - const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - planner_data_.current_acceleration = msg; + if (!pc.empty()) tier4_autoware_utils::transformPointCloud(pc, *pc_transformed, affine); + return *pc_transformed; } void MotionVelocityPlannerNode::set_velocity_smoother_params() { planner_data_.velocity_smoother_ = - std::make_shared(*this); + std::make_shared(*this); } void MotionVelocityPlannerNode::on_lanelet_map( @@ -253,11 +211,9 @@ void MotionVelocityPlannerNode::on_lanelet_map( has_received_map_ = true; } -void MotionVelocityPlannerNode::on_traffic_signals( +void MotionVelocityPlannerNode::process_traffic_signals( const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { - std::lock_guard lock(mutex_); - // clear previous observation planner_data_.traffic_light_id_map_raw_.clear(); const auto traffic_light_id_map_last_observed_old = @@ -290,19 +246,12 @@ void MotionVelocityPlannerNode::on_traffic_signals( } } -void MotionVelocityPlannerNode::on_virtual_traffic_light_states( - const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg) -{ - std::lock_guard lock(mutex_); - planner_data_.virtual_traffic_light_states = msg; -} - void MotionVelocityPlannerNode::on_trajectory( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) { std::unique_lock lk(mutex_); - if (!is_data_ready()) { + if (!update_planner_data()) { return; } @@ -367,9 +316,9 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, const autoware::motion_velocity_planner::PlannerData & planner_data) const { - const geometry_msgs::msg::Pose current_pose = planner_data.current_odometry->pose; - const double v0 = planner_data.current_velocity->twist.linear.x; - const double a0 = planner_data.current_acceleration->accel.accel.linear.x; + const 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_; @@ -399,7 +348,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; 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/src/node.hpp index 4b943dcbb67f2..799f80db25ad2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -22,10 +22,12 @@ #include #include #include +#include #include #include #include +#include #include #include #include @@ -60,31 +62,33 @@ class MotionVelocityPlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Subscription::SharedPtr - sub_predicted_objects_; - rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; - rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; - rclcpp::Subscription::SharedPtr sub_acceleration_; + tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_perception_msgs::msg::PredictedObjects> + sub_predicted_objects_{this, "~/input/dynamic_objects"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_no_ground_pointcloud_{this, "~/input/no_ground_pointcloud"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_vehicle_odometry_{this, "~/input/vehicle_odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber< + geometry_msgs::msg::AccelWithCovarianceStamped> + sub_acceleration_{this, "~/input/accel"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_occupancy_grid_{this, "~/input/occupancy_grid"}; + tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_perception_msgs::msg::TrafficLightGroupArray> + sub_traffic_signals_{this, "~/input/traffic_signals"}; + tier4_autoware_utils::InterProcessPollingSubscriber< + tier4_v2x_msgs::msg::VirtualTrafficLightStateArray> + sub_virtual_traffic_light_states_{this, "~/input/virtual_traffic_light_states"}; rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr - sub_traffic_signals_; - rclcpp::Subscription::SharedPtr - sub_virtual_traffic_light_states_; - rclcpp::Subscription::SharedPtr sub_occupancy_grid_; void on_trajectory( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); - void on_predicted_objects( - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); - void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); + std::optional> process_no_ground_pointcloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void on_lanelet_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); - void on_traffic_signals( + void process_traffic_signals( const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); - void on_virtual_traffic_light_states( - const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); - void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); // publishers rclcpp::Publisher::SharedPtr trajectory_pub_; @@ -118,7 +122,9 @@ class MotionVelocityPlannerNode : public rclcpp::Node std::mutex mutex_; // function - bool is_data_ready() const; + /// @brief update the PlannerData instance with the latest messages received + /// @return false if some data is not available + bool update_planner_data(); void insert_stop( autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const; diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 2ccd000657948..8b338e9d0cd9a 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -88,7 +88,7 @@ stop: max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint - max_lat_margin_against_unknown: -0.3 # lateral margin between the unknown obstacles and ego's footprint + max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint crossing_obstacle: collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] @@ -218,3 +218,15 @@ lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start + stop: + type_specified_params: + labels: # For the listed types, the node try to read the following type specified values + - "default" + - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc + unknown: + limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". + abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 052e7bb721592..34ae20980d3fd 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -113,14 +113,15 @@ struct StopObstacle : public TargetObstacleInterface { StopObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape, - const double arg_lon_velocity, const double arg_lat_velocity, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const Shape & arg_shape, const double arg_lon_velocity, const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point, const double arg_dist_to_collide_on_decimated_traj) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), shape(arg_shape), collision_point(arg_collision_point), - dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj) + dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj), + classification(object_classification) { } Shape shape; @@ -129,6 +130,7 @@ struct StopObstacle : public TargetObstacleInterface // calculateMarginFromObstacleOnCurve() and should be removed as it can be // replaced by ”dist_to_collide_on_decimated_traj” double dist_to_collide_on_decimated_traj; + ObjectClassification classification; }; struct CruiseObstacle : public TargetObstacleInterface diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index fd65446408db1..8f9724d9c8bed 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -53,7 +53,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // main functions std::vector createOneStepPolygons( const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; std::vector convertToObstacles( const Odometry & odometry, const PredictedObjects & objects, @@ -272,8 +272,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool is_driving_forward_{true}; bool enable_slow_down_planning_{false}; - // previous closest obstacle - std::shared_ptr prev_closest_stop_obstacle_ptr_{nullptr}; + std::vector prev_closest_stop_obstacles_{}; std::unique_ptr logger_configure_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 44626206ec038..a832e74c76db0 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -15,11 +15,11 @@ #ifndef OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ #define OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" #include "obstacle_cruise_planner/type_alias.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -35,8 +35,8 @@ class OptimizationBasedPlanner : public PlannerInterface public: OptimizationBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr); std::vector generateCruiseTrajectory( const PlannerData & planner_data, const std::vector & stop_traj_points, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index 26fd3a8ce2ce2..9d688cdac419b 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -49,8 +49,8 @@ class PIDBasedPlanner : public PlannerInterface PIDBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr); std::vector generateCruiseTrajectory( const PlannerData & planner_data, const std::vector & stop_traj_points, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 416c36b116059..f3dadbf5e5e51 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -23,6 +23,8 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include +#include #include #include #include @@ -36,13 +38,14 @@ class PlannerInterface public: PlannerInterface( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) : 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)) + slow_down_param_(SlowDownParam(node)), + stop_param_(StopParam(node, longitudinal_info)) { stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factors_pub_ = @@ -91,6 +94,7 @@ class PlannerInterface updateCommonParam(parameters); updateCruiseParam(parameters); slow_down_param_.onParam(parameters); + stop_param_.onParam(parameters, longitudinal_info_); } Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const @@ -131,7 +135,7 @@ class PlannerInterface rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; // Vehicle Parameters - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; EgoNearestParam ego_nearest_param_; @@ -333,6 +337,84 @@ class PlannerInterface double lpf_gain_dist_to_slow_down; }; SlowDownParam slow_down_param_; + struct StopParam + { + struct ObstacleSpecificParams + { + double limit_min_acc; + double sudden_object_acc_threshold; + double sudden_object_dist_threshold; + bool abandon_to_stop; + }; + const std::unordered_map types_maps = { + {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + std::unordered_map type_specified_param_list; + explicit StopParam(rclcpp::Node & node, const LongitudinalInfo & longitudinal_info) + { + const std::string param_prefix = "stop.type_specified_params."; + std::vector obstacle_labels{"default"}; + obstacle_labels = + node.declare_parameter>(param_prefix + "labels", obstacle_labels); + + for (const auto & type_str : obstacle_labels) { + if (type_str != "default") { + ObstacleSpecificParams param{ + node.declare_parameter(param_prefix + type_str + ".limit_min_acc"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_acc_threshold"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_dist_threshold"), + node.declare_parameter(param_prefix + type_str + ".abandon_to_stop")}; + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + + type_specified_param_list.emplace(type_str, param); + } + } + } + void onParam( + const std::vector & parameters, const LongitudinalInfo & longitudinal_info) + { + const std::string param_prefix = "stop.type_specified_params."; + for (auto & [type_str, param] : type_specified_param_list) { + if (type_str == "default") { + continue; + } + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".sudden_object_acc_threshold", + param.sudden_object_acc_threshold); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".sudden_object_dist_threshold", + param.sudden_object_dist_threshold); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + } + } + std::string getParamType(const ObjectClassification label) + { + const auto type_str = types_maps.at(label.label); + if (type_specified_param_list.count(type_str) == 0) { + return "default"; + } + return type_str; + } + ObstacleSpecificParams getParam(const ObjectClassification label) + { + return type_specified_param_list.at(getParamType(label)); + } + }; + StopParam stop_param_; double moving_object_speed_threshold; double moving_object_hysteresis_range; std::vector prev_slow_down_output_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 71d0f84637abc..455d40a7a7e87 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -15,10 +15,10 @@ #ifndef OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ #define OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -36,12 +36,12 @@ using tier4_autoware_utils::Polygon2d; Polygon2d createOneStepPolygon( const std::vector & last_poses, const std::vector & current_poses, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lat_margin); std::optional> getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward, - const vehicle_info_util::VehicleInfo & vehicle_info); + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); std::vector getCollisionPoints( const std::vector & traj_points, const std::vector & traj_polygons, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index 979cef8610cbd..196526c22aac9 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -15,7 +15,7 @@ #ifndef OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ #define OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ -#include "vehicle_info_util/vehicle_info_util.hpp" +#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" @@ -35,6 +35,7 @@ #include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" #include "visualization_msgs/msg/marker_array.hpp" +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; @@ -56,7 +57,6 @@ using tier4_planning_msgs::msg::StopReasonArray; using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; namespace bg = boost::geometry; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 3b4ab577c2988..48bab514cbf77 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -36,8 +36,7 @@ PoseWithStamp getCurrentObjectPose( const PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time, const bool use_prediction); -std::optional getClosestStopObstacle( - const std::vector & stop_obstacles); +std::vector getClosestStopObstacles(const std::vector & stop_obstacles); template size_t getIndexWithLongitudinalOffset( diff --git a/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml index a6c95d65acc6a..8db1ae8b46223 100644 --- a/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml +++ b/planning/obstacle_cruise_planner/launch/obstacle_cruise_planner.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index e394add65b05d..2a6e240093b14 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -21,6 +21,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils geometry_msgs interpolation lanelet2_extension @@ -37,7 +38,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index c5f0c73a13b78..866090a3c4696 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -353,7 +353,7 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_cruise_planner", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), debug_data_ptr_(std::make_shared()) { using std::placeholders::_1; @@ -558,7 +558,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms std::vector ObstacleCruisePlannerNode::createOneStepPolygons( const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const { const auto & p = behavior_determination_param_; @@ -1253,9 +1253,9 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{ - obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, - tangent_vel, normal_vel, collision_point->first, collision_point->second}; + return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, + obstacle.pose, obstacle.shape, tangent_vel, + normal_vel, collision_point->first, collision_point->second}; } std::optional ObstacleCruisePlannerNode::createSlowDownObstacle( @@ -1386,50 +1386,36 @@ void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) { - const auto current_closest_stop_obstacle = - obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - - if (!prev_closest_stop_obstacle_ptr_) { - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { + const auto predicted_object_itr = std::find_if( + predicted_objects.objects.begin(), predicted_objects.objects.end(), + [&prev_closest_stop_obstacle](const PredictedObject & po) { + return tier4_autoware_utils::toHexString(po.object_id) == prev_closest_stop_obstacle.uuid; + }); + // If previous closest obstacle disappear from the perception result, do nothing anymore. + if (predicted_object_itr == predicted_objects.objects.end()) { + continue; } - return; - } - const auto predicted_object_itr = std::find_if( - predicted_objects.objects.begin(), predicted_objects.objects.end(), - [&](PredictedObject predicted_object) { - return tier4_autoware_utils::toHexString(predicted_object.object_id) == - prev_closest_stop_obstacle_ptr_->uuid; - }); - // If previous closest obstacle disappear from the perception result, do nothing anymore. - if (predicted_object_itr == predicted_objects.objects.end()) { - return; - } - - const auto is_disappeared_from_stop_obstacle = std::none_of( - stop_obstacles.begin(), stop_obstacles.end(), - [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; }); - if (is_disappeared_from_stop_obstacle) { - // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" - // condition is satisfied - const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); - if ( - predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < - behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && - elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { - stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); - return; + const auto is_disappeared_from_stop_obstacle = std::none_of( + stop_obstacles.begin(), stop_obstacles.end(), + [&prev_closest_stop_obstacle](const StopObstacle & so) { + return so.uuid == prev_closest_stop_obstacle.uuid; + }); + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied + const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); + if ( + predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < + behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && + elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { + stop_obstacles.push_back(prev_closest_stop_obstacle); + } } } - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - prev_closest_stop_obstacle_ptr_ = nullptr; - } + prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); } bool ObstacleCruisePlannerNode::isObstacleCrossing( diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 1b5120eba0a11..a040ac598681f 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -38,8 +38,8 @@ constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3; OptimizationBasedPlanner::OptimizationBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) : PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr) { smoothed_traj_sub_ = node.create_subscription( diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index a5afbe74cc9c4..3084764432c0e 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -58,8 +58,8 @@ T getSign(const T val) PIDBasedPlanner::PIDBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, - const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, - const std::shared_ptr debug_data_ptr) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) : PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr) { min_accel_during_cruise_ = diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index f7ce218cf3ccf..6e1de97378d16 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -259,129 +259,171 @@ std::vector PlannerInterface::generateStopTrajectory( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_, "stop planning"); - // Get Closest Stop Obstacle - const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - if (!closest_stop_obstacle) { - // delete marker - const auto markers = - motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); - - prev_stop_distance_info_ = std::nullopt; - return planner_data.traj_points; - } - - const auto ego_segment_idx = - ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); - const double dist_to_collide_on_ref_traj = - motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + - closest_stop_obstacle->dist_to_collide_on_decimated_traj; - - const double margin_from_obstacle_considering_behavior_module = [&]() { - const double margin_from_obstacle = - calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); - // Use terminal margin (terminal_safe_distance_margin) for obstacle stop - const auto ref_traj_length = motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, planner_data.traj_points.size() - 1); - if (dist_to_collide_on_ref_traj > ref_traj_length) { - return longitudinal_info_.terminal_safe_distance_margin; - } - - // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin - // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const auto closest_behavior_stop_idx = - motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); - if (closest_behavior_stop_idx) { - const double closest_behavior_stop_dist_on_ref_traj = - motion_utils::calcSignedArcLength(planner_data.traj_points, 0, *closest_behavior_stop_idx); - const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - - (dist_to_collide_on_ref_traj - margin_from_obstacle); - if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { - return min_behavior_stop_margin_; + std::optional determined_stop_obstacle{}; + std::optional determined_zero_vel_dist{}; + std::optional determined_desired_margin{}; + + const auto closest_stop_obstacles = + obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); + for (const auto & stop_obstacle : closest_stop_obstacles) { + const auto ego_segment_idx = + ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); + const double dist_to_collide_on_ref_traj = + motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + + stop_obstacle.dist_to_collide_on_decimated_traj; + + const double desired_margin = [&]() { + const double margin_from_obstacle = + calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle); + // Use terminal margin (terminal_safe_distance_margin) for obstacle stop + const auto ref_traj_length = motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.traj_points.size() - 1); + if (dist_to_collide_on_ref_traj > ref_traj_length) { + return longitudinal_info_.terminal_safe_distance_margin; } - } - return margin_from_obstacle; - }(); - // Generate Output Trajectory - const auto [zero_vel_dist, will_collide_with_obstacle] = [&]() { - double candidate_zero_vel_dist = - std::max(0.0, dist_to_collide_on_ref_traj - margin_from_obstacle_considering_behavior_module); + // If behavior stop point is ahead of the closest_obstacle_stop point within a certain + // margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance + const auto closest_behavior_stop_idx = + motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); + if (closest_behavior_stop_idx) { + const double closest_behavior_stop_dist_on_ref_traj = motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, *closest_behavior_stop_idx); + const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - + (dist_to_collide_on_ref_traj - margin_from_obstacle); + if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { + return min_behavior_stop_margin_; + } + } + return margin_from_obstacle; + }(); - // Check feasibility to stop + // calc stop point against the obstacle + double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin); if (suppress_sudden_obstacle_stop_) { - // Calculate feasible stop margin (Check the feasibility) - const double feasible_stop_dist = - calcMinimumDistanceToStop( - planner_data.ego_vel, longitudinal_info_.limit_max_accel, - longitudinal_info_.limit_min_accel) + - motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, planner_data.ego_pose.position); + const auto acceptable_stop_acc = [&]() -> std::optional { + if (stop_param_.getParamType(stop_obstacle.classification) == "default") { + return longitudinal_info_.limit_min_accel; + } + const double distance_to_judge_suddenness = std::min( + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, + stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold), + stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold); + if (candidate_zero_vel_dist > distance_to_judge_suddenness) { + return longitudinal_info_.limit_min_accel; + } + if (stop_param_.getParam(stop_obstacle.classification).abandon_to_stop) { + RCLCPP_WARN( + rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"), + "[Cruise] abandon to stop against %s object", + stop_param_.types_maps.at(stop_obstacle.classification.label).c_str()); + return std::nullopt; + } else { + return stop_param_.getParam(stop_obstacle.classification).limit_min_acc; + } + }(); + if (!acceptable_stop_acc) { + continue; + } - if (candidate_zero_vel_dist < feasible_stop_dist) { - candidate_zero_vel_dist = feasible_stop_dist; - return std::make_pair(candidate_zero_vel_dist, true); + const double acceptable_stop_pos = + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value()); + if (acceptable_stop_pos > candidate_zero_vel_dist) { + candidate_zero_vel_dist = acceptable_stop_pos; } } - // Hold previous stop distance if necessary - if ( - std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && - prev_stop_distance_info_) { - // NOTE: We assume that the current trajectory's front point is ahead of the previous - // trajectory's front point. - const size_t traj_front_point_prev_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_stop_distance_info_->first, planner_data.traj_points.front().pose); - const double diff_dist_front_points = motion_utils::calcSignedArcLength( - prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, - traj_front_point_prev_seg_idx); - - const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if (determined_stop_obstacle) { + const bool is_same_param_types = + (stop_obstacle.classification.label == determined_stop_obstacle->classification.label); if ( - std::abs(prev_zero_vel_dist - candidate_zero_vel_dist) < - longitudinal_info_.hold_stop_distance_threshold) { - candidate_zero_vel_dist = prev_zero_vel_dist; + (is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj > + determined_stop_obstacle->dist_to_collide_on_decimated_traj) || + (!is_same_param_types && candidate_zero_vel_dist > determined_zero_vel_dist)) { + continue; } } - return std::make_pair(candidate_zero_vel_dist, false); - }(); + determined_zero_vel_dist = candidate_zero_vel_dist; + determined_stop_obstacle = stop_obstacle; + determined_desired_margin = desired_margin; + } + + if (!determined_zero_vel_dist) { + // delete marker + const auto markers = + motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return planner_data.traj_points; + } + + // Hold previous stop distance if necessary + if ( + std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, planner_data.traj_points.front().pose); + const double diff_dist_front_points = motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) < + longitudinal_info_.hold_stop_distance_threshold) { + determined_zero_vel_dist.value() = prev_zero_vel_dist; + } + } // Insert stop point auto output_traj_points = planner_data.traj_points; - const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); + const auto zero_vel_idx = + motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); - debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle); + debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; const auto stop_reasons_msg = - makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle); + makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); - // Publish if ego vehicle collides with the obstacle with a limit acceleration + // Publish if ego vehicle will over run against the stop point with a limit acceleration + + const bool will_over_run = determined_zero_vel_dist.value() > + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + determined_stop_obstacle->dist_to_collide_on_decimated_traj + + determined_desired_margin.value() + 0.1; const auto stop_speed_exceeded_msg = - createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); + createStopSpeedExceededMsg(planner_data.current_time, will_over_run); stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); - prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist); + prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value()); } stop_planning_debug_info_.set( StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, - closest_stop_obstacle->dist_to_collide_on_decimated_traj); + determined_stop_obstacle->dist_to_collide_on_decimated_traj); stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); - + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, + determined_stop_obstacle->velocity); stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, - margin_from_obstacle_considering_behavior_module); + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, determined_desired_margin.value()); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 1bd2de0bd985c..f354c53365dc1 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -95,7 +95,7 @@ namespace polygon_utils std::optional> getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward, - const vehicle_info_util::VehicleInfo & vehicle_info) + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) { const auto collision_info = getCollisionIndex(traj_points, traj_polygons, obstacle.pose, obstacle.stamp, obstacle.shape); diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 6b32ccafc7f42..6c1b3999e40c1 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -95,16 +95,21 @@ PoseWithStamp getCurrentObjectPose( return PoseWithStamp{obj_base_time, *interpolated_pose}; } -std::optional getClosestStopObstacle(const std::vector & stop_obstacles) +std::vector getClosestStopObstacles(const std::vector & stop_obstacles) { - std::optional candidate_obstacle = std::nullopt; + std::vector candidates{}; for (const auto & stop_obstacle : stop_obstacles) { - if ( - !candidate_obstacle || stop_obstacle.dist_to_collide_on_decimated_traj < - candidate_obstacle->dist_to_collide_on_decimated_traj) { - candidate_obstacle = stop_obstacle; + const auto itr = + std::find_if(candidates.begin(), candidates.end(), [&stop_obstacle](const StopObstacle & co) { + return co.classification.label == stop_obstacle.classification.label; + }); + if (itr == candidates.end()) { + candidates.emplace_back(stop_obstacle); + } else if ( + stop_obstacle.dist_to_collide_on_decimated_traj < itr->dist_to_collide_on_decimated_traj) { + *itr = stop_obstacle; } } - return candidate_obstacle; + return candidates; } } // namespace obstacle_cruise_utils diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 723d7e8d12e56..d7fc83dbdf7ed 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -21,13 +21,13 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include #include #include #include #include #include #include -#include #include #include @@ -77,6 +77,7 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -90,7 +91,6 @@ using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index b43e51873c09e..7b30ac61af46f 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -18,8 +18,8 @@ #include "obstacle_stop_planner/planner_data.hpp" +#include #include -#include #include #include @@ -45,10 +45,10 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using std_msgs::msg::Header; +using autoware::vehicle_info_utils::VehicleInfo; using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; -using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; diff --git a/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml b/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml index bec3aef6b4558..db38370d36dfb 100644 --- a/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml +++ b/planning/obstacle_stop_planner/launch/obstacle_stop_planner.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index fcf09b96c1345..39195f2840b81 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -24,6 +24,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils diagnostic_msgs motion_utils nav_msgs @@ -41,7 +42,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 31c044c93af22..3a269d738e02d 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -56,7 +56,7 @@ ObstacleStopPlannerNode::ObstacleStopPlannerNode(const rclcpp::NodeOptions & nod : Node("obstacle_stop_planner", node_options) { // Vehicle Parameters - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); const auto & i = vehicle_info_; diff --git a/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml b/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml index 4781ceda528bf..cb6cab3b125e2 100644 --- a/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml +++ b/planning/obstacle_velocity_limiter/launch/obstacle_velocity_limiter.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml index 49e424bfbc9ef..ad83f397902fc 100644 --- a/planning/obstacle_velocity_limiter/package.xml +++ b/planning/obstacle_velocity_limiter/package.xml @@ -14,6 +14,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils eigen grid_map_msgs grid_map_ros @@ -31,7 +32,6 @@ tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 18cf534da8c7e..4b28d9a62759b 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -22,12 +22,12 @@ #include "obstacle_velocity_limiter/trajectory_preprocessing.hpp" #include "obstacle_velocity_limiter/types.hpp" +#include #include #include #include #include #include -#include #include @@ -71,7 +71,7 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio pub_runtime_ = create_publisher("~/output/runtime_microseconds", 1); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); vehicle_lateral_offset_ = static_cast(vehicle_info.max_lateral_offset_m); vehicle_front_offset_ = static_cast(vehicle_info.max_longitudinal_offset_m); diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt index 455dded2d7e32..da780cfdebdcf 100644 --- a/planning/planning_validator/CMakeLists.txt +++ b/planning/planning_validator/CMakeLists.txt @@ -1,22 +1,22 @@ cmake_minimum_required(VERSION 3.22) -project(planning_validator) +project(autoware_planning_validator) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(planning_validator_helpers SHARED +ament_auto_add_library(autoware_planning_validator_helpers SHARED src/utils.cpp src/debug_marker.cpp ) # planning validator -ament_auto_add_library(planning_validator_component SHARED - include/planning_validator/planning_validator.hpp +ament_auto_add_library(autoware_planning_validator_component SHARED + include/autoware_planning_validator/planning_validator.hpp src/planning_validator.cpp ) -target_link_libraries(planning_validator_component planning_validator_helpers) -rclcpp_components_register_node(planning_validator_component - PLUGIN "planning_validator::PlanningValidator" +target_link_libraries(autoware_planning_validator_component autoware_planning_validator_helpers) +rclcpp_components_register_node(autoware_planning_validator_component + PLUGIN "autoware::planning_validator::PlanningValidator" EXECUTABLE planning_validator_node ) @@ -25,7 +25,7 @@ ament_auto_add_library(invalid_trajectory_publisher_node SHARED src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp ) rclcpp_components_register_node(invalid_trajectory_publisher_node - PLUGIN "planning_validator::InvalidTrajectoryPublisherNode" + PLUGIN "autoware::planning_validator::InvalidTrajectoryPublisherNode" EXECUTABLE invalid_trajectory_publisher ) @@ -37,29 +37,29 @@ rosidl_generate_interfaces( # to use a message defined in the same package if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(planning_validator_component + rosidl_target_interfaces(autoware_planning_validator_component ${PROJECT_NAME} "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(planning_validator_component "${cpp_typesupport_target}") + target_link_libraries(autoware_planning_validator_component "${cpp_typesupport_target}") endif() if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_planning_validator + ament_add_ros_isolated_gtest(test_autoware_planning_validator test/src/test_main.cpp test/src/test_planning_validator_functions.cpp test/src/test_planning_validator_helper.cpp test/src/test_planning_validator_pubsub.cpp - test/src/test_${PROJECT_NAME}_node_interface.cpp + test/src/test_planning_validator_node_interface.cpp ) - ament_target_dependencies(test_planning_validator + ament_target_dependencies(test_autoware_planning_validator rclcpp autoware_planning_msgs ) - target_link_libraries(test_planning_validator - planning_validator_component + target_link_libraries(test_autoware_planning_validator + autoware_planning_validator_component ) endif() diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index 9d70e7f78a5bb..92c9b86c8b31c 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -1,8 +1,8 @@ # Planning Validator -The `planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory". +The `autoware_planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `autoware_planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory". -![planning_validator](./image/planning_validator.drawio.svg) +![autoware_planning_validator](./image/planning_validator.drawio.svg) ## Supported features @@ -29,7 +29,7 @@ The following features are to be implemented. ### Inputs -The `planning_validator` takes in the following inputs: +The `autoware_planning_validator` takes in the following inputs: | Name | Type | Description | | -------------------- | --------------------------------- | ---------------------------------------------- | @@ -48,7 +48,7 @@ It outputs the following: ## Parameters -The following parameters can be set for the `planning_validator`: +The following parameters can be set for the `autoware_planning_validator`: ### System parameters diff --git a/planning/planning_validator/config/planning_validator_plotjugler_config.xml b/planning/planning_validator/config/planning_validator_plotjugler_config.xml index d6afc0b017d32..04d5d4d8a0e38 100644 --- a/planning/planning_validator/config/planning_validator_plotjugler_config.xml +++ b/planning/planning_validator/config/planning_validator_plotjugler_config.xml @@ -9,21 +9,21 @@ - + - + - + @@ -32,21 +32,21 @@ - + - + - + @@ -55,21 +55,21 @@ - + - + - + @@ -78,7 +78,7 @@ - + @@ -86,23 +86,23 @@ - - - - - + + + + + - - - - - - + + + + + + diff --git a/planning/planning_validator/image/planning_validator.drawio.svg b/planning/planning_validator/image/planning_validator.drawio.svg index 070c1a6d68b81..0a2267cba159e 100644 --- a/planning/planning_validator/image/planning_validator.drawio.svg +++ b/planning/planning_validator/image/planning_validator.drawio.svg @@ -38,12 +38,12 @@ >
- planning_validator + autoware_planning_validator
- planning_validator + autoware_planning_validator diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp similarity index 91% rename from planning/planning_validator/include/planning_validator/debug_marker.hpp rename to planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp index 4671a0b0d7297..955ba5bbf64ab 100644 --- a/planning/planning_validator/include/planning_validator/debug_marker.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ -#define PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ #include @@ -57,4 +57,4 @@ class PlanningValidatorDebugMarkerPublisher } }; -#endif // PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp similarity index 86% rename from planning/planning_validator/include/planning_validator/planning_validator.hpp rename to planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp index fed6e161c4d90..b2a9c8f2e94a4 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#define PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#include "planning_validator/debug_marker.hpp" -#include "planning_validator/msg/planning_validator_status.hpp" +#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/msg/planning_validator_status.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -33,14 +34,14 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { 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 planning_validator::msg::PlanningValidatorStatus; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; @@ -102,7 +103,8 @@ class PlanningValidator : public rclcpp::Node void setStatus(DiagnosticStatusWrapper & stat, const bool & is_ok, const std::string & msg); - rclcpp::Subscription::SharedPtr sub_kinematics_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; rclcpp::Publisher::SharedPtr pub_status_; @@ -124,7 +126,7 @@ class PlanningValidator : public rclcpp::Node PlanningValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds - vehicle_info_util::VehicleInfo vehicle_info_; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; bool isAllValid(const PlanningValidatorStatus & status) const; @@ -141,6 +143,6 @@ class PlanningValidator : public rclcpp::Node StopWatch stop_watch_; }; -} // namespace planning_validator +} // namespace autoware::planning_validator -#endif // PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/autoware_planning_validator/utils.hpp similarity index 90% rename from planning/planning_validator/include/planning_validator/utils.hpp rename to planning/planning_validator/include/autoware_planning_validator/utils.hpp index 11eaf4f37f400..1c943f5a60e08 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__UTILS_HPP_ -#define PLANNING_VALIDATOR__UTILS_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -65,6 +65,6 @@ bool checkFinite(const TrajectoryPoint & point); void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal); -} // namespace planning_validator +} // namespace autoware::planning_validator -#endif // PLANNING_VALIDATOR__UTILS_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ diff --git a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml index 3f73e0f462cc2..a1cdc7c7ecc55 100644 --- a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml +++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml index 81573adb4dc24..aeb9c37d67cda 100644 --- a/planning/planning_validator/launch/planning_validator.launch.xml +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 5a413f27dc7a3..541396b52b02b 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -1,9 +1,9 @@ - planning_validator + autoware_planning_validator 0.1.0 - ros node for planning_validator + ros node for autoware_planning_validator Takamasa Horibe Kosuke Takeuchi Apache License 2.0 @@ -17,6 +17,7 @@ autoware_planning_msgs autoware_planning_test_manager + autoware_vehicle_info_utils diagnostic_updater geometry_msgs motion_utils @@ -24,7 +25,6 @@ rclcpp rclcpp_components tier4_autoware_utils - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 82117d199c36e..413611a72b5ab 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/debug_marker.hpp" #include #include @@ -90,7 +90,7 @@ void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs: { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "planning_validator", now, 0); + motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp index 6cca99cd9f14c..645d6c92e3698 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp @@ -18,7 +18,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { InvalidTrajectoryPublisherNode::InvalidTrajectoryPublisherNode( const rclcpp::NodeOptions & node_options) @@ -72,7 +72,7 @@ void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::Const traj_sub_.reset(); } -} // namespace planning_validator +} // namespace autoware::planning_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::InvalidTrajectoryPublisherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_validator::InvalidTrajectoryPublisherNode) diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp index 1d1e47ad10407..0025d759217bf 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp @@ -21,7 +21,7 @@ #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -41,6 +41,6 @@ class InvalidTrajectoryPublisherNode : public rclcpp::Node Trajectory::ConstSharedPtr current_trajectory_ = nullptr; }; -} // namespace planning_validator +} // namespace autoware::planning_validator #endif // INVALID_TRAJECTORY_PUBLISHER__INVALID_TRAJECTORY_PUBLISHER_HPP_ diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index d14b1d9ff37d2..9f54547eb62e1 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" -#include "planning_validator/utils.hpp" +#include "autoware_planning_validator/utils.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using diagnostic_msgs::msg::DiagnosticStatus; @@ -32,9 +32,6 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) { using std::placeholders::_1; - sub_kinematics_ = create_subscription( - "~/input/kinematics", 1, - [this](const Odometry::ConstSharedPtr msg) { current_kinematics_ = msg; }); sub_traj_ = create_subscription( "~/input/trajectory", 1, std::bind(&PlanningValidator::onTrajectory, this, _1)); @@ -92,7 +89,7 @@ void PlanningValidator::setupParameters() } try { - vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); } catch (...) { RCLCPP_ERROR(get_logger(), "failed to get vehicle info. use default value."); vehicle_info_.front_overhang_m = 0.5; @@ -195,6 +192,9 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) current_trajectory_ = msg; + // receive data + current_kinematics_ = sub_kinematics_.takeData(); + if (!isDataReady()) return; if (publish_diag_ && !diag_updater_) { @@ -584,7 +584,7 @@ void PlanningValidator::displayStatus() warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!"); } -} // namespace planning_validator +} // namespace autoware::planning_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::PlanningValidator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_validator::PlanningValidator) diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index aada0bea2da9b..4710d3ae84ea3 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/utils.hpp" +#include "autoware_planning_validator/utils.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using tier4_autoware_utils::calcCurvature; using tier4_autoware_utils::calcDistance2d; @@ -324,4 +324,4 @@ void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal) pose.position.y += std::sin(yaw) * longitudinal; } -} // namespace planning_validator +} // namespace autoware::planning_validator diff --git a/planning/planning_validator/test/src/test_planning_validator_functions.cpp b/planning/planning_validator/test/src/test_planning_validator_functions.cpp index e406b6c5d7da2..41ae41a80bdc5 100644 --- a/planning/planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/debug_marker.hpp" -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" @@ -21,8 +21,8 @@ #include +using autoware::planning_validator::PlanningValidator; using autoware_planning_msgs::msg::Trajectory; -using planning_validator::PlanningValidator; TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) { diff --git a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp index 2b2a32bf54618..78f57e0fe104d 100644 --- a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include #include @@ -22,8 +22,8 @@ #include +using autoware::planning_validator::PlanningValidator; using planning_test_utils::PlanningInterfaceTestManager; -using planning_validator::PlanningValidator; std::shared_ptr generateTestManager() { @@ -46,7 +46,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto planning_validator_dir = - ament_index_cpp::get_package_share_directory("planning_validator"); + ament_index_cpp::get_package_share_directory("autoware_planning_validator"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index 2329a3b9bb6c8..8cf56525edbc5 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" @@ -29,11 +29,11 @@ * This test checks the diagnostics message published from the planning_validator node */ +using autoware::planning_validator::PlanningValidator; using autoware_planning_msgs::msg::Trajectory; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; -using planning_validator::PlanningValidator; constexpr double epsilon = 0.001; constexpr double scale_margin = 1.1; @@ -101,7 +101,8 @@ bool hasError(const std::vector & diags, const throw std::runtime_error(name + " is not contained in the diagnostic message."); } -std::pair, std::shared_ptr> +std::pair< + std::shared_ptr, std::shared_ptr> prepareTest(const Trajectory & trajectory, const Odometry & ego_odom) { auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 24a20f86ed718..d692337b10bd6 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -1,4 +1,4 @@ -// Copyright 2021-2023 Tier IV, Inc. +// Copyright 2021-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. @@ -51,6 +51,20 @@ enum class Direction { NONE, LEFT, RIGHT }; enum class PullOverDirection { NONE, LEFT, RIGHT }; enum class PullOutDirection { NONE, LEFT, RIGHT }; +struct ReferencePoint +{ + bool is_waypoint{false}; + geometry_msgs::msg::Point point; +}; +using PiecewiseReferencePoints = std::vector; + +struct PiecewiseWaypoints +{ + lanelet::Id lanelet_id; + std::vector piecewise_waypoints; +}; +using Waypoints = std::vector; + class RouteHandler { public: @@ -270,6 +284,13 @@ class RouteHandler PathWithLaneId getCenterLinePath( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact = true) const; + std::vector calcWaypointsVector(const lanelet::ConstLanelets & lanelet_sequence) const; + void removeOverlappedCenterlineWithWaypoints( + std::vector & piecewise_ref_points_vec, + const std::vector & piecewise_waypoints, + const lanelet::ConstLanelets & lanelet_sequence, + const size_t piecewise_waypoints_lanelet_sequence_index, + const bool is_removing_direction_forward) const; std::optional getLaneChangeTarget( const lanelet::ConstLanelets & lanelets, const Direction direction = Direction::NONE) const; std::optional getLaneChangeTargetExceptPreferredLane( diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index d410cb2668175..1fd0dd7747e49 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1,4 +1,4 @@ -// Copyright 2021-2023 Tier IV, Inc. +// Copyright 2021-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. @@ -38,9 +38,12 @@ #include #include #include +#include #include #include +namespace route_handler +{ namespace { using autoware_planning_msgs::msg::LaneletPrimitive; @@ -129,10 +132,49 @@ std::string toString(const geometry_msgs::msg::Pose & pose) return ss.str(); } -} // namespace +bool isClose( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const double epsilon) +{ + return std::abs(p1.x - p2.x) < epsilon && std::abs(p1.y - p2.y) < epsilon; +} -namespace route_handler +PiecewiseReferencePoints convertWaypointsToReferencePoints( + const std::vector & piecewise_waypoints) { + PiecewiseReferencePoints piecewise_ref_points; + for (const auto & piecewise_waypoint : piecewise_waypoints) { + piecewise_ref_points.push_back(ReferencePoint{true, piecewise_waypoint}); + } + return piecewise_ref_points; +} + +template +bool isIndexWithinVector(const std::vector & vec, const int index) +{ + return 0 <= index && index < static_cast(vec.size()); +} + +template +void removeIndicesFromVector(std::vector & vec, std::vector indices) +{ + // sort indices in a descending order + std::sort(indices.begin(), indices.end(), std::greater()); + + // remove indices from vector + for (const size_t index : indices) { + vec.erase(vec.begin() + index); + } +} + +lanelet::ArcCoordinates calcArcCoordinates( + const lanelet::ConstLanelet & lanelet, const geometry_msgs::msg::Point & point) +{ + return lanelet::geometry::toArcCoordinates( + to2D(lanelet.centerline()), + to2D(lanelet::utils::conversion::toLaneletPoint(point)).basicPoint()); +} +} // namespace + RouteHandler::RouteHandler(const LaneletMapBin & map_msg) { setMap(map_msg); @@ -1494,21 +1536,21 @@ PathWithLaneId RouteHandler::getCenterLinePath( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact) const { - PathWithLaneId reference_path{}; - double s = 0; + using lanelet::utils::to2D; + using lanelet::utils::conversion::toLaneletPoint; + // 1. calculate reference points by lanelets' centerline + // NOTE: This vector aligns the vector lanelet_sequence. + std::vector piecewise_ref_points_vec; + const auto add_ref_point = [&](const auto & pt) { + piecewise_ref_points_vec.back().push_back( + ReferencePoint{false, lanelet::utils::conversion::toGeomMsgPt(pt)}); + }; + double s = 0; for (const auto & llt : lanelet_sequence) { - const lanelet::traffic_rules::SpeedLimitInformation limit = traffic_rules_ptr_->speedLimit(llt); - const lanelet::ConstLineString3d centerline = llt.centerline(); - - const auto add_path_point = [&reference_path, &limit, &llt](const auto & pt) { - PathPointWithLaneId p{}; - p.point.pose.position = lanelet::utils::conversion::toGeomMsgPt(pt); - p.lane_ids.push_back(llt.id()); - p.point.longitudinal_velocity_mps = static_cast(limit.speedLimit.value()); - reference_path.points.push_back(p); - }; + piecewise_ref_points_vec.push_back(std::vector{}); + const lanelet::ConstLineString3d centerline = llt.centerline(); for (size_t i = 0; i < centerline.size(); i++) { const auto & pt = centerline[i]; const lanelet::ConstPoint3d next_pt = @@ -1517,19 +1559,102 @@ PathWithLaneId RouteHandler::getCenterLinePath( if (s < s_start && s + distance > s_start) { const auto p = use_exact ? get3DPointFrom2DArcLength(lanelet_sequence, s_start) : pt; - add_path_point(p); + add_ref_point(p); } if (s >= s_start && s <= s_end) { - add_path_point(pt); + add_ref_point(pt); } if (s < s_end && s + distance > s_end) { const auto p = use_exact ? get3DPointFrom2DArcLength(lanelet_sequence, s_end) : next_pt; - add_path_point(p); + add_ref_point(p); } s += distance; } } + // 2. calculate waypoints + const auto waypoints_vec = calcWaypointsVector(lanelet_sequence); + + // 3. remove points in the margin of the waypoint + for (const auto & waypoints : waypoints_vec) { + for (auto piecewise_waypoints_itr = waypoints.begin(); + piecewise_waypoints_itr != waypoints.end(); ++piecewise_waypoints_itr) { + const auto & piecewise_waypoints = piecewise_waypoints_itr->piecewise_waypoints; + const auto lanelet_id = piecewise_waypoints_itr->lanelet_id; + + // calculate index of lanelet_sequence which corresponds to piecewise_waypoints. + const auto lanelet_sequence_itr = std::find_if( + lanelet_sequence.begin(), lanelet_sequence.end(), + [&](const auto & lanelet) { return lanelet.id() == lanelet_id; }); + if (lanelet_sequence_itr == lanelet_sequence.end()) { + continue; + } + const size_t piecewise_waypoints_lanelet_sequence_index = + std::distance(lanelet_sequence.begin(), lanelet_sequence_itr); + + // calculate reference points by waypoints + const auto ref_points_by_waypoints = convertWaypointsToReferencePoints(piecewise_waypoints); + + // update reference points by waypoints + const bool is_first_waypoint_contained = piecewise_waypoints_itr == waypoints.begin(); + const bool is_last_waypoint_contained = piecewise_waypoints_itr == waypoints.end() - 1; + if (is_first_waypoint_contained || is_last_waypoint_contained) { + // If piecewise_waypoints_itr is the end (first or last) of piecewise_waypoints + + const auto original_piecewise_ref_points = + piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index); + + // define current_piecewise_ref_points, and initialize it with waypoints + auto & current_piecewise_ref_points = + piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index); + current_piecewise_ref_points = ref_points_by_waypoints; + if (is_first_waypoint_contained) { + // add original reference points to current reference points, and remove reference points + // overlapped with waypoints + current_piecewise_ref_points.insert( + current_piecewise_ref_points.begin(), original_piecewise_ref_points.begin(), + original_piecewise_ref_points.end()); + const bool is_removing_direction_forward = false; + removeOverlappedCenterlineWithWaypoints( + piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, + piecewise_waypoints_lanelet_sequence_index, is_removing_direction_forward); + } + if (is_last_waypoint_contained) { + // add original reference points to current reference points, and remove reference points + // overlapped with waypoints + current_piecewise_ref_points.insert( + current_piecewise_ref_points.end(), original_piecewise_ref_points.begin(), + original_piecewise_ref_points.end()); + const bool is_removing_direction_forward = true; + removeOverlappedCenterlineWithWaypoints( + piecewise_ref_points_vec, piecewise_waypoints, lanelet_sequence, + piecewise_waypoints_lanelet_sequence_index, is_removing_direction_forward); + } + } else { + // If piecewise_waypoints_itr is not the end (first or last) of piecewise_waypoints, + // remove all the reference points and add waypoints. + piecewise_ref_points_vec.at(piecewise_waypoints_lanelet_sequence_index) = + ref_points_by_waypoints; + } + } + } + + // 4. convert to PathPointsWithLaneIds + PathWithLaneId reference_path{}; + for (size_t lanelet_idx = 0; lanelet_idx < lanelet_sequence.size(); ++lanelet_idx) { + const auto & lanelet = lanelet_sequence.at(lanelet_idx); + const float speed_limit = + static_cast(traffic_rules_ptr_->speedLimit(lanelet).speedLimit.value()); + + const auto & piecewise_ref_points = piecewise_ref_points_vec.at(lanelet_idx); + for (const auto & ref_point : piecewise_ref_points) { + PathPointWithLaneId p{}; + p.point.pose.position = ref_point.point; + p.lane_ids.push_back(lanelet.id()); + p.point.longitudinal_velocity_mps = speed_limit; + reference_path.points.push_back(p); + } + } reference_path = removeOverlappingPoints(reference_path); // append a point only when having one point so that yaw calculation would work @@ -1565,6 +1690,133 @@ PathWithLaneId RouteHandler::getCenterLinePath( return reference_path; } +std::vector RouteHandler::calcWaypointsVector( + const lanelet::ConstLanelets & lanelet_sequence) const +{ + std::vector waypoints_vec; + for (size_t lanelet_idx = 0; lanelet_idx < lanelet_sequence.size(); ++lanelet_idx) { + const auto & lanelet = lanelet_sequence.at(lanelet_idx); + if (!lanelet.hasAttribute("waypoints")) { + continue; + } + + // generate piecewise waypoints + PiecewiseWaypoints piecewise_waypoints{lanelet.id(), {}}; + const auto waypoints_id = lanelet.attribute("waypoints").asId().value(); + for (const auto & waypoint : lanelet_map_ptr_->lineStringLayer.get(waypoints_id)) { + piecewise_waypoints.piecewise_waypoints.push_back( + lanelet::utils::conversion::toGeomMsgPt(waypoint)); + } + if (piecewise_waypoints.piecewise_waypoints.empty()) { + continue; + } + + // check if the piecewise waypoints are connected to the previous piecewise waypoints + if ( + !waypoints_vec.empty() && isClose( + waypoints_vec.back().back().piecewise_waypoints.back(), + piecewise_waypoints.piecewise_waypoints.front(), 1.0)) { + waypoints_vec.back().push_back(piecewise_waypoints); + } else { + // add new waypoints + Waypoints new_waypoints; + new_waypoints.push_back(piecewise_waypoints); + waypoints_vec.push_back(new_waypoints); + } + } + + return waypoints_vec; +} + +void RouteHandler::removeOverlappedCenterlineWithWaypoints( + std::vector & piecewise_ref_points_vec, + const std::vector & piecewise_waypoints, + const lanelet::ConstLanelets & lanelet_sequence, + const size_t piecewise_waypoints_lanelet_sequence_index, + const bool is_removing_direction_forward) const +{ + const double waypoints_interpolation_arc_margin_ratio = 10.0; + + // calculate arc length threshold + const double front_arc_length_threshold = [&]() { + const auto front_waypoint_arc_coordinates = calcArcCoordinates( + lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index), piecewise_waypoints.front()); + const double lanelet_arc_length = boost::geometry::length( + lanelet::utils::to2D(lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index) + .centerline() + .basicLineString())); + return -lanelet_arc_length + front_waypoint_arc_coordinates.length - + std::abs(front_waypoint_arc_coordinates.distance) * + waypoints_interpolation_arc_margin_ratio; + }(); + const double back_arc_length_threshold = [&]() { + const auto back_waypoint_arc_coordinates = calcArcCoordinates( + lanelet_sequence.at(piecewise_waypoints_lanelet_sequence_index), piecewise_waypoints.back()); + return back_waypoint_arc_coordinates.length + std::abs(back_waypoint_arc_coordinates.distance) * + waypoints_interpolation_arc_margin_ratio; + }(); + + double offset_arc_length = 0.0; + int target_lanelet_sequence_index = static_cast(piecewise_waypoints_lanelet_sequence_index); + while (isIndexWithinVector(lanelet_sequence, target_lanelet_sequence_index)) { + auto & target_piecewise_ref_points = piecewise_ref_points_vec.at(target_lanelet_sequence_index); + const double target_lanelet_arc_length = boost::geometry::length(lanelet::utils::to2D( + lanelet_sequence.at(target_lanelet_sequence_index).centerline().basicLineString())); + + // search overlapped ref points in the target lanelet + std::vector overlapped_ref_points_indices{}; + const bool is_search_finished = [&]() { + for (size_t ref_point_unsigned_index = 0; + ref_point_unsigned_index < target_piecewise_ref_points.size(); + ++ref_point_unsigned_index) { + const size_t ref_point_index = + is_removing_direction_forward + ? ref_point_unsigned_index + : target_piecewise_ref_points.size() - 1 - ref_point_unsigned_index; + const auto & ref_point = target_piecewise_ref_points.at(ref_point_index); + + // skip waypoints + if (ref_point.is_waypoint) { + if ( + target_lanelet_sequence_index == + static_cast(piecewise_waypoints_lanelet_sequence_index)) { + overlapped_ref_points_indices.clear(); + } + continue; + } + + const double ref_point_arc_length = + (is_removing_direction_forward ? 0 : -target_lanelet_arc_length) + + calcArcCoordinates(lanelet_sequence.at(target_lanelet_sequence_index), ref_point.point) + .length; + if (is_removing_direction_forward) { + if (back_arc_length_threshold < offset_arc_length + ref_point_arc_length) { + return true; + } + } else { + if (offset_arc_length + ref_point_arc_length < front_arc_length_threshold) { + return true; + } + } + + overlapped_ref_points_indices.push_back(ref_point_index); + } + return false; + }(); + + // remove overlapped indices from ref_points + removeIndicesFromVector(target_piecewise_ref_points, overlapped_ref_points_indices); + + // break if searching overlapped centerline is finished. + if (is_search_finished) { + break; + } + + target_lanelet_sequence_index += is_removing_direction_forward ? 1 : -1; + offset_arc_length = (is_removing_direction_forward ? 1 : -1) * target_lanelet_arc_length; + } +} + bool RouteHandler::isMapMsgReady() const { return is_map_msg_ready_; diff --git a/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt b/planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt similarity index 80% rename from planning/sampling_based_planner/bezier_sampler/CMakeLists.txt rename to planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt index 76d0fc99843f1..8d5bd554ab282 100644 --- a/planning/sampling_based_planner/bezier_sampler/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_bezier_sampler/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(bezier_sampler) +project(autoware_bezier_sampler) find_package(autoware_cmake REQUIRED) autoware_package() @@ -7,7 +7,7 @@ autoware_package() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -ament_auto_add_library(bezier_sampler SHARED +ament_auto_add_library(autoware_bezier_sampler SHARED src/bezier.cpp src/bezier_sampling.cpp ) diff --git a/planning/sampling_based_planner/bezier_sampler/README.md b/planning/sampling_based_planner/autoware_bezier_sampler/README.md similarity index 100% rename from planning/sampling_based_planner/bezier_sampler/README.md rename to planning/sampling_based_planner/autoware_bezier_sampler/README.md diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp similarity index 94% rename from planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp rename to planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp index 93c6ec6b87f36..5d16c55d793af 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier.hpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEZIER_SAMPLER__BEZIER_HPP_ -#define BEZIER_SAMPLER__BEZIER_HPP_ +#ifndef AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ +#define AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ #include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { // Coefficients for matrix calculation of the quintic Bézier curve. @@ -70,6 +70,6 @@ class Bezier /// @brief calculate the curvature for the given parameter t [[nodiscard]] double curvature(const double t) const; }; -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler -#endif // BEZIER_SAMPLER__BEZIER_HPP_ +#endif // AUTOWARE_BEZIER_SAMPLER__BEZIER_HPP_ diff --git a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp similarity index 80% rename from planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp rename to planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp index 18df9dcb796d8..a832ef1f5cd39 100644 --- a/planning/sampling_based_planner/bezier_sampler/include/bezier_sampler/bezier_sampling.hpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/include/autoware_bezier_sampler/bezier_sampling.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ -#define BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#ifndef AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#define AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ -#include +#include +#include #include #include -#include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { struct SamplingParameters { @@ -38,13 +38,13 @@ struct SamplingParameters /// @details from Section IV of A. Artuñedoet al.: Real-Time Motion Planning Approach for Automated /// Driving in Urban Environments std::vector sample( - const sampler_common::State & initial, const sampler_common::State & final, + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, const SamplingParameters & params); /// @brief generate a Bezier curve for the given states, velocities, and accelerations Bezier generate( const Eigen::Vector2d & initial_pose, const Eigen::Vector2d & final_pose, const Eigen::Vector2d & initial_velocity, const Eigen::Vector2d & initial_acceleration, const Eigen::Vector2d & final_velocity, const Eigen::Vector2d & final_acceleration); -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler -#endif // BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ +#endif // AUTOWARE_BEZIER_SAMPLER__BEZIER_SAMPLING_HPP_ diff --git a/planning/sampling_based_planner/bezier_sampler/package.xml b/planning/sampling_based_planner/autoware_bezier_sampler/package.xml similarity index 89% rename from planning/sampling_based_planner/bezier_sampler/package.xml rename to planning/sampling_based_planner/autoware_bezier_sampler/package.xml index efe65e830871a..62dce46816119 100644 --- a/planning/sampling_based_planner/bezier_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_bezier_sampler/package.xml @@ -1,6 +1,6 @@ - bezier_sampler + autoware_bezier_sampler 0.0.1 Package to sample trajectories using Bézier curves Maxime CLEMENT @@ -11,8 +11,8 @@ autoware_cmake + autoware_sampler_common eigen - sampler_common ament_cmake_ros ament_lint_auto diff --git a/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp similarity index 96% rename from planning/sampling_based_planner/bezier_sampler/src/bezier.cpp rename to planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp index fc4c09840f507..9a882ad06659a 100644 --- a/planning/sampling_based_planner/bezier_sampler/src/bezier.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include -namespace bezier_sampler +namespace autoware::bezier_sampler { Bezier::Bezier(Eigen::Matrix control_points) : control_points_(std::move(control_points)) @@ -117,4 +117,4 @@ double Bezier::heading(const double t) const return std::atan2(vel.y(), vel.x()); } -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler diff --git a/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp similarity index 94% rename from planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp rename to planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp index a9fa318980250..e9a1a9ef32de5 100644 --- a/planning/sampling_based_planner/bezier_sampler/src/bezier_sampling.cpp +++ b/planning/sampling_based_planner/autoware_bezier_sampler/src/bezier_sampling.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include -namespace bezier_sampler +namespace autoware::bezier_sampler { std::vector sample( - const sampler_common::State & initial, const sampler_common::State & final, + const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final, const SamplingParameters & params) { std::vector curves; @@ -75,4 +75,4 @@ Bezier generate( (1.0 / 20.0) * final_acceleration.transpose(); return Bezier(control_points); } -} // namespace bezier_sampler +} // namespace autoware::bezier_sampler diff --git a/planning/sampling_based_planner/frenet_planner/CMakeLists.txt b/planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt similarity index 78% rename from planning/sampling_based_planner/frenet_planner/CMakeLists.txt rename to planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt index 0d7918daa215b..f928702a387b9 100644 --- a/planning/sampling_based_planner/frenet_planner/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_frenet_planner/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(frenet_planner) +project(autoware_frenet_planner) find_package(autoware_cmake REQUIRED) autoware_package() @@ -7,7 +7,7 @@ autoware_package() find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -ament_auto_add_library(frenet_planner SHARED +ament_auto_add_library(autoware_frenet_planner SHARED DIRECTORY src/ ) diff --git a/planning/sampling_based_planner/frenet_planner/README.md b/planning/sampling_based_planner/autoware_frenet_planner/README.md similarity index 100% rename from planning/sampling_based_planner/frenet_planner/README.md rename to planning/sampling_based_planner/autoware_frenet_planner/README.md diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp similarity index 81% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp index f8575d8f65936..dc504ba1386eb 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/conversions.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/conversions.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__CONVERSIONS_HPP_ -#define FRENET_PLANNER__CONVERSIONS_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ +#define AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ -#include "frenet_planner/polynomials.hpp" +#include "autoware_frenet_planner/polynomials.hpp" -#include +#include #include -namespace frenet_planner +namespace autoware::frenet_planner { /// @brief calculate the lengths and yaws vectors of the given trajectory @@ -44,6 +44,6 @@ inline void calculateLengthsAndYaws(Trajectory & trajectory) const auto last_yaw = trajectory.yaws.empty() ? 0.0 : trajectory.yaws.back(); trajectory.yaws.push_back(last_yaw); } -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__CONVERSIONS_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__CONVERSIONS_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp similarity index 67% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp index 7d60214a52722..88eb2f3244e40 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/frenet_planner.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/frenet_planner.hpp @@ -12,34 +12,34 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__FRENET_PLANNER_HPP_ -#define FRENET_PLANNER__FRENET_PLANNER_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ +#define AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ -#include "frenet_planner/structures.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include #include -namespace frenet_planner +namespace autoware::frenet_planner { /// @brief generate trajectories relative to the reference for the given initial state and sampling /// parameters std::vector generateTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate trajectories relative to the reference for the given initial state and sampling /// parameters std::vector generateLowVelocityTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate paths relative to the reference for the given initial state and sampling /// parameters std::vector generatePaths( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters); + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters); /// @brief generate a candidate path /// @details one polynomial for lateral motion (d) is calculated over the longitudinal displacement /// (s): d(s). @@ -58,11 +58,12 @@ Trajectory generateLowVelocityCandidate( const FrenetState & initial_state, const FrenetState & target_state, const double duration, const double time_resolution); /// @brief calculate the cartesian frame of the given path -void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path); +void calculateCartesian( + const autoware::sampler_common::transform::Spline2D & reference, Path & path); /// @brief calculate the cartesian frame of the given trajectory void calculateCartesian( - const sampler_common::transform::Spline2D & reference, Trajectory & trajectory); + const autoware::sampler_common::transform::Spline2D & reference, Trajectory & trajectory); -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__FRENET_PLANNER_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__FRENET_PLANNER_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp similarity index 88% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp index 65157c76d5db3..02217ef8ad444 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/polynomials.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/polynomials.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__POLYNOMIALS_HPP_ -#define FRENET_PLANNER__POLYNOMIALS_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ +#define AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ -namespace frenet_planner +namespace autoware::frenet_planner { class Polynomial { @@ -50,6 +50,6 @@ class Polynomial /// @brief Get the jerk at the given time [[nodiscard]] double jerk(const double t) const; }; -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__POLYNOMIALS_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__POLYNOMIALS_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp similarity index 73% rename from planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp rename to planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp index c44cb5d814d71..d2a71d06a7933 100644 --- a/planning/sampling_based_planner/frenet_planner/include/frenet_planner/structures.hpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/include/autoware_frenet_planner/structures.hpp @@ -12,52 +12,54 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef FRENET_PLANNER__STRUCTURES_HPP_ -#define FRENET_PLANNER__STRUCTURES_HPP_ +#ifndef AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ +#define AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ -#include "frenet_planner/polynomials.hpp" +#include "autoware_frenet_planner/polynomials.hpp" -#include +#include #include #include -namespace frenet_planner +namespace autoware::frenet_planner { struct FrenetState { - sampler_common::FrenetPoint position = {0, 0}; + autoware::sampler_common::FrenetPoint position = {0, 0}; double lateral_velocity{}; double longitudinal_velocity{}; double lateral_acceleration{}; double longitudinal_acceleration{}; }; -struct Path : sampler_common::Path +struct Path : autoware::sampler_common::Path { - std::vector frenet_points{}; + std::vector frenet_points{}; std::optional lateral_polynomial{}; Path() = default; - explicit Path(const sampler_common::Path & path) : sampler_common::Path(path) {} + explicit Path(const autoware::sampler_common::Path & path) : autoware::sampler_common::Path(path) + { + } void clear() override { - sampler_common::Path::clear(); + autoware::sampler_common::Path::clear(); frenet_points.clear(); lateral_polynomial.reset(); } void reserve(const size_t size) override { - sampler_common::Path::reserve(size); + autoware::sampler_common::Path::reserve(size); frenet_points.reserve(size); } [[nodiscard]] Path extend(const Path & path) const { - Path extended_traj(sampler_common::Path::extend(path)); + Path extended_traj(autoware::sampler_common::Path::extend(path)); extended_traj.frenet_points.insert( extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end()); extended_traj.frenet_points.insert( @@ -69,7 +71,7 @@ struct Path : sampler_common::Path [[nodiscard]] Path * subset(const size_t from_idx, const size_t to_idx) const override { - auto * subpath = new Path(*sampler_common::Path::subset(from_idx, to_idx)); + auto * subpath = new Path(*autoware::sampler_common::Path::subset(from_idx, to_idx)); assert(to_idx >= from_idx); subpath->reserve(to_idx - from_idx); @@ -87,19 +89,22 @@ struct SamplingParameter FrenetState target_state; }; -struct Trajectory : sampler_common::Trajectory +struct Trajectory : autoware::sampler_common::Trajectory { - std::vector frenet_points{}; + std::vector frenet_points{}; std::optional lateral_polynomial{}; std::optional longitudinal_polynomial{}; SamplingParameter sampling_parameter; Trajectory() = default; - explicit Trajectory(const sampler_common::Trajectory & traj) : sampler_common::Trajectory(traj) {} + explicit Trajectory(const autoware::sampler_common::Trajectory & traj) + : autoware::sampler_common::Trajectory(traj) + { + } void clear() override { - sampler_common::Trajectory::clear(); + autoware::sampler_common::Trajectory::clear(); frenet_points.clear(); lateral_polynomial.reset(); longitudinal_polynomial.reset(); @@ -107,13 +112,13 @@ struct Trajectory : sampler_common::Trajectory void reserve(const size_t size) override { - sampler_common::Trajectory::reserve(size); + autoware::sampler_common::Trajectory::reserve(size); frenet_points.reserve(size); } [[nodiscard]] Trajectory extend(const Trajectory & traj) const { - Trajectory extended_traj(sampler_common::Trajectory::extend(traj)); + Trajectory extended_traj(autoware::sampler_common::Trajectory::extend(traj)); extended_traj.frenet_points.insert( extended_traj.frenet_points.end(), frenet_points.begin(), frenet_points.end()); extended_traj.frenet_points.insert( @@ -126,7 +131,8 @@ struct Trajectory : sampler_common::Trajectory [[nodiscard]] Trajectory * subset(const size_t from_idx, const size_t to_idx) const override { - auto * sub_trajectory = new Trajectory(*sampler_common::Trajectory::subset(from_idx, to_idx)); + auto * sub_trajectory = + new Trajectory(*autoware::sampler_common::Trajectory::subset(from_idx, to_idx)); assert(to_idx >= from_idx); sub_trajectory->reserve(to_idx - from_idx); @@ -151,6 +157,6 @@ struct SamplingParameters std::vector parameters; double resolution; }; -} // namespace frenet_planner +} // namespace autoware::frenet_planner -#endif // FRENET_PLANNER__STRUCTURES_HPP_ +#endif // AUTOWARE_FRENET_PLANNER__STRUCTURES_HPP_ diff --git a/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch b/planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch similarity index 78% rename from planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch rename to planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch index 9570d488eaecc..69e5961b9cdb8 100644 --- a/planning/sampling_based_planner/frenet_planner/launch/frenet_planner.launch +++ b/planning/sampling_based_planner/autoware_frenet_planner/launch/frenet_planner.launch @@ -3,7 +3,7 @@ - + diff --git a/planning/sampling_based_planner/frenet_planner/package.xml b/planning/sampling_based_planner/autoware_frenet_planner/package.xml similarity index 76% rename from planning/sampling_based_planner/frenet_planner/package.xml rename to planning/sampling_based_planner/autoware_frenet_planner/package.xml index c378c6ae25e9b..1ba7c8a148007 100644 --- a/planning/sampling_based_planner/frenet_planner/package.xml +++ b/planning/sampling_based_planner/autoware_frenet_planner/package.xml @@ -1,8 +1,8 @@ - frenet_planner + autoware_frenet_planner 0.0.1 - The frenet_planner package for trajectory generation in Frenet frame + The autoware_frenet_planner package for trajectory generation in Frenet frame Maxime CLEMENT Maxime CLEMENT Apache License 2.0 @@ -12,7 +12,7 @@ autoware_cmake autoware_auto_common - sampler_common + autoware_sampler_common ament_cmake_ros ament_lint_auto diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp similarity index 89% rename from planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp rename to planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp index 6f0bef882743c..ba3156c6b085d 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "frenet_planner/frenet_planner.hpp" +#include "autoware_frenet_planner/frenet_planner.hpp" #include +#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include #include "autoware_planning_msgs/msg/path.hpp" #include @@ -31,11 +31,11 @@ #include #include -namespace frenet_planner +namespace autoware::frenet_planner { std::vector generateTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector trajectories; trajectories.reserve(sampling_parameters.parameters.size()); @@ -54,8 +54,8 @@ std::vector generateTrajectories( } std::vector generateLowVelocityTrajectories( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector trajectories; trajectories.reserve(sampling_parameters.parameters.size()); @@ -73,8 +73,8 @@ std::vector generateLowVelocityTrajectories( } std::vector generatePaths( - const sampler_common::transform::Spline2D & reference_spline, const FrenetState & initial_state, - const SamplingParameters & sampling_parameters) + const autoware::sampler_common::transform::Spline2D & reference_spline, + const FrenetState & initial_state, const SamplingParameters & sampling_parameters) { std::vector candidates; candidates.reserve(sampling_parameters.parameters.size()); @@ -148,7 +148,8 @@ Path generateCandidate( return path; } -void calculateCartesian(const sampler_common::transform::Spline2D & reference, Path & path) +void calculateCartesian( + const autoware::sampler_common::transform::Spline2D & reference, Path & path) { if (!path.frenet_points.empty()) { path.points.reserve(path.frenet_points.size()); @@ -191,7 +192,7 @@ void calculateCartesian(const sampler_common::transform::Spline2D & reference, P } } void calculateCartesian( - const sampler_common::transform::Spline2D & reference, Trajectory & trajectory) + const autoware::sampler_common::transform::Spline2D & reference, Trajectory & trajectory) { if (!trajectory.frenet_points.empty()) { trajectory.points.reserve(trajectory.frenet_points.size()); @@ -240,4 +241,4 @@ void calculateCartesian( } } -} // namespace frenet_planner +} // namespace autoware::frenet_planner diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp similarity index 94% rename from planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp rename to planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp index 13f71dc88f9d6..b785255cd856c 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/polynomials.cpp +++ b/planning/sampling_based_planner/autoware_frenet_planner/src/frenet_planner/polynomials.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include -namespace frenet_planner +namespace autoware::frenet_planner { // @brief Create a polynomial connecting the given initial and target configuration over the given // duration @@ -72,4 +72,4 @@ double Polynomial::jerk(const double t) const const double t2 = t * t; return 60 * a_ * t2 + 24 * b_ * t + 6 * c_; } -} // namespace frenet_planner +} // namespace autoware::frenet_planner diff --git a/planning/sampling_based_planner/path_sampler/CMakeLists.txt b/planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt similarity index 52% rename from planning/sampling_based_planner/path_sampler/CMakeLists.txt rename to planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt index 5a5b99f0aff3a..170646b0c0c9a 100644 --- a/planning/sampling_based_planner/path_sampler/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_path_sampler/CMakeLists.txt @@ -1,16 +1,16 @@ cmake_minimum_required(VERSION 3.14) -project(path_sampler) +project(autoware_path_sampler) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(path_sampler SHARED +ament_auto_add_library(autoware_path_sampler SHARED DIRECTORY src ) # register node -rclcpp_components_register_node(path_sampler - PLUGIN "path_sampler::PathSampler" +rclcpp_components_register_node(autoware_path_sampler + PLUGIN "autoware::path_sampler::PathSampler" EXECUTABLE path_sampler_exe ) diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/autoware_path_sampler/README.md similarity index 100% rename from planning/sampling_based_planner/path_sampler/README.md rename to planning/sampling_based_planner/autoware_path_sampler/README.md diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp similarity index 87% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp index 7cf88cb75e13a..5a8183d8e5926 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/common_structs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/common_structs.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__COMMON_STRUCTS_HPP_ -#define PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#define AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ -#include "path_sampler/type_alias.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/structures.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/structures.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -28,7 +28,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { struct PlannerData { @@ -61,7 +61,7 @@ struct TimeKeeper latest_stream.str(""); if (enable_calculation_time_info) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("path_sampler.time"), msg); + RCLCPP_INFO_STREAM(rclcpp::get_logger("autoware_path_sampler.time"), msg); } } @@ -87,7 +87,7 @@ struct TimeKeeper struct DebugData { - std::vector sampled_candidates{}; + std::vector sampled_candidates{}; size_t previous_sampled_candidates_nb = 0UL; std::vector obstacles{}; std::vector footprints{}; @@ -131,6 +131,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__COMMON_STRUCTS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__COMMON_STRUCTS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp similarity index 74% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp index ab9bc71bc79e4..143b9ee30b2c2 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/node.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__NODE_HPP_ -#define PATH_SAMPLER__NODE_HPP_ - -#include "path_sampler/common_structs.hpp" -#include "path_sampler/parameters.hpp" -#include "path_sampler/type_alias.hpp" +#ifndef AUTOWARE_PATH_SAMPLER__NODE_HPP_ +#define AUTOWARE_PATH_SAMPLER__NODE_HPP_ + +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "rclcpp/rclcpp.hpp" -#include "sampler_common/transform/spline_transform.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" -#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { class PathSampler : public rclcpp::Node { @@ -39,7 +39,7 @@ class PathSampler : public rclcpp::Node protected: // for the static_centerline_generator package // argument variables - vehicle_info_util::VehicleInfo vehicle_info_{}; + autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; mutable DebugData debug_data_{}; mutable std::shared_ptr time_keeper_ptr_{nullptr}; @@ -54,7 +54,7 @@ class PathSampler : public rclcpp::Node PredictedObjects::SharedPtr in_objects_ptr_ = std::make_shared(); // variables for previous information - std::optional prev_path_; + std::optional prev_path_; // interface publisher rclcpp::Publisher::SharedPtr traj_pub_; @@ -86,8 +86,9 @@ class PathSampler : public rclcpp::Node const std::vector & traj_points, const std::vector & optimized_points) const; void resetPreviousData(); - sampler_common::State getPlanningState( - sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const; + autoware::sampler_common::State getPlanningState( + autoware::sampler_common::State & state, + const autoware::sampler_common::transform::Spline2D & path_spline) const; // sub-functions of generateTrajectory void copyZ( @@ -95,13 +96,14 @@ class PathSampler : public rclcpp::Node void copyVelocity( const std::vector & from_traj, std::vector & to_traj, const geometry_msgs::msg::Pose & ego_pose); - sampler_common::Path generatePath(const PlannerData & planner_data); - std::vector generateCandidatesFromPreviousPath( - const PlannerData & planner_data, const sampler_common::transform::Spline2D & path_spline); + autoware::sampler_common::Path generatePath(const PlannerData & planner_data); + std::vector generateCandidatesFromPreviousPath( + const PlannerData & planner_data, + const autoware::sampler_common::transform::Spline2D & path_spline); std::vector generateTrajectoryPoints(const PlannerData & planner_data); void publishVirtualWall(const geometry_msgs::msg::Pose & stop_pose) const; void publishDebugMarker(const std::vector & traj_points) const; }; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__NODE_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__NODE_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp similarity index 77% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp index 03887d8357e0d..0ce0ea875c066 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/parameters.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/parameters.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__PARAMETERS_HPP_ -#define PATH_SAMPLER__PARAMETERS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ +#define AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ -#include "bezier_sampler/bezier_sampling.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_sampler_common/structures.hpp" #include struct Parameters { - sampler_common::Constraints constraints; + autoware::sampler_common::Constraints constraints; struct { bool enable_frenet{}; @@ -37,7 +37,7 @@ struct Parameters std::vector target_lateral_velocities{}; std::vector target_lateral_accelerations{}; } frenet; - bezier_sampler::SamplingParameters bezier{}; + autoware::bezier_sampler::SamplingParameters bezier{}; } sampling; struct @@ -54,4 +54,4 @@ struct Parameters } path_reuse{}; }; -#endif // PATH_SAMPLER__PARAMETERS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__PARAMETERS_HPP_ diff --git a/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp new file mode 100644 index 0000000000000..ef234f1d8207a --- /dev/null +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/path_generation.hpp @@ -0,0 +1,52 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ +#define AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ + +#include "autoware_bezier_sampler/bezier_sampling.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" + +#include + +#include + +namespace autoware::path_sampler +{ +/** + * @brief generate candidate paths for the given problem inputs + * @param [in] initial_state initial ego state + * @param [in] path_spline spline of the reference path + * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path) + * @param [in] params parameters + * @return candidate paths + */ +std::vector generateCandidatePaths( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const double base_length, + const Parameters & params); + +std::vector generateBezierPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); +std::vector generateFrenetPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); +} // namespace autoware::path_sampler + +#endif // AUTOWARE_PATH_SAMPLER__PATH_GENERATION_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp similarity index 64% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp index a9a2728b27c49..1b10be1121481 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/prepare_inputs.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__PREPARE_INPUTS_HPP_ -#define PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#define AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ -#include "frenet_planner/structures.hpp" -#include "path_sampler/parameters.hpp" -#include "path_sampler/type_alias.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/parameters.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include #include @@ -35,20 +35,20 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { /// @brief prepare constraints void prepareConstraints( - sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, + autoware::sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, const std::vector & left_bound, const std::vector & right_bound); /// @brief prepare sampling parameters to generate Frenet paths -frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); +autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params); /// @brief prepare the 2D spline representation of the given Path message -sampler_common::transform::Spline2D preparePathSpline( +autoware::sampler_common::transform::Spline2D preparePathSpline( const std::vector & path_msg, const bool smooth_path); -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__PREPARE_INPUTS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__PREPARE_INPUTS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp similarity index 88% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp index 90e655c086444..95d833c4df47d 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/type_alias.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ -#define PATH_SAMPLER__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ +#define AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ #include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/path.hpp" @@ -28,7 +28,7 @@ #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace path_sampler +namespace autoware::path_sampler { // std_msgs using std_msgs::msg::Header; @@ -46,6 +46,6 @@ using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; // debug using tier4_debug_msgs::msg::StringStamped; -} // namespace path_sampler +} // namespace autoware::path_sampler -#endif // PATH_SAMPLER__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_PATH_SAMPLER__TYPE_ALIAS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp similarity index 78% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp index 875b81b9cd49a..b6c63eeffed18 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/geometry_utils.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ -#define PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#define AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_sampler/common_structs.hpp" -#include "path_sampler/type_alias.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -35,7 +35,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace geometry_utils { @@ -52,5 +52,5 @@ bool isSamePoint(const T1 & t1, const T2 & t2) return true; } } // namespace geometry_utils -} // namespace path_sampler -#endif // PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ +} // namespace autoware::path_sampler +#endif // AUTOWARE_PATH_SAMPLER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp similarity index 90% rename from planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp rename to planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp index a036dcfe638c2..4f346ae910f44 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/autoware_path_sampler/include/autoware_path_sampler/utils/trajectory_utils.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ -#define PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +#include "autoware_path_sampler/common_structs.hpp" +#include "autoware_path_sampler/type_alias.hpp" +#include "autoware_sampler_common/structures.hpp" #include "eigen3/Eigen/Core" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include "path_sampler/common_structs.hpp" -#include "path_sampler/type_alias.hpp" -#include "sampler_common/structures.hpp" #include "autoware_planning_msgs/msg/path_point.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -33,7 +33,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace trajectory_utils { @@ -95,7 +95,8 @@ std::vector convertToTrajectoryPoints(const std::vector & po return traj_points; } -inline std::vector convertToTrajectoryPoints(const sampler_common::Path & path) +inline std::vector convertToTrajectoryPoints( + const autoware::sampler_common::Path & path) { std::vector traj_points; for (auto i = 0UL; i < path.points.size(); ++i) { @@ -160,7 +161,7 @@ std::optional updateFrontPointForFix( motion_utils::calcSignedArcLength(points, 0, front_fix_point.pose.position); if (0 < lon_offset_to_prev_front) { RCLCPP_WARN( - rclcpp::get_logger("path_sampler.trajectory_utils"), + rclcpp::get_logger("autoware_path_sampler.trajectory_utils"), "Fixed point will not be inserted due to the error during calculation."); return std::nullopt; } @@ -171,7 +172,7 @@ std::optional updateFrontPointForFix( constexpr double max_lat_error = 3.0; if (max_lat_error < dist) { RCLCPP_WARN( - rclcpp::get_logger("path_sampler.trajectory_utils"), + rclcpp::get_logger("autoware_path_sampler.trajectory_utils"), "New Fixed point is too far from points %f [m]", dist); } @@ -193,5 +194,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace path_sampler -#endif // PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ +} // namespace autoware::path_sampler +#endif // AUTOWARE_PATH_SAMPLER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/autoware_path_sampler/package.xml similarity index 89% rename from planning/sampling_based_planner/path_sampler/package.xml rename to planning/sampling_based_planner/autoware_path_sampler/package.xml index a57ff6e6033f6..a48e6c0b836bc 100644 --- a/planning/sampling_based_planner/path_sampler/package.xml +++ b/planning/sampling_based_planner/autoware_path_sampler/package.xml @@ -1,7 +1,7 @@ - path_sampler + autoware_path_sampler 0.1.0 Package for the sampling-based path planner Maxime CLEMENT @@ -13,10 +13,11 @@ autoware_cmake + autoware_bezier_sampler + autoware_frenet_planner autoware_perception_msgs autoware_planning_msgs - bezier_sampler - frenet_planner + autoware_vehicle_info_utils geometry_msgs interpolation motion_utils @@ -28,7 +29,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_planning_msgs - vehicle_info_util visualization_msgs ament_cmake_ros diff --git a/planning/sampling_based_planner/path_sampler/src/node.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp similarity index 94% rename from planning/sampling_based_planner/path_sampler/src/node.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/node.cpp index 97bafb68c47a8..9a828d23af5cb 100644 --- a/planning/sampling_based_planner/path_sampler/src/node.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/node.cpp @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/node.hpp" - +#include "autoware_path_sampler/node.hpp" + +#include "autoware_path_sampler/path_generation.hpp" +#include "autoware_path_sampler/prepare_inputs.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" +#include "autoware_path_sampler/utils/trajectory_utils.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" #include "interpolation/spline_interpolation_points_2d.hpp" #include "motion_utils/marker/marker_helper.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_sampler/path_generation.hpp" -#include "path_sampler/prepare_inputs.hpp" -#include "path_sampler/utils/geometry_utils.hpp" -#include "path_sampler/utils/trajectory_utils.hpp" #include "rclcpp/time.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/soft_constraint.hpp" #include #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace { @@ -60,7 +60,7 @@ bool hasZeroVelocity(const TrajectoryPoint & traj_point) PathSampler::PathSampler(const rclcpp::NodeOptions & node_options) : Node("path_sampler", node_options), - vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), time_keeper_ptr_(std::make_shared()) { // interface publisher @@ -220,8 +220,9 @@ void PathSampler::objectsCallback(const PredictedObjects::SharedPtr msg) in_objects_ptr_ = msg; } -sampler_common::State PathSampler::getPlanningState( - sampler_common::State & state, const sampler_common::transform::Spline2D & path_spline) const +autoware::sampler_common::State PathSampler::getPlanningState( + autoware::sampler_common::State & state, + const autoware::sampler_common::transform::Spline2D & path_spline) const { state.frenet = path_spline.frenet(state.pose); if (params_.preprocessing.force_zero_deviation) { @@ -390,13 +391,14 @@ std::vector PathSampler::generateTrajectory(const PlannerData & return generated_traj_points; } -std::vector PathSampler::generateCandidatesFromPreviousPath( - const PlannerData & planner_data, const sampler_common::transform::Spline2D & path_spline) +std::vector PathSampler::generateCandidatesFromPreviousPath( + const PlannerData & planner_data, + const autoware::sampler_common::transform::Spline2D & path_spline) { - std::vector candidates; + std::vector candidates; if (!prev_path_ || prev_path_->points.size() < 2) return candidates; // Update previous path - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {planner_data.ego_pose.position.x, planner_data.ego_pose.position.y}; current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation); const auto closest_iter = std::min_element( @@ -419,7 +421,7 @@ std::vector PathSampler::generateCandidatesFromPreviousPat const auto reuse_step = prev_path_length / params_.sampling.previous_path_reuse_points_nb; for (double reuse_length = reuse_step; reuse_length <= prev_path_length; reuse_length += reuse_step) { - sampler_common::State reuse_state; + autoware::sampler_common::State reuse_state; size_t reuse_idx = 0; for (reuse_idx = 0; reuse_idx + 1 < prev_path_->lengths.size() && prev_path_->lengths[reuse_idx] < reuse_length; @@ -438,13 +440,13 @@ std::vector PathSampler::generateCandidatesFromPreviousPat return candidates; } -sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) +autoware::sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) { time_keeper_ptr_->tic(__func__); - sampler_common::Path generated_path{}; + autoware::sampler_common::Path generated_path{}; if (prev_path_ && prev_path_->points.size() > 1) { - sampler_common::constraints::checkHardConstraints(*prev_path_, params_.constraints); + autoware::sampler_common::constraints::checkHardConstraints(*prev_path_, params_.constraints); if (prev_path_->constraint_results.isValid()) { const auto prev_path_spline = preparePathSpline(trajectory_utils::convertToTrajectoryPoints(*prev_path_), false); @@ -460,7 +462,7 @@ sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) } const auto path_spline = preparePathSpline(planner_data.traj_points, params_.preprocessing.smooth_reference); - sampler_common::State current_state; + autoware::sampler_common::State current_state; current_state.pose = {planner_data.ego_pose.position.x, planner_data.ego_pose.position.y}; current_state.heading = tf2::getYaw(planner_data.ego_pose.orientation); @@ -476,9 +478,9 @@ sampler_common::Path PathSampler::generatePath(const PlannerData & planner_data) debug_data_.footprints.clear(); for (auto & path : candidate_paths) { const auto footprint = - sampler_common::constraints::checkHardConstraints(path, params_.constraints); + autoware::sampler_common::constraints::checkHardConstraints(path, params_.constraints); debug_data_.footprints.push_back(footprint); - sampler_common::constraints::calculateCost(path, params_.constraints, path_spline); + autoware::sampler_common::constraints::calculateCost(path, params_.constraints, path_spline); } const auto best_path_idx = [](const auto & paths) { auto min_cost = std::numeric_limits::max(); @@ -701,7 +703,7 @@ std::vector PathSampler::extendTrajectory( time_keeper_ptr_->toc(__func__, " "); return resampled_traj_points; } -} // namespace path_sampler +} // namespace autoware::path_sampler #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(path_sampler::PathSampler) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_sampler::PathSampler) diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp similarity index 72% rename from planning/sampling_based_planner/path_sampler/src/path_generation.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp index 2d1587c7b45a3..63ca5e18f423b 100644 --- a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/path_generation.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/path_generation.hpp" +#include "autoware_path_sampler/path_generation.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -29,14 +29,14 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { -std::vector generateCandidatePaths( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, const double base_length, +std::vector generateCandidatePaths( + const autoware::sampler_common::State & initial_state, + const autoware::sampler_common::transform::Spline2D & path_spline, const double base_length, const Parameters & params) { - std::vector paths; + std::vector paths; const auto move_to_paths = [&](auto & paths_to_move) { paths.insert( paths.end(), std::make_move_iterator(paths_to_move.begin()), @@ -53,27 +53,27 @@ std::vector generateCandidatePaths( } return paths; } -std::vector generateBezierPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +std::vector generateBezierPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { const auto initial_s = path_spline.frenet(initial_state.pose).s; const auto max_s = path_spline.lastS(); - std::vector bezier_paths; + std::vector bezier_paths; for (const auto target_length : params.sampling.target_lengths) { if (target_length <= base_length) continue; const auto target_s = std::min(max_s, initial_s + target_length - base_length); if (target_s >= max_s) break; - sampler_common::State target_state{}; + autoware::sampler_common::State target_state{}; target_state.pose = path_spline.cartesian({target_s, 0}); target_state.curvature = path_spline.curvature(target_s); target_state.heading = path_spline.yaw(target_s); const auto bezier_samples = - bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); + autoware::bezier_sampler::sample(initial_state, target_state, params.sampling.bezier); const auto step = std::min(0.1, params.sampling.resolution / target_length); for (const auto & bezier : bezier_samples) { - sampler_common::Path path; + autoware::sampler_common::Path path; path.lengths.push_back(0.0); for (double t = 0.0; t <= 1.0; t += step) { const auto x_y = bezier.valueM(t); @@ -93,14 +93,14 @@ std::vector generateBezierPaths( return bezier_paths; } -std::vector generateFrenetPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +std::vector generateFrenetPaths( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { const auto sampling_parameters = prepareSamplingParameters(initial_state, base_length, path_spline, params); - frenet_planner::FrenetState initial_frenet_state; + autoware::frenet_planner::FrenetState initial_frenet_state; initial_frenet_state.position = path_spline.frenet(initial_state.pose); const auto s = initial_frenet_state.position.s; const auto d = initial_frenet_state.position.d; @@ -121,6 +121,7 @@ std::vector generateFrenetPaths( ((1 - path_curvature * d) / (cos_yaw * cos_yaw)) * (initial_state.curvature * ((1 - path_curvature * d) / cos_yaw) - path_curvature); } - return frenet_planner::generatePaths(path_spline, initial_frenet_state, sampling_parameters); + return autoware::frenet_planner::generatePaths( + path_spline, initial_frenet_state, sampling_parameters); } -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp similarity index 84% rename from planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp index fd52764950ca9..64a09087666ff 100644 --- a/planning/sampling_based_planner/path_sampler/src/prepare_inputs.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/prepare_inputs.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/prepare_inputs.hpp" +#include "autoware_path_sampler/prepare_inputs.hpp" -#include "frenet_planner/structures.hpp" -#include "path_sampler/utils/geometry_utils.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_frenet_planner/structures.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -29,22 +29,22 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { void prepareConstraints( - sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, + autoware::sampler_common::Constraints & constraints, const PredictedObjects & predicted_objects, const std::vector & left_bound, const std::vector & right_bound) { - constraints.obstacle_polygons = sampler_common::MultiPolygon2d(); + constraints.obstacle_polygons = autoware::sampler_common::MultiPolygon2d(); for (const auto & o : predicted_objects.objects) if (o.kinematics.initial_twist_with_covariance.twist.linear.x < 0.5) // TODO(Maxime): param constraints.obstacle_polygons.push_back(tier4_autoware_utils::toPolygon2d(o)); constraints.dynamic_obstacles = {}; // TODO(Maxime): not implemented // TODO(Maxime): directly use lines instead of polygon - sampler_common::Polygon2d drivable_area_polygon; + autoware::sampler_common::Polygon2d drivable_area_polygon; for (const auto & p : right_bound) drivable_area_polygon.outer().emplace_back(p.x, p.y); for (auto it = left_bound.rbegin(); it != left_bound.rend(); ++it) drivable_area_polygon.outer().emplace_back(it->x, it->y); @@ -52,9 +52,9 @@ void prepareConstraints( constraints.drivable_polygons = {drivable_area_polygon}; } -frenet_planner::SamplingParameters prepareSamplingParameters( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params) +autoware::frenet_planner::SamplingParameters prepareSamplingParameters( + const autoware::sampler_common::State & initial_state, const double base_length, + const autoware::sampler_common::transform::Spline2D & path_spline, const Parameters & params) { // calculate target lateral positions std::vector target_lateral_positions; @@ -87,10 +87,10 @@ frenet_planner::SamplingParameters prepareSamplingParameters( } else { target_lateral_positions = params.sampling.target_lateral_positions; } - frenet_planner::SamplingParameters sampling_parameters; + autoware::frenet_planner::SamplingParameters sampling_parameters; sampling_parameters.resolution = params.sampling.resolution; const auto max_s = path_spline.lastS(); - frenet_planner::SamplingParameter p; + autoware::frenet_planner::SamplingParameter p; for (const auto target_length : params.sampling.target_lengths) { p.target_state.position.s = std::min( max_s, path_spline.frenet(initial_state.pose).s + std::max(0.0, target_length - base_length)); @@ -109,7 +109,7 @@ frenet_planner::SamplingParameters prepareSamplingParameters( return sampling_parameters; } -sampler_common::transform::Spline2D preparePathSpline( +autoware::sampler_common::transform::Spline2D preparePathSpline( const std::vector & path, const bool smooth_path) { std::vector x; @@ -151,4 +151,4 @@ sampler_common::transform::Spline2D preparePathSpline( } return {x, y}; } -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp similarity index 95% rename from planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp rename to planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp index 07bd4f32787de..d161d60bc724d 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/autoware_path_sampler/src/utils/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "path_sampler/utils/trajectory_utils.hpp" +#include "autoware_path_sampler/utils/trajectory_utils.hpp" +#include "autoware_path_sampler/utils/geometry_utils.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" #include "autoware_planning_msgs/msg/path_point.hpp" @@ -31,7 +31,7 @@ #include #include -namespace path_sampler +namespace autoware::path_sampler { namespace trajectory_utils { @@ -107,4 +107,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace path_sampler +} // namespace autoware::path_sampler diff --git a/planning/sampling_based_planner/sampler_common/CMakeLists.txt b/planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt similarity index 81% rename from planning/sampling_based_planner/sampler_common/CMakeLists.txt rename to planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt index 5e669a705201e..d82f38d3ec0c0 100644 --- a/planning/sampling_based_planner/sampler_common/CMakeLists.txt +++ b/planning/sampling_based_planner/autoware_sampler_common/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(sampler_common) +project(autoware_sampler_common) find_package(autoware_cmake REQUIRED) autoware_package() @@ -8,7 +8,7 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED) -ament_auto_add_library(sampler_common SHARED +ament_auto_add_library(autoware_sampler_common SHARED DIRECTORY src/ ) @@ -22,7 +22,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_sampler_common - sampler_common + autoware_sampler_common ) endif() diff --git a/planning/sampling_based_planner/sampler_common/README.md b/planning/sampling_based_planner/autoware_sampler_common/README.md similarity index 100% rename from planning/sampling_based_planner/sampler_common/README.md rename to planning/sampling_based_planner/autoware_sampler_common/README.md diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp similarity index 73% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp index 061f922001a0f..52dd1a83bd99e 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/footprint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/footprint.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief Calculate the footprint of the given path /// @param path sequence of pose used to build the footprint /// @param constraints input constraint object containing vehicle footprint offsets /// @return the polygon footprint of the path MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constraints); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__FOOTPRINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp similarity index 73% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp index a314e83b065d7..6098e7c15d33d 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/hard_constraint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/hard_constraint.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief Check if the path satisfies the hard constraints MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints); @@ -27,6 +27,6 @@ bool has_collision( const double min_distance = 0.0); bool satisfyMinMax(const std::vector & values, const double min, const double max); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__HARD_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp similarity index 80% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp index ed04d8104a904..7867efa1fa2db 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/map_constraints.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/map_constraints.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { struct MapConstraints { @@ -55,6 +55,6 @@ struct MapConstraints MultiPolygon2d drivable_polygons; std::vector polygon_costs; }; -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__MAP_CONSTRAINTS_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp similarity index 74% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp index 9fdb9fe137e49..290b6b6674e21 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/constraints/soft_constraint.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/constraints/soft_constraint.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ -#define SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#define AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { /// @brief calculate the curvature cost of the given path void calculateCurvatureCost(Path & path, const Constraints & constraints); @@ -30,6 +30,6 @@ void calculateLateralDeviationCost( /// @brief calculate the overall cost of the given path void calculateCost( Path & path, const Constraints & constraints, const transform::Spline2D & reference); -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints -#endif // SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__CONSTRAINTS__SOFT_CONSTRAINT_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp similarity index 98% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp index cef791a01df44..a3ba25b6594e9 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/structures.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/structures.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__STRUCTURES_HPP_ -#define SAMPLER_COMMON__STRUCTURES_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ +#define AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" @@ -36,7 +36,7 @@ #include #include -namespace sampler_common +namespace autoware::sampler_common { using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::MultiPoint2d; @@ -367,6 +367,6 @@ struct ReusableTrajectory Configuration planning_configuration; // planning configuration at the end of the trajectory }; -} // namespace sampler_common +} // namespace autoware::sampler_common -#endif // SAMPLER_COMMON__STRUCTURES_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__STRUCTURES_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp similarity index 88% rename from planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp rename to planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp index ce66141832dbf..c5f78c29cb13b 100644 --- a/planning/sampling_based_planner/sampler_common/include/sampler_common/transform/spline_transform.hpp +++ b/planning/sampling_based_planner/autoware_sampler_common/include/autoware_sampler_common/transform/spline_transform.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ -#define SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#ifndef AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#define AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include -namespace sampler_common::transform +namespace autoware::sampler_common::transform { -using sampler_common::FrenetPoint; +using autoware::sampler_common::FrenetPoint; class Spline { @@ -80,6 +80,6 @@ class Spline2D static std::vector arcLength( const std::vector & x, const std::vector & y); }; -} // namespace sampler_common::transform +} // namespace autoware::sampler_common::transform -#endif // SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ +#endif // AUTOWARE_SAMPLER_COMMON__TRANSFORM__SPLINE_TRANSFORM_HPP_ diff --git a/planning/sampling_based_planner/sampler_common/package.xml b/planning/sampling_based_planner/autoware_sampler_common/package.xml similarity index 94% rename from planning/sampling_based_planner/sampler_common/package.xml rename to planning/sampling_based_planner/autoware_sampler_common/package.xml index 321ca2fd43fef..a1462131d62ad 100644 --- a/planning/sampling_based_planner/sampler_common/package.xml +++ b/planning/sampling_based_planner/autoware_sampler_common/package.xml @@ -1,6 +1,6 @@ - sampler_common + autoware_sampler_common 0.0.1 Common classes and functions for sampling based planners Maxime CLEMENT diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp similarity index 87% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp index 92c8ae65267db..1aafa598142a4 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/footprint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/footprint.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" -#include "sampler_common/structures.hpp" +#include "autoware_sampler_common/structures.hpp" #include @@ -25,7 +25,7 @@ #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { namespace @@ -50,4 +50,4 @@ MultiPoint2d buildFootprintPoints(const Path & path, const Constraints & constra } return footprint; } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp similarity index 89% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp index 7bb29c8837723..9901b3979259c 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/hard_constraint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/hard_constraint.cpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/hard_constraint.hpp" +#include "autoware_sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/constraints/footprint.hpp" +#include "autoware_sampler_common/constraints/footprint.hpp" #include #include #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { bool satisfyMinMax(const std::vector & values, const double min, const double max) { @@ -56,4 +56,4 @@ MultiPoint2d checkHardConstraints(Path & path, const Constraints & constraints) } return footprint; } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp similarity index 85% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp index b1d390e68c49f..71dff81929553 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/constraints/soft_constraint.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/constraints/soft_constraint.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "sampler_common/constraints/soft_constraint.hpp" +#include "autoware_sampler_common/constraints/soft_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" +#include "autoware_sampler_common/structures.hpp" +#include "autoware_sampler_common/transform/spline_transform.hpp" #include -namespace sampler_common::constraints +namespace autoware::sampler_common::constraints { void calculateCurvatureCost(Path & path, const Constraints & constraints) { @@ -51,4 +51,4 @@ void calculateCost( calculateLengthCost(path, constraints); calculateLateralDeviationCost(path, constraints, reference); } -} // namespace sampler_common::constraints +} // namespace autoware::sampler_common::constraints diff --git a/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp similarity index 98% rename from planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp rename to planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp index cf5f1e5478fe5..ce473b1768a54 100644 --- a/planning/sampling_based_planner/sampler_common/src/sampler_common/transform/spline_transform.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/src/sampler_common/transform/spline_transform.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include #include -namespace sampler_common::transform +namespace autoware::sampler_common::transform { Spline::Spline(const std::vector & base_index, const std::vector & base_value) { @@ -312,4 +312,4 @@ double Spline2D::yaw(const double s) const return std::atan2(y_vel, x_vel); } -} // namespace sampler_common::transform +} // namespace autoware::sampler_common::transform diff --git a/planning/sampling_based_planner/sampler_common/test/test_structures.cpp b/planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp similarity index 96% rename from planning/sampling_based_planner/sampler_common/test/test_structures.cpp rename to planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp index 006768c9325e2..77496209290c1 100644 --- a/planning/sampling_based_planner/sampler_common/test/test_structures.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/test/test_structures.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include TEST(Path, extendPath) { - using sampler_common::Path; + using autoware::sampler_common::Path; Path traj1; Path traj2; Path traj3 = traj1.extend(traj2); @@ -43,7 +43,7 @@ TEST(Path, extendPath) TEST(Trajectory, resample) { constexpr auto eps = 1e-6; - using sampler_common::Trajectory; + using autoware::sampler_common::Trajectory; Trajectory t; t.reserve(2); @@ -103,7 +103,7 @@ TEST(Trajectory, resample) TEST(Trajectory, resampleTime) { constexpr auto eps = 1e-6; - using sampler_common::Trajectory; + using autoware::sampler_common::Trajectory; Trajectory t; t.reserve(2); diff --git a/planning/sampling_based_planner/sampler_common/test/test_transform.cpp b/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp similarity index 85% rename from planning/sampling_based_planner/sampler_common/test/test_transform.cpp rename to planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp index d7998d7b8879e..6c5f43e929766 100644 --- a/planning/sampling_based_planner/sampler_common/test/test_transform.cpp +++ b/planning/sampling_based_planner/autoware_sampler_common/test/test_transform.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include @@ -24,13 +24,13 @@ constexpr auto TOL = 1E-6; // 1µm tolerance TEST(splineTransform, makeSpline2D) { - sampler_common::transform::Spline2D spline1({0.0, 1.0, 2.0}, {0.0, 1.0, 2.0}); - sampler_common::transform::Spline2D spline2({-10.0, 5.0, -2.0}, {99.0, 3.0, -20.0}); + autoware::sampler_common::transform::Spline2D spline1({0.0, 1.0, 2.0}, {0.0, 1.0, 2.0}); + autoware::sampler_common::transform::Spline2D spline2({-10.0, 5.0, -2.0}, {99.0, 3.0, -20.0}); } TEST(splineTransform, toFrenet) { - sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); auto frenet = spline.frenet({0.0, 0.0}); EXPECT_NEAR(frenet.s, 0.0, TOL); EXPECT_NEAR(frenet.d, 0.0, TOL); @@ -56,7 +56,7 @@ TEST(splineTransform, toFrenet) EXPECT_NEAR(frenet.s, 1.0, TOL); EXPECT_NEAR(frenet.d, -1.0, TOL); // EDGE CASE 90 deg angle - sampler_common::transform::Spline2D spline2({0.0, 1.0, 2.0}, {0.0, 1.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline2({0.0, 1.0, 2.0}, {0.0, 1.0, 0.0}); frenet = spline2.frenet({1.0, 2.0}); EXPECT_NEAR(frenet.s, 1.0, TOL); EXPECT_NEAR(frenet.d, 1.0, TOL); @@ -64,7 +64,7 @@ TEST(splineTransform, toFrenet) TEST(splineTransform, toCartesian) { - sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); + autoware::sampler_common::transform::Spline2D spline({0.0, 1.0, 2.0}, {0.0, 0.0, 0.0}); auto cart = spline.cartesian({1.0, 0.0}); EXPECT_NEAR(cart.x(), 1.0, TOL); EXPECT_NEAR(cart.y(), 0.0, TOL); @@ -85,7 +85,7 @@ TEST(splineTransform, benchFrenet) auto y = 0.0; std::generate(xs.begin(), xs.end(), [&]() { return ++x; }); std::generate(ys.begin(), ys.end(), [&]() { return ++y; }); - sampler_common::transform::Spline2D spline(xs, ys); + autoware::sampler_common::transform::Spline2D spline(xs, ys); auto points_distribution = std::uniform_real_distribution(0.0, static_cast(size)); constexpr auto nb_iter = 1e3; for (auto iter = 0; iter < nb_iter; ++iter) { diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp deleted file mode 100644 index 448400236c9f8..0000000000000 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/path_generation.hpp +++ /dev/null @@ -1,52 +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 PATH_SAMPLER__PATH_GENERATION_HPP_ -#define PATH_SAMPLER__PATH_GENERATION_HPP_ - -#include "bezier_sampler/bezier_sampling.hpp" -#include "frenet_planner/structures.hpp" -#include "path_sampler/parameters.hpp" -#include "sampler_common/constraints/hard_constraint.hpp" -#include "sampler_common/structures.hpp" -#include "sampler_common/transform/spline_transform.hpp" - -#include - -#include - -namespace path_sampler -{ -/** - * @brief generate candidate paths for the given problem inputs - * @param [in] initial_state initial ego state - * @param [in] path_spline spline of the reference path - * @param [in] base_length base length of the reuse path (= 0.0 if not reusing a previous path) - * @param [in] params parameters - * @return candidate paths - */ -std::vector generateCandidatePaths( - const sampler_common::State & initial_state, - const sampler_common::transform::Spline2D & path_spline, const double base_length, - const Parameters & params); - -std::vector generateBezierPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); -std::vector generateFrenetPaths( - const sampler_common::State & initial_state, const double base_length, - const sampler_common::transform::Spline2D & path_spline, const Parameters & params); -} // namespace path_sampler - -#endif // PATH_SAMPLER__PATH_GENERATION_HPP_ diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index ded596a8a9898..b3be5be12b75d 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -4,27 +4,32 @@ project(imu_corrector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(gyro_bias_estimation_module SHARED - src/gyro_bias_estimation_module.cpp -) - -ament_auto_add_executable(imu_corrector +ament_auto_add_library(${PROJECT_NAME} SHARED src/imu_corrector_core.cpp - src/imu_corrector_node.cpp ) -ament_auto_add_executable(gyro_bias_estimator +ament_auto_add_library(gyro_bias_estimator SHARED src/gyro_bias_estimator.cpp - src/gyro_bias_estimator_node.cpp + src/gyro_bias_estimation_module.cpp ) -target_link_libraries(gyro_bias_estimator gyro_bias_estimation_module) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "imu_corrector::ImuCorrector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + +rclcpp_components_register_node(gyro_bias_estimator + PLUGIN "imu_corrector::GyroBiasEstimator" + EXECUTABLE gyro_bias_estimator_node + EXECUTOR SingleThreadedExecutor +) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_link_libraries("${test_name}" gyro_bias_estimation_module) + target_link_libraries("${test_name}" gyro_bias_estimator) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml index e16ccef446aba..100168b17171a 100644 --- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/sensing/imu_corrector/launch/imu_corrector.launch.xml b/sensing/imu_corrector/launch/imu_corrector.launch.xml index d87d6e77c110d..8430f2a50a85c 100755 --- a/sensing/imu_corrector/launch/imu_corrector.launch.xml +++ b/sensing/imu_corrector/launch/imu_corrector.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index e99667ed1c4a6..d79626f66801b 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -23,8 +23,8 @@ namespace imu_corrector { -GyroBiasEstimator::GyroBiasEstimator() -: Node("gyro_bias_validator"), +GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) +: rclcpp::Node("gyro_bias_validator", options), gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), @@ -244,3 +244,6 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW } } // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 3592ff1f7d3b4..671c18de78f86 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -42,7 +42,7 @@ class GyroBiasEstimator : public rclcpp::Node using Odometry = nav_msgs::msg::Odometry; public: - GyroBiasEstimator(); + explicit GyroBiasEstimator(const rclcpp::NodeOptions & options); private: void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp b/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp deleted file mode 100644 index a491957bbb57f..0000000000000 --- a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp +++ /dev/null @@ -1,26 +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 "gyro_bias_estimator.hpp" - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index a26c64925729c..c8e7698cb4e37 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -53,8 +53,9 @@ geometry_msgs::msg::Vector3 transformVector3( namespace imu_corrector { -ImuCorrector::ImuCorrector() -: Node("imu_corrector"), output_frame_(declare_parameter("base_link", "base_link")) +ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options) +: rclcpp::Node("imu_corrector", options), + output_frame_(declare_parameter("base_link", "base_link")) { transform_listener_ = std::make_shared(this); @@ -123,3 +124,6 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m } } // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector) diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp index 7709c611ab714..4d5377274ae78 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -34,7 +34,7 @@ class ImuCorrector : public rclcpp::Node using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - ImuCorrector(); + explicit ImuCorrector(const rclcpp::NodeOptions & options); private: void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); diff --git a/sensing/imu_corrector/src/imu_corrector_node.cpp b/sensing/imu_corrector/src/imu_corrector_node.cpp deleted file mode 100644 index c1df5bee28439..0000000000000 --- a/sensing/imu_corrector/src/imu_corrector_node.cpp +++ /dev/null @@ -1,26 +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 "imu_corrector_core.hpp" - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py b/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py index 175209d49fa8b..98bc5a6dd2699 100644 --- a/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py +++ b/sensing/pointcloud_preprocessor/launch/polygon_remover.launch.py @@ -26,7 +26,7 @@ def generate_launch_description(): pkg = "pointcloud_preprocessor" param_file = os.path.join( - get_package_share_directory("vehicle_info_util"), "config/polygon_remover.yaml" + get_package_share_directory("autoware_vehicle_info_utils"), "config/polygon_remover.yaml" ) with open(param_file, "r") as f: diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index a8eb88f55df19..fd01f42928947 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -91,7 +91,7 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg( "vehicle_info_param_file", [ - FindPackageShare("vehicle_info_util"), + FindPackageShare("autoware_vehicle_info_utils"), "/config/vehicle_info.param.yaml", ], "path to the parameter file of vehicle information", diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 5f04fba4fdb8e..b820c5c6c3df0 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -18,6 +18,7 @@ autoware_control_msgs autoware_map_msgs autoware_planning_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs geometry_msgs lanelet2_core @@ -35,7 +36,6 @@ tier4_autoware_utils tier4_external_api_msgs tier4_vehicle_msgs - vehicle_info_util autoware_cmake launch_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 3b65218aaf03d..c66a0c6f1f922 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -14,13 +14,13 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include #include @@ -232,7 +232,7 @@ void SimplePlanningSimulator::initialize_vehicle_model() const double debug_acc_scaling_factor = declare_parameter("debug_acc_scaling_factor", 1.0); const double debug_steer_scaling_factor = declare_parameter("debug_steer_scaling_factor", 1.0); - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); const double wheelbase = vehicle_info.wheel_base_m; std::vector model_module_paths = declare_parameter>( diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 77276c7acb0e3..315fdea27a3f3 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -17,6 +17,7 @@ autoware_adapi_version_msgs autoware_planning_msgs autoware_system_msgs + autoware_vehicle_info_utils autoware_vehicle_msgs component_interface_specs component_interface_utils @@ -32,7 +33,6 @@ tier4_api_msgs tier4_control_msgs tier4_system_msgs - vehicle_info_util python3-flask diff --git a/system/default_ad_api/src/vehicle_info.cpp b/system/default_ad_api/src/vehicle_info.cpp index cff7ac5cd5eae..8bf7da65f6e33 100644 --- a/system/default_ad_api/src/vehicle_info.cpp +++ b/system/default_ad_api/src/vehicle_info.cpp @@ -14,7 +14,7 @@ #include "vehicle_info.hpp" -#include +#include namespace { @@ -41,7 +41,7 @@ VehicleInfoNode::VehicleInfoNode(const rclcpp::NodeOptions & options) res->dimensions = dimensions_; }; - const auto vehicle = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); dimensions_.wheel_radius = vehicle.wheel_radius_m; dimensions_.wheel_width = vehicle.wheel_width_m; dimensions_.wheel_base = vehicle.wheel_base_m; diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 012bb55af3068..cd66cde64c9a5 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -20,6 +20,8 @@ #include // Autoware +#include + #include #include #include @@ -56,33 +58,26 @@ class EmergencyHandler : public rclcpp::Node explicit EmergencyHandler(const rclcpp::NodeOptions & options); private: - // Subscribers + // Subscribers with callback rclcpp::Subscription::SharedPtr sub_hazard_status_stamped_; rclcpp::Subscription::SharedPtr sub_prev_control_command_; - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_control_mode_; - rclcpp::Subscription::SharedPtr - sub_mrm_comfortable_stop_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_emergency_stop_status_; + // Subscribers without callback + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_mode_{this, "~/input/control_mode"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; autoware_control_msgs::msg::Control::ConstSharedPtr prev_control_command_; - nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; void onHazardStatusStamped( const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); void onPrevControlCommand(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); - void onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); // Publisher rclcpp::Publisher::SharedPtr pub_control_command_; @@ -135,9 +130,13 @@ class EmergencyHandler : public rclcpp::Node void updateMrmState(); void operateMrm(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); - bool isStopped(); + + bool isAutonomous(); bool isDrivingBackwards(); bool isEmergency(); + bool isStopped(); + bool isComfortableStopStatusAvailable(); + bool isEmergencyStopStatusAvailable(); }; #endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index aa2090c86edb8..f26080fd8ef56 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -21,6 +21,7 @@ rclcpp_components std_msgs std_srvs + tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index ae3d60e32a445..b2ee12afc9c76 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -30,24 +30,13 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) using std::placeholders::_1; - // Subscriber + // Subscribers with callback sub_hazard_status_stamped_ = create_subscription( "~/input/hazard_status", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1)); sub_prev_control_command_ = create_subscription( "~/input/prev_control_command", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); - sub_odom_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1)); - // subscribe control mode - sub_control_mode_ = create_subscription( - "~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1)); - sub_mrm_comfortable_stop_status_ = create_subscription( - "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onMrmComfortableStopStatus, this, _1)); - sub_mrm_emergency_stop_status_ = create_subscription( - "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); // Publisher pub_control_command_ = create_publisher( @@ -72,13 +61,6 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) client_mrm_emergency_stop_group_); // Initialize - odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - prev_control_command_ = - autoware_control_msgs::msg::Control::ConstSharedPtr(new autoware_control_msgs::msg::Control); - mrm_comfortable_stop_status_ = - std::make_shared(); - mrm_emergency_stop_status_ = std::make_shared(); mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; @@ -105,29 +87,6 @@ void EmergencyHandler::onPrevControlCommand( prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); } -void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - odom_ = msg; -} - -void EmergencyHandler::onControlMode( - const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - control_mode_ = msg; -} - -void EmergencyHandler::onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_comfortable_stop_status_ = msg; -} - -void EmergencyHandler::onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_emergency_stop_status_ = msg; -} - autoware_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() { using autoware_vehicle_msgs::msg::HazardLightsCommand; @@ -293,17 +252,14 @@ bool EmergencyHandler::isDataReady() return false; } - if ( - param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == - tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm comfortable stop to become available..."); return false; } - if ( - mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (!isEmergencyStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm emergency stop to become available..."); @@ -381,7 +337,7 @@ void EmergencyHandler::updateMrmState() const bool is_emergency = isEmergency(); // Get mode - const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; + const bool is_auto_mode = isAutonomous(); // State Machine if (mrm_state_.state == MrmState::NORMAL) { @@ -447,6 +403,14 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre return mrm_state_.behavior; } +bool EmergencyHandler::isAutonomous() +{ + using autoware_vehicle_msgs::msg::ControlModeReport; + auto mode = sub_control_mode_.takeData(); + if (mode == nullptr) return false; + return mode->mode == ControlModeReport::AUTONOMOUS; +} + bool EmergencyHandler::isEmergency() { return hazard_status_stamped_->status.emergency || @@ -455,14 +419,32 @@ bool EmergencyHandler::isEmergency() bool EmergencyHandler::isStopped() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_stopped_velocity = 0.001; - return (std::abs(odom_->twist.twist.linear.x) < th_stopped_velocity); + return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); +} + +bool EmergencyHandler::isComfortableStopStatusAvailable() +{ + auto status = sub_mrm_comfortable_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool EmergencyHandler::isEmergencyStopStatusAvailable() +{ + auto status = sub_mrm_emergency_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; } bool EmergencyHandler::isDrivingBackwards() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_moving_backwards = -0.001; - return odom_->twist.twist.linear.x < th_moving_backwards; + return odom->twist.twist.linear.x < th_moving_backwards; } #include diff --git a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp b/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp deleted file mode 100644 index 03bad4198776d..0000000000000 --- a/system/mrm_comfortable_stop_operator/src/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_node.cpp +++ /dev/null @@ -1,27 +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 "mrm_comfortable_stop_operator/mrm_comfortable_stop_operator_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 7a160b6c531e9..33d8af5004c47 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -22,6 +22,8 @@ #include // Autoware +#include + #include #include #include @@ -66,39 +68,28 @@ class MrmHandler : public rclcpp::Node // type enum RequestType { CALL, CANCEL }; - // Subscribers - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_control_mode_; + // Subscribers with callback rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; - rclcpp::Subscription::SharedPtr - sub_mrm_pull_over_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_comfortable_stop_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_emergency_stop_status_; - rclcpp::Subscription::SharedPtr - sub_operation_mode_state_; - - nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + // Subscribers without callback + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_mode_{this, "~/input/control_mode"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_pull_over_status_{this, "~/input/mrm/pull_over/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_adapi_v1_msgs::msg::OperationModeState> + sub_operation_mode_state_{this, "~/input/api/operation_mode/state"}; + tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; - autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); - void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); // Publisher @@ -156,6 +147,10 @@ class MrmHandler : public rclcpp::Node bool isStopped(); bool isDrivingBackwards(); bool isEmergency() const; + bool isAutonomous(); + bool isPullOverStatusAvailable(); + bool isComfortableStopStatusAvailable(); + bool isEmergencyStopStatusAvailable(); bool isArrivedAtGoal(); }; diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index 2db22cecaa82d..5774cce3215a9 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -21,6 +21,7 @@ rclcpp_components std_msgs std_srvs + tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index d70303a9801ac..326fbb392fd35 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -36,28 +36,11 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" using std::placeholders::_1; - // Subscriber - sub_odom_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmHandler::onOdometry, this, _1)); - // subscribe control mode - sub_control_mode_ = create_subscription( - "~/input/control_mode", rclcpp::QoS{1}, std::bind(&MrmHandler::onControlMode, this, _1)); + // Subscribers with callback sub_operation_mode_availability_ = create_subscription( "~/input/operation_mode_availability", rclcpp::QoS{1}, std::bind(&MrmHandler::onOperationModeAvailability, this, _1)); - sub_mrm_pull_over_status_ = create_subscription( - "~/input/mrm/pull_over/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmPullOverStatus, this, _1)); - sub_mrm_comfortable_stop_status_ = create_subscription( - "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmComfortableStopStatus, this, _1)); - sub_mrm_emergency_stop_status_ = create_subscription( - "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmEmergencyStopStatus, this, _1)); - sub_operation_mode_state_ = create_subscription( - "~/input/api/operation_mode/state", rclcpp::QoS{1}, - std::bind(&MrmHandler::onOperationModeState, this, _1)); // Publisher pub_hazard_cmd_ = create_publisher( @@ -84,13 +67,6 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" client_mrm_emergency_stop_group_); // Initialize - odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - mrm_pull_over_status_ = std::make_shared(); - mrm_comfortable_stop_status_ = - std::make_shared(); - mrm_emergency_stop_status_ = std::make_shared(); - operation_mode_state_ = std::make_shared(); mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; @@ -102,17 +78,6 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" this, get_clock(), update_period_ns, std::bind(&MrmHandler::onTimer, this)); } -void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - odom_ = msg; -} - -void MrmHandler::onControlMode( - const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - control_mode_ = msg; -} - void MrmHandler::onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg) { @@ -141,30 +106,6 @@ void MrmHandler::onOperationModeAvailability( operation_mode_availability_ = msg; } -void MrmHandler::onMrmPullOverStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_pull_over_status_ = msg; -} - -void MrmHandler::onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_comfortable_stop_status_ = msg; -} - -void MrmHandler::onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_emergency_stop_status_ = msg; -} - -void MrmHandler::onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) -{ - operation_mode_state_ = msg; -} - void MrmHandler::publishHazardCmd() { using autoware_vehicle_msgs::msg::HazardLightsCommand; @@ -352,26 +293,21 @@ bool MrmHandler::isDataReady() return false; } - if ( - param_.use_pull_over && - mrm_pull_over_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_pull_over && !isPullOverStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm pull over to become available..."); return false; } - if ( - param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == - tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm comfortable stop to become available..."); return false; } - if ( - mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (!isEmergencyStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm emergency stop to become available..."); @@ -460,7 +396,7 @@ void MrmHandler::updateMrmState() } // Get mode - const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; + const bool is_auto_mode = isAutonomous(); // State Machine switch (mrm_state_.state) { @@ -581,14 +517,18 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB bool MrmHandler::isStopped() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_stopped_velocity = 0.001; - return std::abs((odom_->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity); + return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); } bool MrmHandler::isDrivingBackwards() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_moving_backwards = -0.001; - return odom_->twist.twist.linear.x < th_moving_backwards; + return odom->twist.twist.linear.x < th_moving_backwards; } bool MrmHandler::isEmergency() const @@ -597,11 +537,41 @@ bool MrmHandler::isEmergency() const is_operation_mode_availability_timeout; } +bool MrmHandler::isAutonomous() +{ + using autoware_vehicle_msgs::msg::ControlModeReport; + auto mode = sub_control_mode_.takeData(); + if (mode == nullptr) return false; + return mode->mode == ControlModeReport::AUTONOMOUS; +} + +bool MrmHandler::isPullOverStatusAvailable() +{ + auto status = sub_mrm_pull_over_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool MrmHandler::isComfortableStopStatusAvailable() +{ + auto status = sub_mrm_comfortable_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool MrmHandler::isEmergencyStopStatusAvailable() +{ + auto status = sub_mrm_emergency_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + bool MrmHandler::isArrivedAtGoal() { using autoware_adapi_v1_msgs::msg::OperationModeState; - - return operation_mode_state_->mode == OperationModeState::STOP; + auto state = sub_operation_mode_state_.takeData(); + if (state == nullptr) return false; + return state->mode == OperationModeState::STOP; } #include diff --git a/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml b/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml index a1ebddee73e2a..3c8a5c77b2e44 100644 --- a/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml +++ b/system/system_error_monitor/config/diagnostic_aggregator/control.param.yaml @@ -6,7 +6,7 @@ analyzers: autonomous_emergency_braking: type: diagnostic_aggregator/AnalyzerGroup - path: autonomous_emergency_braking + path: autoware_autonomous_emergency_braking analyzers: performance_monitoring: type: diagnostic_aggregator/AnalyzerGroup diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index d3fc221bac768..63fdbd0081af6 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -20,7 +20,7 @@ /autoware/control/autonomous_driving/node_alive_monitoring: default /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/localization/node_alive_monitoring: default /autoware/localization/performance_monitoring/scan_matching_status: { sf_at: "warn", lf_at: "none", spf_at: "none" } @@ -45,7 +45,7 @@ external_control: /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 43e8c56353a36..c9ab89a8fce96 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -20,7 +20,7 @@ /autoware/control/autonomous_driving/node_alive_monitoring: default /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/scan_matching_status: { sf_at: "warn", lf_at: "none", spf_at: "none" } @@ -46,7 +46,7 @@ external_control: /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + /autoware/control/autoware_autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/control/external_control/external_command_selector/node_alive_monitoring: default /autoware/system/node_alive_monitoring: default diff --git a/vehicle/autoware_steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml index 4246f8dee8a4e..08db4f5903abe 100644 --- a/vehicle/autoware_steer_offset_estimator/package.xml +++ b/vehicle/autoware_steer_offset_estimator/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto autoware_cmake + autoware_vehicle_info_utils autoware_vehicle_msgs diagnostic_updater geometry_msgs @@ -17,7 +18,6 @@ std_msgs tier4_autoware_utils tier4_debug_msgs - vehicle_info_util global_parameter_loader pose2twist diff --git a/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp b/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp index fdb5fdb404c0c..375e989f8f201 100644 --- a/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp +++ b/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp @@ -14,7 +14,7 @@ #include "autoware_steer_offset_estimator/steer_offset_estimator_node.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include #include @@ -26,7 +26,7 @@ SteerOffsetEstimatorNode::SteerOffsetEstimatorNode(const rclcpp::NodeOptions & n { using std::placeholders::_1; // get parameter - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo(); wheel_base_ = vehicle_info.wheel_base_m; covariance_ = this->declare_parameter("initial_covariance"); forgetting_factor_ = this->declare_parameter("forgetting_factor"); diff --git a/vehicle/vehicle_info_util/CMakeLists.txt b/vehicle/autoware_vehicle_info_utils/CMakeLists.txt similarity index 71% rename from vehicle/vehicle_info_util/CMakeLists.txt rename to vehicle/autoware_vehicle_info_utils/CMakeLists.txt index ca72b0ed2db65..6d090ab3ebff7 100644 --- a/vehicle/vehicle_info_util/CMakeLists.txt +++ b/vehicle/autoware_vehicle_info_utils/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.14) -project(vehicle_info_util) +project(autoware_vehicle_info_utils) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(vehicle_info_util SHARED +ament_auto_add_library(vehicle_info_utils SHARED src/vehicle_info.cpp - src/vehicle_info_util.cpp + src/vehicle_info_utils.cpp ) ament_auto_package( diff --git a/vehicle/vehicle_info_util/Readme.md b/vehicle/autoware_vehicle_info_utils/Readme.md similarity index 89% rename from vehicle/vehicle_info_util/Readme.md rename to vehicle/autoware_vehicle_info_utils/Readme.md index 600fd62270d81..537dc04aa8c69 100644 --- a/vehicle/vehicle_info_util/Readme.md +++ b/vehicle/autoware_vehicle_info_utils/Readme.md @@ -42,15 +42,15 @@ We have implemented a versioning system for the `vehicle_info.param.yaml` file t #### Minimum turning radius ```sh -$ ros2 run vehicle_info_util min_turning_radius_calculator.py -yaml path is /home/autoware/pilot-auto/install/vehicle_info_util/share/vehicle_info_util/config/vehicle_info.param.yaml +$ ros2 run autoware_vehicle_info_utils min_turning_radius_calculator.py +yaml path is /home/autoware/pilot-auto/install/autoware_vehicle_info_utils/share/autoware_vehicle_info_utils/config/vehicle_info.param.yaml Minimum turning radius is 3.253042620027102 [m] for rear, 4.253220695862465 [m] for front. ``` You can designate yaml file with `-y` option as follows. ```sh -ros2 run vehicle_info_util min_turning_radius_calculator.py -y +ros2 run autoware_vehicle_info_utils min_turning_radius_calculator.py -y ``` ## Assumptions / Known limits diff --git a/vehicle/vehicle_info_util/config/polygon_remover.yaml b/vehicle/autoware_vehicle_info_utils/config/polygon_remover.yaml similarity index 100% rename from vehicle/vehicle_info_util/config/polygon_remover.yaml rename to vehicle/autoware_vehicle_info_utils/config/polygon_remover.yaml diff --git a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml b/vehicle/autoware_vehicle_info_utils/config/vehicle_info.param.yaml similarity index 100% rename from vehicle/vehicle_info_util/config/vehicle_info.param.yaml rename to vehicle/autoware_vehicle_info_utils/config/vehicle_info.param.yaml diff --git a/vehicle/vehicle_info_util/config/vehicle_mirror.param.yaml b/vehicle/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml similarity index 100% rename from vehicle/vehicle_info_util/config/vehicle_mirror.param.yaml rename to vehicle/autoware_vehicle_info_utils/config/vehicle_mirror.param.yaml diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp similarity index 89% rename from vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp rename to vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp index cdbbe0427b145..c94ea01224da2 100644 --- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info.hpp +++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ -#define VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ +#ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ +#define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -namespace vehicle_info_util +namespace autoware::vehicle_info_utils { /// Data class for vehicle info struct VehicleInfo @@ -59,6 +59,6 @@ VehicleInfo createVehicleInfo( const double left_overhang_m, const double right_overhang_m, const double vehicle_height_m, const double max_steer_angle_rad); -} // namespace vehicle_info_util +} // namespace autoware::vehicle_info_utils -#endif // VEHICLE_INFO_UTIL__VEHICLE_INFO_HPP_ +#endif // AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_HPP_ diff --git a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info_util.hpp b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp similarity index 71% rename from vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info_util.hpp rename to vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp index 5e1623ada5ed3..d3ca339f78451 100644 --- a/vehicle/vehicle_info_util/include/vehicle_info_util/vehicle_info_util.hpp +++ b/vehicle/autoware_vehicle_info_utils/include/autoware_vehicle_info_utils/vehicle_info_utils.hpp @@ -12,23 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_ -#define VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_ +#ifndef AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_ +#define AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_ -#include "vehicle_info_util/vehicle_info.hpp" +#include "autoware_vehicle_info_utils/vehicle_info.hpp" #include -namespace vehicle_info_util +namespace autoware::vehicle_info_utils { /// This is a convenience class for saving you from declaring all parameters /// manually and calculating derived parameters. /// This class supposes that necessary parameters are set when the node is launched. -class VehicleInfoUtil +class VehicleInfoUtils { public: /// Constructor - explicit VehicleInfoUtil(rclcpp::Node & node); + explicit VehicleInfoUtils(rclcpp::Node & node); /// Get vehicle info VehicleInfo getVehicleInfo(); @@ -38,6 +38,6 @@ class VehicleInfoUtil VehicleInfo vehicle_info_; }; -} // namespace vehicle_info_util +} // namespace autoware::vehicle_info_utils -#endif // VEHICLE_INFO_UTIL__VEHICLE_INFO_UTIL_HPP_ +#endif // AUTOWARE_VEHICLE_INFO_UTILS__VEHICLE_INFO_UTILS_HPP_ diff --git a/vehicle/vehicle_info_util/launch/sample.launch.py b/vehicle/autoware_vehicle_info_utils/launch/sample.launch.py similarity index 91% rename from vehicle/vehicle_info_util/launch/sample.launch.py rename to vehicle/autoware_vehicle_info_utils/launch/sample.launch.py index c0be2bc4c4c48..5f83bc695230e 100644 --- a/vehicle/vehicle_info_util/launch/sample.launch.py +++ b/vehicle/autoware_vehicle_info_utils/launch/sample.launch.py @@ -30,14 +30,14 @@ def launch_setup(context, *args, **kwargs): set_use_sim_time = SetParameter(name="use_sim_time", value=LaunchConfiguration("use_sim_time")) vehicle_info_param_path = os.path.join( - get_package_share_directory("vehicle_info_util"), + get_package_share_directory("autoware_vehicle_info_utils"), "config", "vehicle_info.param.yaml", ) # vehicle_info load_vehicle_info = IncludeLaunchDescription( PythonLaunchDescriptionSource( - [FindPackageShare("vehicle_info_util"), "/launch/vehicle_info.launch.py"] + [FindPackageShare("autoware_vehicle_info_utils"), "/launch/vehicle_info.launch.py"] ), launch_arguments={"vehicle_info_param_file": [vehicle_info_param_path]}.items(), ) diff --git a/vehicle/vehicle_info_util/launch/vehicle_info.launch.py b/vehicle/autoware_vehicle_info_utils/launch/vehicle_info.launch.py similarity index 100% rename from vehicle/vehicle_info_util/launch/vehicle_info.launch.py rename to vehicle/autoware_vehicle_info_utils/launch/vehicle_info.launch.py diff --git a/vehicle/vehicle_info_util/package.xml b/vehicle/autoware_vehicle_info_utils/package.xml similarity index 88% rename from vehicle/vehicle_info_util/package.xml rename to vehicle/autoware_vehicle_info_utils/package.xml index 139f8439d60c9..bd8ec0cb601a0 100644 --- a/vehicle/vehicle_info_util/package.xml +++ b/vehicle/autoware_vehicle_info_utils/package.xml @@ -1,9 +1,9 @@ - vehicle_info_util + autoware_vehicle_info_utils 0.1.0 - The vehicle_info_util package + The autoware_vehicle_info_utils package Taiki Tanaka diff --git a/vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py b/vehicle/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py similarity index 93% rename from vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py rename to vehicle/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py index 9e25b928c8b74..0b581786b25ad 100755 --- a/vehicle/vehicle_info_util/scripts/min_turning_radius_calculator.py +++ b/vehicle/autoware_vehicle_info_utils/scripts/min_turning_radius_calculator.py @@ -41,7 +41,8 @@ def main(yaml_path): if __name__ == "__main__": default_yaml_path = ( - get_package_share_directory("vehicle_info_util") + "/config/vehicle_info.param.yaml" + get_package_share_directory("autoware_vehicle_info_utils") + + "/config/vehicle_info.param.yaml" ) parser = argparse.ArgumentParser() diff --git a/vehicle/vehicle_info_util/src/vehicle_info.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp similarity index 95% rename from vehicle/vehicle_info_util/src/vehicle_info.cpp rename to vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp index 1a47252f82d04..c7f587d20b2f4 100644 --- a/vehicle/vehicle_info_util/src/vehicle_info.cpp +++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "vehicle_info_util/vehicle_info.hpp" +#include "autoware_vehicle_info_utils/vehicle_info.hpp" -namespace vehicle_info_util +namespace autoware::vehicle_info_utils { tier4_autoware_utils::LinearRing2d VehicleInfo::createFootprint(const double margin) const { @@ -85,4 +85,4 @@ VehicleInfo createVehicleInfo( }; } -} // namespace vehicle_info_util +} // namespace autoware::vehicle_info_utils diff --git a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp b/vehicle/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp similarity index 90% rename from vehicle/vehicle_info_util/src/vehicle_info_util.cpp rename to vehicle/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp index 262a4b21c0cf7..e313391f6207a 100644 --- a/vehicle/vehicle_info_util/src/vehicle_info_util.cpp +++ b/vehicle/autoware_vehicle_info_utils/src/vehicle_info_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "vehicle_info_util/vehicle_info_util.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" #include @@ -36,9 +36,9 @@ T getParameter(rclcpp::Node & node, const std::string & name) } } // namespace -namespace vehicle_info_util +namespace autoware::vehicle_info_utils { -VehicleInfoUtil::VehicleInfoUtil(rclcpp::Node & node) +VehicleInfoUtils::VehicleInfoUtils(rclcpp::Node & node) { vehicle_info_.wheel_radius_m = getParameter(node, "wheel_radius"); vehicle_info_.wheel_width_m = getParameter(node, "wheel_width"); @@ -52,7 +52,7 @@ VehicleInfoUtil::VehicleInfoUtil(rclcpp::Node & node) vehicle_info_.max_steer_angle_rad = getParameter(node, "max_steer_angle"); } -VehicleInfo VehicleInfoUtil::getVehicleInfo() +VehicleInfo VehicleInfoUtils::getVehicleInfo() { return createVehicleInfo( vehicle_info_.wheel_radius_m, vehicle_info_.wheel_width_m, vehicle_info_.wheel_base_m, @@ -60,4 +60,4 @@ VehicleInfo VehicleInfoUtil::getVehicleInfo() vehicle_info_.left_overhang_m, vehicle_info_.right_overhang_m, vehicle_info_.vehicle_height_m, vehicle_info_.max_steer_angle_rad); } -} // namespace vehicle_info_util +} // namespace autoware::vehicle_info_utils