diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 278329cd3c725..084349cda0af3 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_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,22 +144,31 @@ 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_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/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_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_intersection_module/** kyoichi.sugahara@tier4.jp mamo 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_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/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/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/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/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/surround_obstacle_checker/** satoshi.ota@tier4.jp +planning/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,9 @@ 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/steer_offset_estimator/** taiki.tanaka@tier4.jp vehicle/vehicle_info_util/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp ### Copied from .github/CODEOWNERS-manual ### diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index fb98d321fde88..6745976aa543a 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -74,40 +74,3 @@ jobs: - name: Show disk space after the tasks run: df -h - - clang-tidy-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda - steps: - - name: Check out repository - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - - name: Show disk space before the tasks - run: df -h - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Get modified files - id: get-modified-files - uses: tj-actions/changed-files@v35 - with: - files: | - **/*.cpp - **/*.hpp - - - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 - with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy - build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index a469e7a33d190..85c531a02fca9 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -9,7 +9,7 @@ on: jobs: build-and-test: if: ${{ github.event_name != 'push' || github.ref_name == github.event.repository.default_branch }} - runs-on: ubuntu-latest + runs-on: [self-hosted, linux, X64] container: ${{ matrix.container }}${{ matrix.container-suffix }} strategy: fail-fast: false diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..f7ec81e3394e5 --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,51 @@ +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: + 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/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml deleted file mode 100644 index 4ecbd93198276..0000000000000 --- a/.github/workflows/openai-pr-reviewer.yaml +++ /dev/null @@ -1,39 +0,0 @@ -name: Code Review By ChatGPT - -permissions: - contents: read - pull-requests: write - -on: - pull_request_target: - types: - - labeled - pull_request_review_comment: - types: [created] - -concurrency: - group: ${{ github.repository }}-${{ github.event.number || github.head_ref || - github.sha }}-${{ github.workflow }}-${{ github.event_name == - 'pull_request_review_comment' && 'pr_comment' || 'pr' }} - cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }} - -jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 - with: - label: tag:openai-pr-reviewer - review: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: ubuntu-latest - steps: - - uses: fluxninja/openai-pr-reviewer@latest - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }} - with: - debug: false - review_simple_changes: false - review_comment_lgtm: false - openai_light_model: gpt-3.5-turbo-0613 - openai_heavy_model: gpt-3.5-turbo-0613 # The default is to use GPT4, which needs to be changed to join ChatGPT Plus. diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml index f4794b7497402..81b192a1c2095 100644 --- a/.github/workflows/pr-agent.yaml +++ b/.github/workflows/pr-agent.yaml @@ -26,6 +26,13 @@ jobs: env: OPENAI_KEY: ${{ secrets.OPENAI_KEY_PR_AGENT }} GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - github_action_config.auto_review: "false" - github_action_config.auto_describe: "false" - github_action_config.auto_improve: "false" + github_action_config.auto_review: false + github_action_config.auto_describe: false + github_action_config.auto_improve: false + # https://github.com/Codium-ai/pr-agent/blob/main/pr_agent/settings/configuration.toml + pr_description.publish_labels: false + config.model: gpt-4o + config.model_turbo: gpt-4o + config.max_model_tokens: 64000 + pr_code_suggestions.max_context_tokens: 12000 + pr_code_suggestions.commitable_code_suggestions: true diff --git a/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/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 98% 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..b87f421de7c8c 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,8 +12,8 @@ // 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 @@ -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 96% rename from control/autonomous_emergency_braking/package.xml rename to control/autoware_autonomous_emergency_braking/package.xml index bf27e6b7e1575..221fc2c000826 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 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..3ec8cc68df9b1 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 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 97% rename from control/mpc_lateral_controller/package.xml rename to control/autoware_mpc_lateral_controller/package.xml index 000bddc65aa1f..3e62d69f6ebb6 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 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 98% 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..3d66078eed1f2 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/autoware_mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -12,14 +12,14 @@ // 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 "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" 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..7b9d999a3decb 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,15 +12,15 @@ // 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 "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" @@ -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 97% rename from control/pid_longitudinal_controller/package.xml rename to control/autoware_pid_longitudinal_controller/package.xml index 82b297a69fee6..50ae7ccaf2e55 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 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..e1dd414b2446d 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" @@ -1148,7 +1148,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 92% 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..c9e83bf9878b8 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 97% rename from control/vehicle_cmd_gate/package.xml rename to control/autoware_vehicle_cmd_gate/package.xml index e01d566c37df9..da0e885cc564a 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 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 99% 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..e8d35beb94b1f 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 @@ -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 98% 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..22613de5d1f07 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/autoware_vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -20,11 +20,11 @@ #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "vehicle_cmd_filter.hpp" +#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 99% 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..6f148fe6c8140 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,7 +369,7 @@ 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"); 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/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 95% 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..d90dbc9474fd8 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,8 +12,8 @@ // 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 @@ -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; @@ -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 91% 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..917317361d702 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,8 +25,8 @@ // 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 @@ -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..7ea2e8b2de60a 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..466dc7351cd9e 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 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..7956410fdf898 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) @@ -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/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index 99439aa7e3ab7..81517675281c3 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 diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index b4820aee5cec4..0747766af54c8 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 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_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/package.xml b/control/trajectory_follower_node/package.xml index 65446dfb3dd01..92b99e690931f 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -21,11 +21,11 @@ autoware_adapi_v1_msgs autoware_control_msgs + autoware_mpc_lateral_controller + autoware_pid_longitudinal_controller autoware_planning_msgs autoware_vehicle_msgs motion_utils - mpc_lateral_controller - pid_longitudinal_controller pure_pursuit rclcpp rclcpp_components 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/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/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..dea0bc1d7c442 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"), @@ -432,7 +432,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..cfa1a07d4ea45 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_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/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_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 b093bc6295e65..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,37 +100,37 @@ - @@ -261,7 +255,6 @@ - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index a44e46acb01a6..196e7b2c7262c 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 @@ - + @@ -232,7 +232,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..db5a9a0093e7c 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 @@ - + 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..f96cac4f017a6 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 @@ -27,7 +27,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index b26b6d4fb6276..e5def4c7aed0c 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,13 +58,15 @@ autoware_cmake autoware_behavior_velocity_planner + autoware_costmap_generator autoware_external_velocity_limit_selector autoware_path_optimizer autoware_planning_topic_converter + autoware_planning_validator autoware_remaining_distance_time_calculator + autoware_surround_obstacle_checker autoware_velocity_smoother behavior_path_planner - costmap_generator external_cmd_selector freespace_planner glog_component @@ -72,9 +74,7 @@ obstacle_cruise_planner obstacle_stop_planner planning_evaluator - planning_validator scenario_selector - surround_obstacle_checker ament_lint_auto autoware_lint_common 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/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index 2a586aa9cd049..ee63d9f43559a 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -14,6 +14,13 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTOR SingleThreadedExecutor ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_angular_velocity + test/test_angular_velocity.cpp + ) +endif() + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/README.md b/localization/pose2twist/README.md index 07d9c37b710fc..f1f7d6408fafb 100644 --- a/localization/pose2twist/README.md +++ b/localization/pose2twist/README.md @@ -5,7 +5,7 @@ This `pose2twist` calculates the velocity from the input pose history. In addition to the computed twist, this node outputs the linear-x and angular-z components as a float message to simplify debugging. The `twist.linear.x` is calculated as `sqrt(dx * dx + dy * dy + dz * dz) / dt`, and the values in the `y` and `z` fields are zero. -The `twist.angular` is calculated as `d_roll / dt`, `d_pitch / dt` and `d_yaw / dt` for each field. +The `twist.angular` is calculated as `relative_rotation_vector / dt` for each field. ## Inputs / Outputs diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index 459a62ea5cd13..d1ff6ee5ff8b6 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -21,14 +21,23 @@ #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +// Compute the relative rotation of q2 from q1 as a rotation vector +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2); + class Pose2Twist : public rclcpp::Node { public: explicit Pose2Twist(const rclcpp::NodeOptions & options); - ~Pose2Twist() = default; private: - void callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); + void callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr); rclcpp::Subscription::SharedPtr pose_sub_; diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index 4b98ec6c81ad4..cdde78ed7e357 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -14,12 +14,6 @@ #include "pose2twist/pose2twist_core.hpp" -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include @@ -38,35 +32,27 @@ Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose create_publisher("angular_z", durable_qos); // Note: this callback publishes topics above pose_sub_ = create_subscription( - "pose", queue_size, std::bind(&Pose2Twist::callbackPose, this, _1)); -} - -double calcDiffForRadian(const double lhs_rad, const double rhs_rad) -{ - double diff_rad = lhs_rad - rhs_rad; - if (diff_rad > M_PI) { - diff_rad = diff_rad - 2 * M_PI; - } else if (diff_rad < -M_PI) { - diff_rad = diff_rad + 2 * M_PI; - } - return diff_rad; + "pose", queue_size, std::bind(&Pose2Twist::callback_pose, this, _1)); } -// x: roll, y: pitch, z: yaw -geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +tf2::Quaternion get_quaternion(const geometry_msgs::msg::PoseStamped::SharedPtr & pose_stamped_ptr) { - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; + const auto & orientation = pose_stamped_ptr->pose.orientation; + return tf2::Quaternion{orientation.x, orientation.y, orientation.z, orientation.w}; } -geometry_msgs::msg::Vector3 getRPY(geometry_msgs::msg::PoseStamped::SharedPtr pose) +geometry_msgs::msg::Vector3 compute_relative_rotation_vector( + const tf2::Quaternion & q1, const tf2::Quaternion & q2) { - return getRPY(pose->pose); + // If we define q2 as the rotation obtained by applying dq after applying q1, + // then q2 = q1 * dq . + // Therefore, dq = q1.inverse() * q2 . + const tf2::Quaternion diff_quaternion = q1.inverse() * q2; + const tf2::Vector3 axis = diff_quaternion.getAxis() * diff_quaternion.getAngle(); + return geometry_msgs::msg::Vector3{}.set__x(axis.x()).set__y(axis.y()).set__z(axis.z()); } -geometry_msgs::msg::TwistStamped calcTwist( +geometry_msgs::msg::TwistStamped calc_twist( geometry_msgs::msg::PoseStamped::SharedPtr pose_a, geometry_msgs::msg::PoseStamped::SharedPtr pose_b) { @@ -79,18 +65,16 @@ geometry_msgs::msg::TwistStamped calcTwist( return twist; } - const auto pose_a_rpy = getRPY(pose_a); - const auto pose_b_rpy = getRPY(pose_b); + const auto pose_a_quaternion = get_quaternion(pose_a); + const auto pose_b_quaternion = get_quaternion(pose_b); geometry_msgs::msg::Vector3 diff_xyz; - geometry_msgs::msg::Vector3 diff_rpy; + const geometry_msgs::msg::Vector3 relative_rotation_vector = + compute_relative_rotation_vector(pose_a_quaternion, pose_b_quaternion); diff_xyz.x = pose_b->pose.position.x - pose_a->pose.position.x; diff_xyz.y = pose_b->pose.position.y - pose_a->pose.position.y; diff_xyz.z = pose_b->pose.position.z - pose_a->pose.position.z; - diff_rpy.x = calcDiffForRadian(pose_b_rpy.x, pose_a_rpy.x); - diff_rpy.y = calcDiffForRadian(pose_b_rpy.y, pose_a_rpy.y); - diff_rpy.z = calcDiffForRadian(pose_b_rpy.z, pose_a_rpy.z); geometry_msgs::msg::TwistStamped twist; twist.header = pose_b->header; @@ -99,34 +83,34 @@ geometry_msgs::msg::TwistStamped calcTwist( dt; twist.twist.linear.y = 0; twist.twist.linear.z = 0; - twist.twist.angular.x = diff_rpy.x / dt; - twist.twist.angular.y = diff_rpy.y / dt; - twist.twist.angular.z = diff_rpy.z / dt; + twist.twist.angular.x = relative_rotation_vector.x / dt; + twist.twist.angular.y = relative_rotation_vector.y / dt; + twist.twist.angular.z = relative_rotation_vector.z / dt; return twist; } -void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) +void Pose2Twist::callback_pose(geometry_msgs::msg::PoseStamped::SharedPtr pose_msg_ptr) { // TODO(YamatoAndo) check time stamp diff // TODO(YamatoAndo) check suddenly move // TODO(YamatoAndo) apply low pass filter - geometry_msgs::msg::PoseStamped::SharedPtr current_pose_msg = pose_msg_ptr; + const geometry_msgs::msg::PoseStamped::SharedPtr & current_pose_msg = pose_msg_ptr; static geometry_msgs::msg::PoseStamped::SharedPtr prev_pose_msg = current_pose_msg; - geometry_msgs::msg::TwistStamped twist_msg = calcTwist(prev_pose_msg, current_pose_msg); + geometry_msgs::msg::TwistStamped twist_msg = calc_twist(prev_pose_msg, current_pose_msg); prev_pose_msg = current_pose_msg; twist_msg.header.frame_id = "base_link"; twist_pub_->publish(twist_msg); tier4_debug_msgs::msg::Float32Stamped linear_x_msg; linear_x_msg.stamp = this->now(); - linear_x_msg.data = twist_msg.twist.linear.x; + linear_x_msg.data = static_cast(twist_msg.twist.linear.x); linear_x_pub_->publish(linear_x_msg); tier4_debug_msgs::msg::Float32Stamped angular_z_msg; angular_z_msg.stamp = this->now(); - angular_z_msg.data = twist_msg.twist.angular.z; + angular_z_msg.data = static_cast(twist_msg.twist.angular.z); angular_z_pub_->publish(angular_z_msg); } diff --git a/localization/pose2twist/test/test_angular_velocity.cpp b/localization/pose2twist/test/test_angular_velocity.cpp new file mode 100644 index 0000000000000..bf2ca0a3ba5c2 --- /dev/null +++ b/localization/pose2twist/test/test_angular_velocity.cpp @@ -0,0 +1,115 @@ +// Copyright 2022 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "pose2twist/pose2twist_core.hpp" + +#include + +// 1e-3 radian = 0.057 degrees +constexpr double acceptable_error = 1e-3; + +TEST(AngularVelocityFromQuaternion, CheckMultiplicationOrder) +{ + // If we define q2 as the rotation obtained by applying dq after applying q1, then q2 = q1 * dq . + // + // IT IS NOT q2 = dq * q1 . + // + // This test checks that the multiplication order is correct. + + const tf2::Vector3 target_vector(1, 2, 3); + // initial state + // Now, car is facing to the +x direction + // z + // ^ y + // | ^ + // | / + // | / + // car -> x + // + // + // + + tf2::Quaternion q1; + q1.setRPY(0., 0., M_PI / 2.); // yaw = 90 degrees + const tf2::Vector3 initially_rotated_vector = tf2::quatRotate(q1, target_vector); + // after applying q1 + // Now, car is facing to the +y direction + // z + // ^ + // | y + // | ^ + // | / + // <--car x + // + // + // + EXPECT_NEAR(initially_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.y(), 1., acceptable_error); + EXPECT_NEAR(initially_rotated_vector.z(), 3., acceptable_error); + + tf2::Quaternion dq; + dq.setRPY(0., M_PI / 2., 0.); // pitch = 90 degrees + const tf2::Vector3 finally_rotated_vector = tf2::quatRotate(q1 * dq, target_vector); + // after applying dq + // Now, car is facing to the -z direction + // z y + // ^ + // / + // / + // / + // <--car x + // | + // v + // + EXPECT_NEAR(finally_rotated_vector.x(), -2., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.y(), 3., acceptable_error); + EXPECT_NEAR(finally_rotated_vector.z(), -1., acceptable_error); + + // Failure case + { + const tf2::Vector3 false_rotated_vector = tf2::quatRotate(dq * q1, target_vector); + + EXPECT_FALSE(std::abs(false_rotated_vector.x() - (-2)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.y() - (3)) < acceptable_error); + EXPECT_FALSE(std::abs(false_rotated_vector.z() - (-1)) < acceptable_error); + } +} + +TEST(AngularVelocityFromQuaternion, CheckNumericalValidity) +{ + auto test = [](const tf2::Vector3 & expected_axis, const double expected_angle) -> void { + tf2::Quaternion expected_q; + expected_q.setRotation(expected_axis, expected_angle); + + // Create a random initial quaternion + tf2::Quaternion initial_q; + initial_q.setRPY(0.2, 0.3, 0.4); + + // Calculate the final quaternion by rotating the initial quaternion by the expected + // quaternion + const tf2::Quaternion final_q = initial_q * expected_q; + + // Calculate the relative rotation between the initial and final quaternion + const geometry_msgs::msg::Vector3 rotation_vector = + compute_relative_rotation_vector(initial_q, final_q); + + EXPECT_NEAR(rotation_vector.x, expected_axis.x() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.y, expected_axis.y() * expected_angle, acceptable_error); + EXPECT_NEAR(rotation_vector.z, expected_axis.z() * expected_angle, acceptable_error); + }; + + test(tf2::Vector3(1.0, 0.0, 0.0).normalized(), 0.1); // 0.1 radian = 5.7 degrees + test(tf2::Vector3(1.0, 1.0, 0.0).normalized(), -0.2); // 0.2 radian = 11.4 degrees + test(tf2::Vector3(1.0, 2.0, 3.0).normalized(), 0.3); // 0.3 radian = 17.2 degrees +} diff --git a/localization/pose_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/lidar_transfusion/CMakeLists.txt b/perception/lidar_transfusion/CMakeLists.txt new file mode 100644 index 0000000000000..c0e2d85f27e62 --- /dev/null +++ b/perception/lidar_transfusion/CMakeLists.txt @@ -0,0 +1,156 @@ +cmake_minimum_required(VERSION 3.14) +project(lidar_transfusion) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +add_compile_options(-Wno-deprecated-declarations) + +option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) + +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if(CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if(CUDA_VERBOSE) + message("CUDA is available!") + message("CUDA Libs: ${CUDA_LIBRARIES}") + message("CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif() + # Note: cublas_device was depreciated in CUDA version 9.2 + # https://forums.developer.nvidia.com/t/where-can-i-find-libcublas-device-so-or-libcublas-device-a/67251/4 + # In LibTorch, CUDA_cublas_device_LIBRARY is used. + unset(CUDA_cublas_device_LIBRARY CACHE) + set(CUDA_AVAIL ON) +else() + message("CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif() + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER nvinfer) +find_library(NVONNXPARSER nvonnxparser) +if(NVINFER AND NVONNXPARSER) + if(CUDA_VERBOSE) + message("TensorRT is available!") + message("NVINFER: ${NVINFER}") + message("NVONNXPARSER: ${NVONNXPARSER}") + endif() + set(TRT_AVAIL ON) +else() + message("TensorRT is NOT Available") + set(TRT_AVAIL OFF) +endif() + +# set flags for CUDNN availability +option(CUDNN_AVAIL "CUDNN available" OFF) +# try to find the CUDNN module +find_library(CUDNN_LIBRARY +NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} +PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} +PATH_SUFFIXES lib lib64 bin +DOC "CUDNN library." +) +if(CUDNN_LIBRARY) + if(CUDA_VERBOSE) + message(STATUS "CUDNN is available!") + message(STATUS "CUDNN_LIBRARY: ${CUDNN_LIBRARY}") + endif() + set(CUDNN_AVAIL ON) +else() + message("CUDNN is NOT Available") + set(CUDNN_AVAIL OFF) +endif() + +if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + include_directories( + include + ${CUDA_INCLUDE_DIRS} + ) + + if(CMAKE_BUILD_TYPE STREQUAL "Debug") + set(CMAKE_CUDA_FLAGS ${CMAKE_CUDA_FLAGS} "-g -G") + set(CUDA_NVCC_FLAGS "-g -G") + endif() + + ament_auto_add_library(transfusion_lib SHARED + lib/detection_class_remapper.cpp + lib/network/network_trt.cpp + lib/postprocess/non_maximum_suppression.cpp + lib/preprocess/voxel_generator.cpp + lib/preprocess/pointcloud_densification.cpp + lib/ros_utils.cpp + lib/transfusion_trt.cpp + ) + + cuda_add_library(transfusion_cuda_lib SHARED + lib/postprocess/circle_nms_kernel.cu + lib/postprocess/postprocess_kernel.cu + lib/preprocess/preprocess_kernel.cu + ) + + target_link_libraries(transfusion_lib + ${NVINFER} + ${NVONNXPARSER} + ${CUDA_LIBRARIES} + ${CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${CUDNN_LIBRARY} + transfusion_cuda_lib + ) + + target_include_directories(transfusion_lib + PUBLIC + $ + $ + ) + + # To suppress unknown-pragmas error. The root-cause is CUB library in CUDA 11.6. + # This issue was fixed by https://github.com/NVIDIA/cub/commit/7d608bf1dc14553e2fb219eabeed80b76621b6fe + target_include_directories(transfusion_lib + SYSTEM PUBLIC + ${CUDA_INCLUDE_DIRS} + ) + + ament_auto_add_library(lidar_transfusion_component SHARED + src/lidar_transfusion_node.cpp + ) + + target_link_libraries(lidar_transfusion_component + transfusion_lib + ) + + rclcpp_components_register_node(lidar_transfusion_component + PLUGIN "lidar_transfusion::LidarTransfusionNode" + EXECUTABLE lidar_transfusion_node + ) + + if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + endif() + + ament_auto_package( + INSTALL_TO_SHARE + launch + config + ) + +else() + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + + ament_auto_package( + INSTALL_TO_SHARE + launch + ) +endif() diff --git a/perception/lidar_transfusion/README.md b/perception/lidar_transfusion/README.md new file mode 100644 index 0000000000000..7974714720c32 --- /dev/null +++ b/perception/lidar_transfusion/README.md @@ -0,0 +1,113 @@ +# lidar_transfusion + +## Purpose + +The `lidar_transfusion` package is used for 3D object detection based on lidar data (x, y, z, intensity). + +## Inner-workings / Algorithms + +The implementation bases on TransFusion [1] work. It uses TensorRT library for data process and network inference. + +We trained the models using . + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| -------------------- | ------------------------------- | ----------------- | +| `~/input/pointcloud` | `sensor_msgs::msg::PointCloud2` | Input pointcloud. | + +### Output + +| Name | Type | Description | +| -------------------------------------- | ------------------------------------------------ | --------------------------- | +| `~/output/objects` | `autoware_perception_msgs::msg::DetectedObjects` | Detected objects. | +| `debug/cyclic_time_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Cyclic time (ms). | +| `debug/pipeline_latency_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Pipeline latency time (ms). | +| `debug/processing_time/preprocess_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Preprocess (ms). | +| `debug/processing_time/inference_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Inference time (ms). | +| `debug/processing_time/postprocess_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Postprocess time (ms). | +| `debug/processing_time/total_ms` | `tier4_debug_msgs::msg::Float64Stamped` | Total processing time (ms). | + +## Parameters + +### TransFusion + +{{ json_to_markdown("perception/lidar_transfusion/schema/transfusion.schema.json") }} + +### Detection class remapper + +{{ json_to_markdown("perception/lidar_transfusion/schema/detection_class_remapper.schema.json") }} + +### The `build_only` option + +The `lidar_transfusion` node has `build_only` option to build the TensorRT engine file from the ONNX file. +Although it is preferred to move all the ROS parameters in `.param.yaml` file in Autoware Universe, the `build_only` option is not moved to the `.param.yaml` file for now, because it may be used as a flag to execute the build as a pre-task. You can execute with the following command: + +```bash +ros2 launch lidar_transfusion lidar_transfusion.launch.xml build_only:=true +``` + +### The `log_level` option + +The default logging severity level for `lidar_transfusion` is `info`. For debugging purposes, the developer may decrease severity level using `log_level` parameter: + +```bash +ros2 launch lidar_transfusion lidar_transfusion.launch.xml log_level:=debug +``` + +## Assumptions / Known limits + +## Trained Models + +You can download the onnx format of trained models by clicking on the links below. + +- TransFusion: [transfusion.onnx](https://awf.ml.dev.web.auto/perception/models/transfusion/v1/transfusion.onnx) + +The model was trained in TIER IV's internal database (~11k lidar frames) for 20 epochs. + +### Changelog + +## (Optional) Error detection and handling + + + +## (Optional) Performance characterization + + + +## References/External links + +[1] Xuyang Bai, Zeyu Hu, Xinge Zhu, Qingqiu Huang, Yilun Chen, Hongbo Fu and Chiew-Lan Tai. "TransFusion: Robust LiDAR-Camera Fusion for 3D Object Detection with Transformers." arXiv preprint arXiv:2203.11496 (2022). + +[2] + +[3] + +[4] + +[5] + +## (Optional) Future extensions / Unimplemented parts + + diff --git a/perception/lidar_transfusion/config/detection_class_remapper.param.yaml b/perception/lidar_transfusion/config/detection_class_remapper.param.yaml new file mode 100644 index 0000000000000..baea087c96bca --- /dev/null +++ b/perception/lidar_transfusion/config/detection_class_remapper.param.yaml @@ -0,0 +1,38 @@ +/**: + ros__parameters: + allow_remapping_by_area_matrix: + # NOTE(kl): We turn all vehicles into trailers if they go over 3x12 [m^2]. + # NOTE(kl): We turn cars into trucks if they have an area between 2.2 x 5.5 and 3.0 * 12.0 [m^2] + # row: original class. column: class to remap to + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN + [0, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN + 0, 0, 1, 0, 1, 0, 0, 0, #CAR + 0, 0, 0, 0, 1, 0, 0, 0, #TRUCK + 0, 0, 0, 0, 1, 0, 0, 0, #BUS + 0, 0, 0, 0, 0, 0, 0, 0, #TRAILER + 0, 0, 0, 0, 0, 0, 0, 0, #MOTORBIKE + 0, 0, 0, 0, 0, 0, 0, 0, #BICYCLE + 0, 0, 0, 0, 0, 0, 0, 0] #PEDESTRIAN + + min_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 12.100, 0.000, 36.000, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 36.000, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN + + + max_area_matrix: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN + 0.000, 0.000, 36.000, 0.000, 999.999, 0.000, 0.000, 0.000, #CAR + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #TRUCK + 0.000, 0.000, 0.000, 0.000, 999.999, 0.000, 0.000, 0.000, #BUS + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #TRAILER + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #MOTORBIKE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #BICYCLE + 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] #PEDESTRIAN diff --git a/perception/lidar_transfusion/config/transfusion.param.yaml b/perception/lidar_transfusion/config/transfusion.param.yaml new file mode 100644 index 0000000000000..feabe359caf1f --- /dev/null +++ b/perception/lidar_transfusion/config/transfusion.param.yaml @@ -0,0 +1,20 @@ +/**: + ros__parameters: + # network + class_names: ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"] + trt_precision: fp16 + voxels_num: [5000, 30000, 60000] # [min, opt, max] + point_cloud_range: [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0] # [x_min, y_min, z_min, x_max, y_max, z_max] + voxel_size: [0.3, 0.3, 8.0] # [x, y, z] + onnx_path: "$(var model_path)/transfusion.onnx" + engine_path: "$(var model_path)/transfusion.engine" + # pre-process params + densification_num_past_frames: 1 + densification_world_frame_id: map + # post-process params + circle_nms_dist_threshold: 0.5 + iou_nms_target_class_names: ["CAR"] + iou_nms_search_distance_2d: 10.0 + iou_nms_threshold: 0.1 + yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] # refers to the class_names + score_threshold: 0.2 diff --git a/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp new file mode 100644 index 0000000000000..5c25a936d5392 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/cuda_utils.hpp @@ -0,0 +1,126 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright (c) 2020, NVIDIA CORPORATION. All rights reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a + * copy of this software and associated documentation files (the "Software"), + * to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, + * and/or sell copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL + * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER + * DEALINGS IN THE SOFTWARE. + */ + +/* + * This code is licensed under CC0 1.0 Universal (Public Domain). + * You can use this without any limitation. + * https://creativecommons.org/publicdomain/zero/1.0/deed.en + */ + +#ifndef LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ +#define LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ + +#include + +#include +#include +#include +#include + +#define CHECK_CUDA_ERROR(e) (cuda::check_error(e, __FILE__, __LINE__)) + +namespace cuda +{ +inline void check_error(const ::cudaError_t e, const char * f, int n) +{ + if (e != ::cudaSuccess) { + ::std::stringstream s; + s << ::cudaGetErrorName(e) << " (" << e << ")@" << f << "#L" << n << ": " + << ::cudaGetErrorString(e); + throw ::std::runtime_error{s.str()}; + } +} + +struct deleter +{ + void operator()(void * p) const { CHECK_CUDA_ERROR(::cudaFree(p)); } +}; + +template +using unique_ptr = ::std::unique_ptr; + +template +typename ::std::enable_if<::std::is_array::value, cuda::unique_ptr>::type make_unique( + const ::std::size_t n) +{ + using U = typename ::std::remove_extent::type; + U * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(U) * n)); + return cuda::unique_ptr{p}; +} + +template +cuda::unique_ptr make_unique() +{ + T * p; + CHECK_CUDA_ERROR(::cudaMalloc(reinterpret_cast(&p), sizeof(T))); + return cuda::unique_ptr{p}; +} + +constexpr size_t CUDA_ALIGN = 256; + +template +inline size_t get_size_aligned(size_t num_elem) +{ + size_t size = num_elem * sizeof(T); + size_t extra_align = 0; + if (size % CUDA_ALIGN != 0) { + extra_align = CUDA_ALIGN - size % CUDA_ALIGN; + } + return size + extra_align; +} + +template +inline T * get_next_ptr(size_t num_elem, void *& workspace, size_t & workspace_size) +{ + size_t size = get_size_aligned(num_elem); + if (size > workspace_size) { + throw ::std::runtime_error("Workspace is too small!"); + } + workspace_size -= size; + T * ptr = reinterpret_cast(workspace); + workspace = reinterpret_cast(reinterpret_cast(workspace) + size); + return ptr; +} + +template +void clear_async(T * ptr, size_t num_elem, cudaStream_t stream) +{ + CHECK_CUDA_ERROR(::cudaMemsetAsync(ptr, 0, sizeof(T) * num_elem, stream)); +} + +} // namespace cuda + +#endif // LIDAR_TRANSFUSION__CUDA_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp b/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp new file mode 100644 index 0000000000000..ace7dc5ff3405 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/detection_class_remapper.hpp @@ -0,0 +1,47 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ +#define LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ + +#include + +#include +#include +#include +#include + +#include + +namespace lidar_transfusion +{ + +class DetectionClassRemapper +{ +public: + void setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix); + void mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg); + +protected: + Eigen::Matrix allow_remapping_by_area_matrix_; + Eigen::MatrixXd min_area_matrix_; + Eigen::MatrixXd max_area_matrix_; + int num_labels_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__DETECTION_CLASS_REMAPPER_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp new file mode 100644 index 0000000000000..86303618d7877 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/lidar_transfusion_node.hpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ +#define LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ + +#include "lidar_transfusion/detection_class_remapper.hpp" +#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/transfusion_trt.hpp" +#include "lidar_transfusion/visibility_control.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace lidar_transfusion +{ + +class LIDAR_TRANSFUSION_PUBLIC LidarTransfusionNode : public rclcpp::Node +{ +public: + explicit LidarTransfusionNode(const rclcpp::NodeOptions & options); + +private: + void cloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + + rclcpp::Subscription::ConstSharedPtr cloud_sub_{nullptr}; + rclcpp::Publisher::SharedPtr objects_pub_{ + nullptr}; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_{tf_buffer_}; + + DetectionClassRemapper detection_class_remapper_; + float score_threshold_{0.0}; + std::vector class_names_; + + NonMaximumSuppression iou_bev_nms_; + + std::unique_ptr detector_ptr_{nullptr}; + + // debugger + std::unique_ptr> stop_watch_ptr_{ + nullptr}; + std::unique_ptr debug_publisher_ptr_{nullptr}; + std::unique_ptr published_time_pub_{nullptr}; +}; +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__LIDAR_TRANSFUSION_NODE_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp new file mode 100644 index 0000000000000..3f8f32e346b0a --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/network/network_trt.hpp @@ -0,0 +1,87 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ +#define LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ + +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +struct ProfileDimension +{ + nvinfer1::Dims min; + nvinfer1::Dims opt; + nvinfer1::Dims max; + + bool operator!=(const ProfileDimension & rhs) const + { + return min.nbDims != rhs.min.nbDims || opt.nbDims != rhs.opt.nbDims || + max.nbDims != rhs.max.nbDims || !std::equal(min.d, min.d + min.nbDims, rhs.min.d) || + !std::equal(opt.d, opt.d + opt.nbDims, rhs.opt.d) || + !std::equal(max.d, max.d + max.nbDims, rhs.max.d); + } +}; + +class NetworkTRT +{ +public: + explicit NetworkTRT(const TransfusionConfig & config); + ~NetworkTRT(); + + bool init( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision); + const char * getTensorName(NetworkIO name); + + tensorrt_common::TrtUniquePtr engine{nullptr}; + tensorrt_common::TrtUniquePtr context{nullptr}; + +private: + bool parseONNX( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision, + size_t workspace_size = (1ULL << 30)); + bool saveEngine(const std::string & engine_path); + bool loadEngine(const std::string & engine_path); + bool createContext(); + bool setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config); + bool validateNetworkIO(); + nvinfer1::Dims validateTensorShape(NetworkIO name, const std::vector shape); + + tensorrt_common::TrtUniquePtr runtime_{nullptr}; + tensorrt_common::TrtUniquePtr plan_{nullptr}; + tensorrt_common::Logger logger_; + TransfusionConfig config_; + std::vector tensors_names_; + + std::array in_profile_dims_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__NETWORK__NETWORK_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp new file mode 100644 index 0000000000000..e231482652b98 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/circle_nms_kernel.hpp @@ -0,0 +1,32 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ + +#include "lidar_transfusion/utils.hpp" + +#include + +namespace lidar_transfusion +{ +// Non-maximum suppression (NMS) uses the distance on the xy plane instead of +// intersection over union (IoU) to suppress overlapped objects. +std::size_t circleNMS( + thrust::device_vector & boxes3d, const float distance_threshold, + thrust::device_vector & keep_mask, cudaStream_t stream); + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__CIRCLE_NMS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp new file mode 100644 index 0000000000000..daf2cc7c1fac1 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/non_maximum_suppression.hpp @@ -0,0 +1,80 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ + +#include "lidar_transfusion/ros_utils.hpp" + +#include + +#include + +#include +#include + +namespace lidar_transfusion +{ +using autoware_perception_msgs::msg::DetectedObject; + +enum class NMS_TYPE { + IoU_BEV + // IoU_3D + // Distance_2D + // Distance_3D +}; + +struct NMSParams +{ + NMS_TYPE nms_type_{}; + std::vector target_class_names_{}; + double search_distance_2d_{}; + double iou_threshold_{}; + // double distance_threshold_{}; +}; + +std::vector classNamesToBooleanMask(const std::vector & class_names) +{ + std::vector mask; + constexpr std::size_t num_object_classification = 8; + mask.resize(num_object_classification); + for (const auto & class_name : class_names) { + const auto semantic_type = getSemanticType(class_name); + mask.at(semantic_type) = true; + } + + return mask; +} + +class NonMaximumSuppression +{ +public: + void setParameters(const NMSParams &); + + std::vector apply(const std::vector &); + +private: + bool isTargetLabel(const std::uint8_t); + + bool isTargetPairObject(const DetectedObject &, const DetectedObject &); + + Eigen::MatrixXd generateIoUMatrix(const std::vector &); + + NMSParams params_{}; + std::vector target_class_mask_{}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__NON_MAXIMUM_SUPPRESSION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp new file mode 100644 index 0000000000000..01435424aa248 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/postprocess/postprocess_kernel.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ + +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include +#include +#include + +#include + +namespace lidar_transfusion +{ + +class PostprocessCuda +{ +public: + explicit PostprocessCuda(const TransfusionConfig & config, cudaStream_t & stream); + + cudaError_t generateDetectedBoxes3D_launch( + const float * cls_output, const float * box_output, const float * dir_cls_output, + std::vector & det_boxes3d, cudaStream_t stream); + +private: + TransfusionConfig config_; + cudaStream_t stream_; + cudaStream_t stream_event_; + cudaEvent_t start_, stop_; + thrust::device_vector boxes3d_d_; + thrust::device_vector yaw_norm_thresholds_d_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__POSTPROCESS__POSTPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp new file mode 100644 index 0000000000000..25095f4dc9c0b --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/pointcloud_densification.hpp @@ -0,0 +1,101 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" + +#include +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +namespace lidar_transfusion +{ + +class DensificationParam +{ +public: + DensificationParam(const std::string & world_frame_id, const unsigned int num_past_frames) + : world_frame_id_(std::move(world_frame_id)), + pointcloud_cache_size_(num_past_frames + /*current frame*/ 1) + { + } + + std::string world_frame_id() const { return world_frame_id_; } + unsigned int pointcloud_cache_size() const { return pointcloud_cache_size_; } + +private: + std::string world_frame_id_; + unsigned int pointcloud_cache_size_{1}; +}; + +struct PointCloudWithTransform +{ + cuda::unique_ptr points_d{nullptr}; + std_msgs::msg::Header header; + size_t num_points{0}; + Eigen::Affine3f affine_past2world; +}; + +class PointCloudDensification +{ +public: + explicit PointCloudDensification(const DensificationParam & param, cudaStream_t & stream); + + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + + double getCurrentTimestamp() const { return current_timestamp_; } + Eigen::Affine3f getAffineWorldToCurrent() const { return affine_world2current_; } + std::list::iterator getPointCloudCacheIter() + { + return pointcloud_cache_.begin(); + } + bool isCacheEnd(std::list::iterator iter) + { + return iter == pointcloud_cache_.end(); + } + size_t getIdx(std::list::iterator iter) + { + return std::distance(pointcloud_cache_.begin(), iter); + } + size_t getCacheSize() + { + return std::distance(pointcloud_cache_.begin(), pointcloud_cache_.end()); + } + unsigned int pointcloud_cache_size() const { return param_.pointcloud_cache_size(); } + +private: + void enqueue(const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine); + void dequeue(); + + DensificationParam param_; + double current_timestamp_{0.0}; + Eigen::Affine3f affine_world2current_; + std::list pointcloud_cache_; + cudaStream_t stream_; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__POINTCLOUD_DENSIFICATION_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp new file mode 100644 index 0000000000000..c571990d84b51 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/preprocess_kernel.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +namespace lidar_transfusion +{ + +class PreprocessCuda +{ +public: + PreprocessCuda(const TransfusionConfig & config, cudaStream_t & stream); + + void generateVoxels( + float * points, unsigned int points_size, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs); + + cudaError_t generateVoxels_random_launch( + float * points, unsigned int points_size, unsigned int * mask, float * voxels); + + cudaError_t generateBaseFeatures_launch( + unsigned int * mask, float * voxels, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs); + + // cudaError_t generateVoxelsInput_launch( + // uint8_t * cloud_data, CloudInfo & cloud_info, unsigned int points_agg, unsigned int + // points_size, float time_lag, float * affine_transform, float * points); + + cudaError_t generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform, float * output_points); + +private: + TransfusionConfig config_; + cudaStream_t stream_; + cuda::unique_ptr mask_{nullptr}; + cuda::unique_ptr voxels_{nullptr}; + unsigned int mask_size_; + unsigned int voxels_size_; +}; +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__PREPROCESS_KERNEL_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp new file mode 100644 index 0000000000000..3e3660e238473 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/preprocess/voxel_generator.hpp @@ -0,0 +1,70 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ +#define LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/transfusion_config.hpp" +#include "lidar_transfusion/utils.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include +#include +#include + +namespace lidar_transfusion +{ +constexpr size_t AFF_MAT_SIZE = 16; // 4x4 matrix +constexpr size_t MAX_CLOUD_STEP_SIZE = 35; // PointXYZIRCADT + +class VoxelGenerator +{ +public: + explicit VoxelGenerator( + const DensificationParam & densification_param, const TransfusionConfig & config, + cudaStream_t & stream); + std::size_t generateSweepPoints( + const sensor_msgs::msg::PointCloud2 & msg, cuda::unique_ptr & points_d); + bool enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + void initCloudInfo(const sensor_msgs::msg::PointCloud2 & msg); + std::tuple getFieldInfo( + const sensor_msgs::msg::PointCloud2 & msg, const std::string & field_name); + +private: + std::unique_ptr pd_ptr_{nullptr}; + std::unique_ptr pre_ptr_{nullptr}; + TransfusionConfig config_; + cuda::unique_ptr cloud_data_d_{nullptr}; + cuda::unique_ptr affine_past2current_d_{nullptr}; + std::vector points_; + cudaStream_t stream_; + CloudInfo cloud_info_; + bool is_initialized_{false}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__PREPROCESS__VOXEL_GENERATOR_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp new file mode 100644 index 0000000000000..f013a02404a29 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/ros_utils.hpp @@ -0,0 +1,39 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__ROS_UTILS_HPP_ +#define LIDAR_TRANSFUSION__ROS_UTILS_HPP_ + +#include "lidar_transfusion/utils.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace lidar_transfusion +{ + +void box3DToDetectedObject( + const Box3D & box3d, const std::vector & class_names, + autoware_perception_msgs::msg::DetectedObject & obj); + +uint8_t getSemanticType(const std::string & class_name); + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__ROS_UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp new file mode 100644 index 0000000000000..31976de56a9da --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_config.hpp @@ -0,0 +1,161 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ +#define LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ + +#include +#include + +namespace lidar_transfusion +{ + +class TransfusionConfig +{ +public: + TransfusionConfig( + const std::vector & voxels_num, const std::vector & point_cloud_range, + const std::vector & voxel_size, const float circle_nms_dist_threshold, + const std::vector & yaw_norm_thresholds, const float score_threshold) + { + if (voxels_num.size() == 3) { + max_voxels_ = voxels_num[2]; + + voxels_num_[0] = voxels_num[0]; + voxels_num_[1] = voxels_num[1]; + voxels_num_[2] = voxels_num[2]; + + min_voxel_size_ = voxels_num[0]; + opt_voxel_size_ = voxels_num[1]; + max_voxel_size_ = voxels_num[2]; + + min_points_size_ = voxels_num[0]; + opt_points_size_ = voxels_num[1]; + max_points_size_ = voxels_num[2]; + + min_coors_size_ = voxels_num[0]; + opt_coors_size_ = voxels_num[1]; + max_coors_size_ = voxels_num[2]; + } + if (point_cloud_range.size() == 6) { + min_x_range_ = static_cast(point_cloud_range[0]); + min_y_range_ = static_cast(point_cloud_range[1]); + min_z_range_ = static_cast(point_cloud_range[2]); + max_x_range_ = static_cast(point_cloud_range[3]); + max_y_range_ = static_cast(point_cloud_range[4]); + max_z_range_ = static_cast(point_cloud_range[5]); + } + if (voxel_size.size() == 3) { + voxel_x_size_ = static_cast(voxel_size[0]); + voxel_y_size_ = static_cast(voxel_size[1]); + voxel_z_size_ = static_cast(voxel_size[2]); + } + if (score_threshold > 0.0) { + score_threshold_ = score_threshold; + } + if (circle_nms_dist_threshold > 0.0) { + circle_nms_dist_threshold_ = circle_nms_dist_threshold; + } + yaw_norm_thresholds_ = + std::vector(yaw_norm_thresholds.begin(), yaw_norm_thresholds.end()); + for (auto & yaw_norm_threshold : yaw_norm_thresholds_) { + yaw_norm_threshold = + (yaw_norm_threshold >= 0.0 && yaw_norm_threshold < 1.0) ? yaw_norm_threshold : 0.0; + } + grid_x_size_ = static_cast((max_x_range_ - min_x_range_) / voxel_x_size_); + grid_y_size_ = static_cast((max_y_range_ - min_y_range_) / voxel_y_size_); + grid_z_size_ = static_cast((max_z_range_ - min_z_range_) / voxel_z_size_); + } + + ///// INPUT PARAMETERS ///// + const std::size_t cloud_capacity_{1000000}; + ///// KERNEL PARAMETERS ///// + const std::size_t threads_for_voxel_{256}; // threads number for a block + const std::size_t points_per_voxel_{20}; + const std::size_t warp_size_{32}; // one warp(32 threads) for one pillar + const std::size_t pillars_per_block_{64}; // one thread deals with one pillar + // and a block has pillars_per_block threads + const std::size_t pillar_feature_size_{64}; + std::size_t max_voxels_{60000}; + + ///// NETWORK PARAMETERS ///// + const std::size_t batch_size_{1}; + const std::size_t num_classes_{5}; + const std::size_t num_point_feature_size_{5}; // x, y, z, intensity, lag + // the dimension of the input cloud + float min_x_range_{-76.8}; + float max_x_range_{76.8}; + float min_y_range_{-76.8}; + float max_y_range_{76.8}; + float min_z_range_{-3.0}; + float max_z_range_{5.0}; + // the size of a pillar + float voxel_x_size_{0.3}; + float voxel_y_size_{0.3}; + float voxel_z_size_{8.0}; + const std::size_t out_size_factor_{4}; + const std::size_t max_num_points_per_pillar_{points_per_voxel_}; + const std::size_t num_point_values_{4}; + const std::size_t num_proposals_{200}; + // the number of feature maps for pillar scatter + const std::size_t num_feature_scatter_{pillar_feature_size_}; + // the score threshold for classification + float score_threshold_{0.2}; + float circle_nms_dist_threshold_{0.5}; + std::vector yaw_norm_thresholds_{0.3, 0.3, 0.3, 0.3, 0.0}; + std::size_t max_num_pillars_{max_voxels_}; + const std::size_t pillar_points_bev_{max_num_points_per_pillar_ * max_num_pillars_}; + // the detected boxes result decode by (x, y, z, w, l, h, yaw) + const std::size_t num_box_values_{8}; + // the input size of the 2D backbone network + std::size_t grid_x_size_{512}; + std::size_t grid_y_size_{512}; + std::size_t grid_z_size_{1}; + // the output size of the 2D backbone network + std::size_t feature_x_size_{grid_x_size_ / out_size_factor_}; + std::size_t feature_y_size_{grid_y_size_ / out_size_factor_}; + + ///// RUNTIME DIMENSIONS ///// + std::vector voxels_num_{5000, 30000, 60000}; + // voxels + std::size_t min_voxel_size_{voxels_num_[0]}; + std::size_t opt_voxel_size_{voxels_num_[1]}; + std::size_t max_voxel_size_{voxels_num_[2]}; + + std::size_t min_point_in_voxel_size_{points_per_voxel_}; + std::size_t opt_point_in_voxel_size_{points_per_voxel_}; + std::size_t max_point_in_voxel_size_{points_per_voxel_}; + + std::size_t min_network_feature_size_{num_point_feature_size_}; + std::size_t opt_network_feature_size_{num_point_feature_size_}; + std::size_t max_network_feature_size_{num_point_feature_size_}; + + // num_points + std::size_t min_points_size_{voxels_num_[0]}; + std::size_t opt_points_size_{voxels_num_[1]}; + std::size_t max_points_size_{voxels_num_[2]}; + + // coors + std::size_t min_coors_size_{voxels_num_[0]}; + std::size_t opt_coors_size_{voxels_num_[1]}; + std::size_t max_coors_size_{voxels_num_[2]}; + + std::size_t min_coors_dim_size_{num_point_values_}; + std::size_t opt_coors_dim_size_{num_point_values_}; + std::size_t max_coors_dim_size_{num_point_values_}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__TRANSFUSION_CONFIG_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp new file mode 100644 index 0000000000000..a94c3c1871136 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/transfusion_trt.hpp @@ -0,0 +1,114 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ +#define LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/network/network_trt.hpp" +#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/preprocess/voxel_generator.hpp" +#include "lidar_transfusion/utils.hpp" +#include "lidar_transfusion/visibility_control.hpp" + +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +class NetworkParam +{ +public: + NetworkParam(std::string onnx_path, std::string engine_path, std::string trt_precision) + : onnx_path_(std::move(onnx_path)), + engine_path_(std::move(engine_path)), + trt_precision_(std::move(trt_precision)) + { + } + + std::string onnx_path() const { return onnx_path_; } + std::string engine_path() const { return engine_path_; } + std::string trt_precision() const { return trt_precision_; } + +private: + std::string onnx_path_; + std::string engine_path_; + std::string trt_precision_; +}; + +class LIDAR_TRANSFUSION_PUBLIC TransfusionTRT +{ +public: + explicit TransfusionTRT( + const NetworkParam & network_param, const DensificationParam & densification_param, + const TransfusionConfig & config); + virtual ~TransfusionTRT(); + + bool detect( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, + std::vector & det_boxes3d, std::unordered_map & proc_timing); + +protected: + void initPtr(); + + bool preprocess(const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer); + + bool inference(); + + bool postprocess(std::vector & det_boxes3d); + + std::unique_ptr network_trt_ptr_{nullptr}; + std::unique_ptr vg_ptr_{nullptr}; + std::unique_ptr> stop_watch_ptr_{ + nullptr}; + std::unique_ptr pre_ptr_{nullptr}; + std::unique_ptr post_ptr_{nullptr}; + cudaStream_t stream_{nullptr}; + + TransfusionConfig config_; + + // input of pre-process + + unsigned int voxel_features_size_{0}; + unsigned int voxel_num_size_{0}; + unsigned int voxel_idxs_size_{0}; + unsigned int cls_size_{0}; + unsigned int box_size_{0}; + unsigned int dir_cls_size_{0}; + cuda::unique_ptr points_d_{nullptr}; + cuda::unique_ptr params_input_d_{nullptr}; + cuda::unique_ptr voxel_features_d_{nullptr}; + cuda::unique_ptr voxel_num_d_{nullptr}; + cuda::unique_ptr voxel_idxs_d_{nullptr}; + cuda::unique_ptr cls_output_d_{nullptr}; + cuda::unique_ptr box_output_d_{nullptr}; + cuda::unique_ptr dir_cls_output_d_{nullptr}; +}; + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__TRANSFUSION_TRT_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp new file mode 100644 index 0000000000000..e73fe7f055953 --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/utils.hpp @@ -0,0 +1,103 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__UTILS_HPP_ +#define LIDAR_TRANSFUSION__UTILS_HPP_ + +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +struct Box3D +{ + int label; + float score; + float x; + float y; + float z; + float width; + float length; + float height; + float yaw; +}; + +struct CloudInfo +{ + uint32_t x_offset; + uint32_t y_offset; + uint32_t z_offset; + uint32_t intensity_offset; + uint8_t x_datatype; + uint8_t y_datatype; + uint8_t z_datatype; + uint8_t intensity_datatype; + uint8_t x_count; + uint8_t y_count; + uint8_t z_count; + uint8_t intensity_count; + uint32_t point_step; + bool is_bigendian; + + CloudInfo() + : x_offset(0), + y_offset(4), + z_offset(8), + intensity_offset(12), + x_datatype(7), + y_datatype(7), + z_datatype(7), + intensity_datatype(7), + x_count(1), + y_count(1), + z_count(1), + intensity_count(1), + point_step(16), + is_bigendian(false) + { + } + + bool operator!=(const CloudInfo & rhs) const + { + return x_offset != rhs.x_offset || y_offset != rhs.y_offset || z_offset != rhs.z_offset || + intensity_offset != rhs.intensity_offset || x_datatype != rhs.x_datatype || + y_datatype != rhs.y_datatype || z_datatype != rhs.z_datatype || + intensity_datatype != rhs.intensity_datatype || x_count != rhs.x_count || + y_count != rhs.y_count || z_count != rhs.z_count || + intensity_count != rhs.intensity_count || is_bigendian != rhs.is_bigendian; + } +}; + +enum NetworkIO { voxels = 0, num_points, coors, cls_score, dir_pred, bbox_pred, ENUM_SIZE }; + +// cspell: ignore divup +template +unsigned int divup(const T1 a, const T2 b) +{ + if (a == 0) { + throw std::runtime_error("A dividend of divup isn't positive."); + } + if (b == 0) { + throw std::runtime_error("A divisor of divup isn't positive."); + } + + return (a + b - 1) / b; +} + +} // namespace lidar_transfusion + +#endif // LIDAR_TRANSFUSION__UTILS_HPP_ diff --git a/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp b/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp new file mode 100644 index 0000000000000..aeab20906628a --- /dev/null +++ b/perception/lidar_transfusion/include/lidar_transfusion/visibility_control.hpp @@ -0,0 +1,37 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ +#define LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ + +//////////////////////////////////////////////////////////////////////////////// +#if defined(__WIN32) +#if defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#define LIDAR_TRANSFUSION_PUBLIC __declspec(dllexport) +#define LIDAR_TRANSFUSION_LOCAL +#else // defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#define LIDAR_TRANSFUSION_PUBLIC __declspec(dllimport) +#define LIDAR_TRANSFUSION_LOCAL +#endif // defined(LIDAR_TRANSFUSION_BUILDING_DLL) || defined(LIDAR_TRANSFUSION_EXPORTS) +#elif defined(__linux__) +#define LIDAR_TRANSFUSION_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_TRANSFUSION_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define LIDAR_TRANSFUSION_PUBLIC __attribute__((visibility("default"))) +#define LIDAR_TRANSFUSION_LOCAL __attribute__((visibility("hidden"))) +#else +#error "Unsupported Build Configuration" +#endif + +#endif // LIDAR_TRANSFUSION__VISIBILITY_CONTROL_HPP_ diff --git a/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml b/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml new file mode 100644 index 0000000000000..492eb8c4d59b7 --- /dev/null +++ b/perception/lidar_transfusion/launch/lidar_transfusion.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_transfusion/lib/detection_class_remapper.cpp b/perception/lidar_transfusion/lib/detection_class_remapper.cpp new file mode 100644 index 0000000000000..e093637402448 --- /dev/null +++ b/perception/lidar_transfusion/lib/detection_class_remapper.cpp @@ -0,0 +1,71 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +namespace lidar_transfusion +{ + +void DetectionClassRemapper::setParameters( + const std::vector & allow_remapping_by_area_matrix, + const std::vector & min_area_matrix, const std::vector & max_area_matrix) +{ + assert(allow_remapping_by_area_matrix.size() == min_area_matrix.size()); + assert(allow_remapping_by_area_matrix.size() == max_area_matrix.size()); + assert( + std::pow(static_cast(std::sqrt(min_area_matrix.size())), 2) == min_area_matrix.size()); + + num_labels_ = static_cast(std::sqrt(min_area_matrix.size())); + + Eigen::Map> + allow_remapping_by_area_matrix_tmp( + allow_remapping_by_area_matrix.data(), num_labels_, num_labels_); + allow_remapping_by_area_matrix_ = allow_remapping_by_area_matrix_tmp.transpose() + .cast(); // Eigen is column major by default + + Eigen::Map min_area_matrix_tmp( + min_area_matrix.data(), num_labels_, num_labels_); + min_area_matrix_ = min_area_matrix_tmp.transpose(); // Eigen is column major by default + + Eigen::Map max_area_matrix_tmp( + max_area_matrix.data(), num_labels_, num_labels_); + max_area_matrix_ = max_area_matrix_tmp.transpose(); // Eigen is column major by default + + min_area_matrix_ = min_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); + max_area_matrix_ = max_area_matrix_.unaryExpr( + [](double v) { return std::isfinite(v) ? v : std::numeric_limits::max(); }); +} + +void DetectionClassRemapper::mapClasses(autoware_perception_msgs::msg::DetectedObjects & msg) +{ + for (auto & object : msg.objects) { + const float bev_area = object.shape.dimensions.x * object.shape.dimensions.y; + + for (auto & classification : object.classification) { + auto & label = classification.label; + + for (int i = 0; i < num_labels_; ++i) { + if ( + allow_remapping_by_area_matrix_(label, i) && bev_area >= min_area_matrix_(label, i) && + bev_area <= max_area_matrix_(label, i)) { + label = i; + break; + } + } + } + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/network/network_trt.cpp b/perception/lidar_transfusion/lib/network/network_trt.cpp new file mode 100644 index 0000000000000..a36890facceec --- /dev/null +++ b/perception/lidar_transfusion/lib/network/network_trt.cpp @@ -0,0 +1,332 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/network/network_trt.hpp" + +#include + +#include +#include +#include + +namespace lidar_transfusion +{ + +std::ostream & operator<<(std::ostream & os, const ProfileDimension & profile) +{ + std::string delim = ""; + os << "min->["; + for (int i = 0; i < profile.min.nbDims; ++i) { + os << delim << profile.min.d[i]; + delim = ", "; + } + os << "], opt->["; + delim = ""; + for (int i = 0; i < profile.opt.nbDims; ++i) { + os << delim << profile.opt.d[i]; + delim = ", "; + } + os << "], max->["; + delim = ""; + for (int i = 0; i < profile.max.nbDims; ++i) { + os << delim << profile.max.d[i]; + delim = ", "; + } + os << "]"; + return os; +} + +NetworkTRT::NetworkTRT(const TransfusionConfig & config) : config_(config) +{ + ProfileDimension voxels_dims = { + nvinfer1::Dims3( + config_.min_voxel_size_, config_.min_point_in_voxel_size_, config_.min_network_feature_size_), + nvinfer1::Dims3( + config_.opt_voxel_size_, config_.opt_point_in_voxel_size_, config_.opt_network_feature_size_), + nvinfer1::Dims3( + config_.max_voxel_size_, config_.max_point_in_voxel_size_, + config_.max_network_feature_size_)}; + ProfileDimension num_points_dims = { + nvinfer1::Dims{1, {static_cast(config_.min_points_size_)}}, + nvinfer1::Dims{1, {static_cast(config_.opt_points_size_)}}, + nvinfer1::Dims{1, {static_cast(config_.max_points_size_)}}}; + ProfileDimension coors_dims = { + nvinfer1::Dims2(config_.min_coors_size_, config_.min_coors_dim_size_), + nvinfer1::Dims2(config_.opt_coors_size_, config_.opt_coors_dim_size_), + nvinfer1::Dims2(config_.max_coors_size_, config_.max_coors_dim_size_)}; + in_profile_dims_ = {voxels_dims, num_points_dims, coors_dims}; +} + +NetworkTRT::~NetworkTRT() +{ + context.reset(); + runtime_.reset(); + plan_.reset(); + engine.reset(); +} + +bool NetworkTRT::init( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision) +{ + runtime_ = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); + if (!runtime_) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create runtime" << std::endl; + return false; + } + + bool success; + std::ifstream engine_file(engine_path); + if (engine_file.is_open()) { + success = loadEngine(engine_path); + } else { + auto log_thread = logger_.log_throttle( + nvinfer1::ILogger::Severity::kINFO, + "Applying optimizations and building TRT CUDA engine. Please wait a minutes...", 5); + success = parseONNX(onnx_path, engine_path, precision); + logger_.stop_throttle(log_thread); + } + success &= createContext(); + + return success; +} + +bool NetworkTRT::setProfile( + nvinfer1::IBuilder & builder, nvinfer1::INetworkDefinition & network, + nvinfer1::IBuilderConfig & config) +{ + auto profile = builder.createOptimizationProfile(); + + auto voxels_name = network.getInput(NetworkIO::voxels)->getName(); + auto num_points_name = network.getInput(NetworkIO::num_points)->getName(); + auto coors_name = network.getInput(NetworkIO::coors)->getName(); + + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::voxels].min); + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::voxels].opt); + profile->setDimensions( + voxels_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::voxels].max); + + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kMIN, + in_profile_dims_[NetworkIO::num_points].min); + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kOPT, + in_profile_dims_[NetworkIO::num_points].opt); + profile->setDimensions( + num_points_name, nvinfer1::OptProfileSelector::kMAX, + in_profile_dims_[NetworkIO::num_points].max); + + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kMIN, in_profile_dims_[NetworkIO::coors].min); + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kOPT, in_profile_dims_[NetworkIO::coors].opt); + profile->setDimensions( + coors_name, nvinfer1::OptProfileSelector::kMAX, in_profile_dims_[NetworkIO::coors].max); + + config.addOptimizationProfile(profile); + return true; +} + +bool NetworkTRT::createContext() +{ + if (!engine) { + tensorrt_common::LOG_ERROR(logger_) + << "Failed to create context: Engine was not created" << std::endl; + return false; + } + + context = + tensorrt_common::TrtUniquePtr(engine->createExecutionContext()); + if (!context) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create context" << std::endl; + return false; + } + + return true; +} + +bool NetworkTRT::parseONNX( + const std::string & onnx_path, const std::string & engine_path, const std::string & precision, + const size_t workspace_size) +{ + auto builder = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); + if (!builder) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create builder" << std::endl; + return false; + } + + auto config = + tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); + if (!config) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create config" << std::endl; + return false; + } +#if (NV_TENSORRT_MAJOR * 1000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 8400 + config->setMemoryPoolLimit(nvinfer1::MemoryPoolType::kWORKSPACE, workspace_size); +#else + config->setMaxWorkspaceSize(workspace_size); +#endif + if (precision == "fp16") { + if (builder->platformHasFastFp16()) { + tensorrt_common::LOG_INFO(logger_) << "Using TensorRT FP16 Inference" << std::endl; + config->setFlag(nvinfer1::BuilderFlag::kFP16); + } else { + tensorrt_common::LOG_INFO(logger_) + << "TensorRT FP16 Inference isn't supported in this environment" << std::endl; + } + } + + const auto flag = + 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); + auto network = + tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); + if (!network) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create network" << std::endl; + return false; + } + + auto parser = tensorrt_common::TrtUniquePtr( + nvonnxparser::createParser(*network, logger_)); + parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); + + if (!setProfile(*builder, *network, *config)) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to set profile" << std::endl; + return false; + } + + plan_ = tensorrt_common::TrtUniquePtr( + builder->buildSerializedNetwork(*network, *config)); + if (!plan_) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create serialized network" << std::endl; + return false; + } + engine = tensorrt_common::TrtUniquePtr( + runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); + if (!engine) { + tensorrt_common::LOG_ERROR(logger_) << "Failed to create engine" << std::endl; + return false; + } + + return saveEngine(engine_path); +} + +bool NetworkTRT::saveEngine(const std::string & engine_path) +{ + tensorrt_common::LOG_INFO(logger_) << "Writing to " << engine_path << std::endl; + std::ofstream file(engine_path, std::ios::out | std::ios::binary); + file.write(reinterpret_cast(plan_->data()), plan_->size()); + return validateNetworkIO(); +} + +bool NetworkTRT::loadEngine(const std::string & engine_path) +{ + std::ifstream engine_file(engine_path); + std::stringstream engine_buffer; + engine_buffer << engine_file.rdbuf(); + std::string engine_str = engine_buffer.str(); + engine = tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( + reinterpret_cast(engine_str.data()), engine_str.size())); + tensorrt_common::LOG_INFO(logger_) << "Loaded engine from " << engine_path << std::endl; + return validateNetworkIO(); +} + +bool NetworkTRT::validateNetworkIO() +{ + // Whether the number of IO match the expected size + if (engine->getNbIOTensors() != NetworkIO::ENUM_SIZE) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid network IO. Expected size: " << NetworkIO::ENUM_SIZE + << ". Actual size: " << engine->getNbIOTensors() << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + for (int i = 0; i < NetworkIO::ENUM_SIZE; ++i) { + tensors_names_.push_back(engine->getIOTensorName(i)); + } + + // Log the network IO + std::string tensors = std::accumulate( + tensors_names_.begin(), tensors_names_.end(), std::string(), + [](const std::string & a, const std::string & b) -> std::string { return a + b + " "; }); + tensorrt_common::LOG_INFO(logger_) << "Network IO: " << tensors << std::endl; + + // Whether the current engine input profile match the config input profile + for (int i = 0; i <= NetworkIO::coors; ++i) { + ProfileDimension engine_dims{ + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMIN), + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kOPT), + engine->getProfileShape(tensors_names_[i], 0, nvinfer1::OptProfileSelector::kMAX)}; + + tensorrt_common::LOG_INFO(logger_) + << "Profile for " << tensors_names_[i] << ": " << engine_dims << std::endl; + + if (engine_dims != in_profile_dims_[i]) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid network input dimension. Config: " << in_profile_dims_[i] + << ". Please change the input profile or delete the engine file and build engine again." + << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + } + + // Whether the IO tensor shapes match the network config, -1 for dynamic size + validateTensorShape( + NetworkIO::voxels, {-1, static_cast(config_.points_per_voxel_), + static_cast(config_.num_point_feature_size_)}); + validateTensorShape(NetworkIO::num_points, {-1}); + validateTensorShape(NetworkIO::coors, {-1, static_cast(config_.num_point_values_)}); + auto cls_score = validateTensorShape( + NetworkIO::cls_score, + {static_cast(config_.batch_size_), static_cast(config_.num_classes_), + static_cast(config_.num_proposals_)}); + tensorrt_common::LOG_INFO(logger_) << "Network num classes: " << cls_score.d[1] << std::endl; + validateTensorShape( + NetworkIO::dir_pred, + {static_cast(config_.batch_size_), 2, static_cast(config_.num_proposals_)}); // x, y + validateTensorShape( + NetworkIO::bbox_pred, + {static_cast(config_.batch_size_), static_cast(config_.num_box_values_), + static_cast(config_.num_proposals_)}); + + return true; +} + +const char * NetworkTRT::getTensorName(NetworkIO name) +{ + return tensors_names_.at(name); +} + +nvinfer1::Dims NetworkTRT::validateTensorShape(NetworkIO name, const std::vector shape) +{ + auto tensor_shape = engine->getTensorShape(tensors_names_[name]); + if (tensor_shape.nbDims != static_cast(shape.size())) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid tensor shape for " << tensors_names_[name] << ". Expected size: " << shape.size() + << ". Actual size: " << tensor_shape.nbDims << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + for (int i = 0; i < tensor_shape.nbDims; ++i) { + if (tensor_shape.d[i] != static_cast(shape[i])) { + tensorrt_common::LOG_ERROR(logger_) + << "Invalid tensor shape for " << tensors_names_[name] << ". Expected: " << shape[i] + << ". Actual: " << tensor_shape.d[i] << "." << std::endl; + throw std::runtime_error("Failed to initialize TRT network."); + } + } + return tensor_shape; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu b/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu new file mode 100644 index 0000000000000..dcb60831825bb --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/circle_nms_kernel.cu @@ -0,0 +1,144 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Modified from +// https://github.com/open-mmlab/OpenPCDet/blob/master/pcdet/ops/iou3d_nms/src/iou3d_nms_kernel.cu + +/* +3D IoU Calculation and Rotated NMS(modified from 2D NMS written by others) +Written by Shaoshuai Shi +All Rights Reserved 2019-2020. +*/ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "lidar_transfusion/utils.hpp" + +#include + +namespace +{ +const std::size_t THREADS_PER_BLOCK_NMS = 16; +} // namespace + +namespace lidar_transfusion +{ + +__device__ inline float dist2dPow(const Box3D * a, const Box3D * b) +{ + return powf(a->x - b->x, 2) + powf(a->y - b->y, 2); +} + +// cspell: ignore divup +__global__ void circleNMS_Kernel( + const Box3D * boxes, const std::size_t num_boxes3d, const std::size_t col_blocks, + const float dist2d_pow_threshold, std::uint64_t * mask) +{ + // params: boxes (N,) + // params: mask (N, divup(N/THREADS_PER_BLOCK_NMS)) + + const auto row_start = blockIdx.y; + const auto col_start = blockIdx.x; + + if (row_start > col_start) return; + + const std::size_t row_size = + fminf(num_boxes3d - row_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS); + const std::size_t col_size = + fminf(num_boxes3d - col_start * THREADS_PER_BLOCK_NMS, THREADS_PER_BLOCK_NMS); + + __shared__ Box3D block_boxes[THREADS_PER_BLOCK_NMS]; + + if (threadIdx.x < col_size) { + block_boxes[threadIdx.x] = boxes[THREADS_PER_BLOCK_NMS * col_start + threadIdx.x]; + } + __syncthreads(); + + if (threadIdx.x < row_size) { + const std::size_t cur_box_idx = THREADS_PER_BLOCK_NMS * row_start + threadIdx.x; + const Box3D * cur_box = boxes + cur_box_idx; + + std::uint64_t t = 0; + std::size_t start = 0; + if (row_start == col_start) { + start = threadIdx.x + 1; + } + for (std::size_t i = start; i < col_size; i++) { + if (dist2dPow(cur_box, block_boxes + i) < dist2d_pow_threshold) { + t |= 1ULL << i; + } + } + mask[cur_box_idx * col_blocks + col_start] = t; + } +} + +cudaError_t circleNMS_launch( + const thrust::device_vector & boxes3d, const std::size_t num_boxes3d, + std::size_t col_blocks, const float distance_threshold, + thrust::device_vector & mask, cudaStream_t stream) +{ + const float dist2d_pow_thres = powf(distance_threshold, 2); + + dim3 blocks(col_blocks, col_blocks); + dim3 threads(THREADS_PER_BLOCK_NMS); + circleNMS_Kernel<<>>( + thrust::raw_pointer_cast(boxes3d.data()), num_boxes3d, col_blocks, dist2d_pow_thres, + thrust::raw_pointer_cast(mask.data())); + + return cudaGetLastError(); +} + +std::size_t circleNMS( + thrust::device_vector & boxes3d, const float distance_threshold, + thrust::device_vector & keep_mask, cudaStream_t stream) +{ + const auto num_boxes3d = boxes3d.size(); + const auto col_blocks = divup(num_boxes3d, THREADS_PER_BLOCK_NMS); + thrust::device_vector mask_d(num_boxes3d * col_blocks); + + CHECK_CUDA_ERROR( + circleNMS_launch(boxes3d, num_boxes3d, col_blocks, distance_threshold, mask_d, stream)); + + // memcpy device to host + thrust::host_vector mask_h(mask_d.size()); + thrust::copy(mask_d.begin(), mask_d.end(), mask_h.begin()); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream)); + + // generate keep_mask + std::vector remv_h(col_blocks); + thrust::host_vector keep_mask_h(keep_mask.size()); + std::size_t num_to_keep = 0; + for (std::size_t i = 0; i < num_boxes3d; i++) { + auto nblock = i / THREADS_PER_BLOCK_NMS; + auto inblock = i % THREADS_PER_BLOCK_NMS; + + if (!(remv_h[nblock] & (1ULL << inblock))) { + keep_mask_h[i] = true; + num_to_keep++; + std::uint64_t * p = &mask_h[0] + i * col_blocks; + for (std::size_t j = nblock; j < col_blocks; j++) { + remv_h[j] |= p[j]; + } + } else { + keep_mask_h[i] = false; + } + } + + // memcpy host to device + keep_mask = keep_mask_h; + + return num_to_keep; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp new file mode 100644 index 0000000000000..c1179a3f6673d --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -0,0 +1,104 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/postprocess/non_maximum_suppression.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ + +void NonMaximumSuppression::setParameters(const NMSParams & params) +{ + assert(params.search_distance_2d_ >= 0.0); + assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0); + + params_ = params; + target_class_mask_ = classNamesToBooleanMask(params.target_class_names_); +} + +bool NonMaximumSuppression::isTargetLabel(const uint8_t label) +{ + if (label >= target_class_mask_.size()) { + return false; + } + return target_class_mask_.at(label); +} + +bool NonMaximumSuppression::isTargetPairObject( + const DetectedObject & object1, const DetectedObject & object2) +{ + const auto label1 = object_recognition_utils::getHighestProbLabel(object1.classification); + const auto label2 = object_recognition_utils::getHighestProbLabel(object2.classification); + + if (isTargetLabel(label1) && isTargetLabel(label2)) { + return true; + } + + const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; + const auto sqr_dist_2d = tier4_autoware_utils::calcSquaredDistance2d( + object_recognition_utils::getPose(object1), object_recognition_utils::getPose(object2)); + return sqr_dist_2d <= search_sqr_dist_2d; +} + +Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( + const std::vector & input_objects) +{ + // NOTE: row = target objects to be suppressed, col = source objects to be compared + Eigen::MatrixXd triangular_matrix = + Eigen::MatrixXd::Zero(input_objects.size(), input_objects.size()); + for (std::size_t target_i = 0; target_i < input_objects.size(); ++target_i) { + for (std::size_t source_i = 0; source_i < target_i; ++source_i) { + const auto & target_obj = input_objects.at(target_i); + const auto & source_obj = input_objects.at(source_i); + if (!isTargetPairObject(target_obj, source_obj)) { + continue; + } + + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + const double iou = object_recognition_utils::get2dIoU(target_obj, source_obj); + triangular_matrix(target_i, source_i) = iou; + // NOTE: If the target object has any objects with iou > iou_threshold, it + // will be suppressed regardless of later results. + if (iou > params_.iou_threshold_) { + break; + } + } + } + } + + return triangular_matrix; +} + +std::vector NonMaximumSuppression::apply( + const std::vector & input_objects) +{ + Eigen::MatrixXd iou_matrix = generateIoUMatrix(input_objects); + + std::vector output_objects; + output_objects.reserve(input_objects.size()); + for (std::size_t i = 0; i < input_objects.size(); ++i) { + const auto value = iou_matrix.row(i).maxCoeff(); + if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { + if (value <= params_.iou_threshold_) { + output_objects.emplace_back(input_objects.at(i)); + } + } + } + + return output_objects; +} +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu b/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu new file mode 100644 index 0000000000000..bca23e9961b40 --- /dev/null +++ b/perception/lidar_transfusion/lib/postprocess/postprocess_kernel.cu @@ -0,0 +1,145 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/postprocess/circle_nms_kernel.hpp" +#include "lidar_transfusion/postprocess/postprocess_kernel.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ +const size_t THREADS_PER_BLOCK = 256; + +struct is_score_greater +{ + is_score_greater(float t) : t_(t) {} + + __device__ bool operator()(const Box3D & b) { return b.score > t_; } + +private: + float t_{0.0}; +}; + +struct is_kept +{ + __device__ bool operator()(const bool keep) { return keep; } +}; + +struct score_greater +{ + __device__ bool operator()(const Box3D & lb, const Box3D & rb) { return lb.score > rb.score; } +}; + +__device__ inline float sigmoid(float x) +{ + return 1.0f / (1.0f + expf(-x)); +} + +__global__ void generateBoxes3D_kernel( + const float * __restrict__ cls_output, const float * __restrict__ box_output, + const float * __restrict__ dir_cls_output, const float voxel_size_x, const float voxel_size_y, + const float min_x_range, const float min_y_range, const int num_proposals, const int num_classes, + const int num_point_values, const float * __restrict__ yaw_norm_thresholds, + Box3D * __restrict__ det_boxes3d) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= num_proposals) { + return; + } + + int class_id = 0; + float max_score = cls_output[point_idx]; + +#pragma unroll + for (int i = 1; i < num_classes; i++) { + float score = cls_output[i * num_proposals + point_idx]; + if (score > max_score) { + max_score = score; + class_id = i; + } + } + + // yaw validation + const float yaw_sin = dir_cls_output[point_idx]; + const float yaw_cos = dir_cls_output[point_idx + num_proposals]; + const float yaw_norm = sqrtf(yaw_sin * yaw_sin + yaw_cos * yaw_cos); + + det_boxes3d[point_idx].label = class_id; + det_boxes3d[point_idx].score = yaw_norm >= yaw_norm_thresholds[class_id] ? max_score : 0.f; + det_boxes3d[point_idx].x = box_output[point_idx] * num_point_values * voxel_size_x + min_x_range; + det_boxes3d[point_idx].y = + box_output[point_idx + num_proposals] * num_point_values * voxel_size_y + min_y_range; + det_boxes3d[point_idx].z = box_output[point_idx + 2 * num_proposals]; + det_boxes3d[point_idx].width = expf(box_output[point_idx + 3 * num_proposals]); + det_boxes3d[point_idx].length = expf(box_output[point_idx + 4 * num_proposals]); + det_boxes3d[point_idx].height = expf(box_output[point_idx + 5 * num_proposals]); + det_boxes3d[point_idx].yaw = + atan2f(dir_cls_output[point_idx], dir_cls_output[point_idx + num_proposals]); +} + +PostprocessCuda::PostprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) +: config_(config), stream_(stream) +{ + boxes3d_d_ = thrust::device_vector(config_.num_proposals_); + yaw_norm_thresholds_d_ = thrust::device_vector( + config_.yaw_norm_thresholds_.begin(), config_.yaw_norm_thresholds_.end()); +} + +// cspell: ignore divup +cudaError_t PostprocessCuda::generateDetectedBoxes3D_launch( + const float * cls_output, const float * box_output, const float * dir_cls_output, + std::vector & det_boxes3d, cudaStream_t stream) +{ + dim3 threads = {THREADS_PER_BLOCK}; + dim3 blocks = {divup(config_.num_proposals_, threads.x)}; + + generateBoxes3D_kernel<<>>( + cls_output, box_output, dir_cls_output, config_.voxel_x_size_, config_.voxel_y_size_, + config_.min_x_range_, config_.min_y_range_, config_.num_proposals_, config_.num_classes_, + config_.num_point_values_, thrust::raw_pointer_cast(yaw_norm_thresholds_d_.data()), + thrust::raw_pointer_cast(boxes3d_d_.data())); + + // suppress by score + const auto num_det_boxes3d = thrust::count_if( + thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), + is_score_greater(config_.score_threshold_)); + if (num_det_boxes3d == 0) { + return cudaGetLastError(); + } + thrust::device_vector det_boxes3d_d(num_det_boxes3d); + thrust::copy_if( + thrust::device, boxes3d_d_.begin(), boxes3d_d_.end(), det_boxes3d_d.begin(), + is_score_greater(config_.score_threshold_)); + + // sort by score + thrust::sort(det_boxes3d_d.begin(), det_boxes3d_d.end(), score_greater()); + + // supress by NMS + thrust::device_vector final_keep_mask_d(num_det_boxes3d); + const auto num_final_det_boxes3d = + circleNMS(det_boxes3d_d, config_.circle_nms_dist_threshold_, final_keep_mask_d, stream); + thrust::device_vector final_det_boxes3d_d(num_final_det_boxes3d); + thrust::copy_if( + thrust::device, det_boxes3d_d.begin(), det_boxes3d_d.end(), final_keep_mask_d.begin(), + final_det_boxes3d_d.begin(), is_kept()); + + // memcpy device to host + det_boxes3d.resize(num_final_det_boxes3d); + thrust::copy(final_det_boxes3d_d.begin(), final_det_boxes3d_d.end(), det_boxes3d.begin()); + return cudaGetLastError(); +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp new file mode 100644 index 0000000000000..774b3a05d71c1 --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/pointcloud_densification.cpp @@ -0,0 +1,117 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/preprocess/pointcloud_densification.hpp" + +#include + +#include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +#include +#include + +namespace +{ + +boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & target_frame_id, + const std::string & source_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_transfusion"), ex.what()); + return boost::none; + } +} + +Eigen::Affine3f transformToEigen(const geometry_msgs::msg::Transform & t) +{ + Eigen::Affine3f a; + a.matrix() = tf2::transformToEigen(t).matrix().cast(); + return a; +} + +} // namespace + +namespace lidar_transfusion +{ + +PointCloudDensification::PointCloudDensification( + const DensificationParam & param, cudaStream_t & stream) +: param_(param), stream_(stream) +{ +} + +bool PointCloudDensification::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & pointcloud_msg, const tf2_ros::Buffer & tf_buffer) +{ + const auto header = pointcloud_msg.header; + + if (param_.pointcloud_cache_size() > 1) { + auto transform_world2current = + getTransform(tf_buffer, header.frame_id, param_.world_frame_id(), header.stamp); + if (!transform_world2current) { + return false; + } + auto affine_world2current = transformToEigen(transform_world2current.get()); + + enqueue(pointcloud_msg, affine_world2current); + } else { + enqueue(pointcloud_msg, Eigen::Affine3f::Identity()); + } + + dequeue(); + + return true; +} + +void PointCloudDensification::enqueue( + const sensor_msgs::msg::PointCloud2 & msg, const Eigen::Affine3f & affine_world2current) +{ + affine_world2current_ = affine_world2current; + current_timestamp_ = rclcpp::Time(msg.header.stamp).seconds(); + + assert(sizeof(uint8_t) * msg.width * msg.height * msg.point_step % sizeof(float) == 1); + auto points_d = cuda::make_unique( + sizeof(uint8_t) * msg.width * msg.height * msg.point_step / sizeof(float)); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + points_d.get(), msg.data.data(), sizeof(uint8_t) * msg.width * msg.height * msg.point_step, + cudaMemcpyHostToDevice, stream_)); + + PointCloudWithTransform pointcloud = { + std::move(points_d), msg.header, msg.width * msg.height, affine_world2current.inverse()}; + + pointcloud_cache_.push_front(std::move(pointcloud)); +} + +void PointCloudDensification::dequeue() +{ + if (pointcloud_cache_.size() > param_.pointcloud_cache_size()) { + pointcloud_cache_.pop_back(); + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu new file mode 100644 index 0000000000000..b8f9ae998fd4f --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/preprocess_kernel.cu @@ -0,0 +1,208 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. + * All rights reserved. SPDX-License-Identifier: Apache-2.0 + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "lidar_transfusion/cuda_utils.hpp" +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" + +namespace lidar_transfusion +{ + +PreprocessCuda::PreprocessCuda(const TransfusionConfig & config, cudaStream_t & stream) +: stream_(stream), config_(config) +{ + mask_size_ = config_.grid_z_size_ * config_.grid_y_size_ * config_.grid_x_size_; + voxels_size_ = config_.grid_z_size_ * config_.grid_y_size_ * config_.grid_x_size_ * + config_.max_num_points_per_pillar_ * config_.num_point_feature_size_ + + 1; + mask_ = cuda::make_unique(mask_size_); + voxels_ = cuda::make_unique(voxels_size_); +} + +void PreprocessCuda::generateVoxels( + float * points, unsigned int points_size, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs) +{ + cuda::clear_async(mask_.get(), mask_size_, stream_); + cuda::clear_async(voxels_.get(), voxels_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(generateVoxels_random_launch(points, points_size, mask_.get(), voxels_.get())); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(generateBaseFeatures_launch( + mask_.get(), voxels_.get(), pillar_num, voxel_features, voxel_num, voxel_idxs)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); +} + +__global__ void generateVoxels_random_kernel( + float * points, unsigned int points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, int points_per_voxel, unsigned int * mask, + float * voxels) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + float x = points[point_idx * 5]; + float y = points[point_idx * 5 + 1]; + float z = points[point_idx * 5 + 2]; + float i = points[point_idx * 5 + 3]; + float t = points[point_idx * 5 + 4]; + + if ( + x <= min_x_range || x >= max_x_range || y <= min_y_range || y >= max_y_range || + z <= min_z_range || z >= max_z_range) + return; + + int voxel_idx = floorf((x - min_x_range) / pillar_x_size); + int voxel_idy = floorf((y - min_y_range) / pillar_y_size); + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + + unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); + + if (point_id >= points_per_voxel) return; + float * address = voxels + (voxel_index * points_per_voxel + point_id) * 5; + atomicExch(address + 0, x); + atomicExch(address + 1, y); + atomicExch(address + 2, z); + atomicExch(address + 3, i); + atomicExch(address + 4, t); +} + +cudaError_t PreprocessCuda::generateVoxels_random_launch( + float * points, unsigned int points_size, unsigned int * mask, float * voxels) +{ + int threadNum = config_.threads_for_voxel_; + dim3 blocks((points_size + threadNum - 1) / threadNum); + dim3 threads(threadNum); + generateVoxels_random_kernel<<>>( + points, points_size, config_.min_x_range_, config_.max_x_range_, config_.min_y_range_, + config_.max_y_range_, config_.min_z_range_, config_.max_z_range_, config_.voxel_x_size_, + config_.voxel_y_size_, config_.voxel_z_size_, config_.grid_y_size_, config_.grid_x_size_, + config_.points_per_voxel_, mask, voxels); + cudaError_t err = cudaGetLastError(); + return err; +} + +__global__ void generateBaseFeatures_kernel( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, float points_per_voxel, + float max_voxels, unsigned int * pillar_num, float * voxel_features, unsigned int * voxel_num, + unsigned int * voxel_idxs) +{ + unsigned int voxel_idx = blockIdx.x * blockDim.x + threadIdx.x; + unsigned int voxel_idy = blockIdx.y * blockDim.y + threadIdx.y; + + if (voxel_idx >= grid_x_size || voxel_idy >= grid_y_size) return; + + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + unsigned int count = mask[voxel_index]; + if (!(count > 0)) return; + count = count < points_per_voxel ? count : points_per_voxel; + + unsigned int current_pillarId = 0; + current_pillarId = atomicAdd(pillar_num, 1); + if (current_pillarId >= max_voxels) return; + + voxel_num[current_pillarId] = count; + + uint4 idx = {0, 0, voxel_idy, voxel_idx}; + ((uint4 *)voxel_idxs)[current_pillarId] = idx; + + for (int i = 0; i < count; i++) { + int inIndex = voxel_index * points_per_voxel + i; + int outIndex = current_pillarId * points_per_voxel + i; + voxel_features[outIndex * 5] = voxels[inIndex * 5]; + voxel_features[outIndex * 5 + 1] = voxels[inIndex * 5 + 1]; + voxel_features[outIndex * 5 + 2] = voxels[inIndex * 5 + 2]; + voxel_features[outIndex * 5 + 3] = voxels[inIndex * 5 + 3]; + voxel_features[outIndex * 5 + 4] = voxels[inIndex * 5 + 4]; + } + + // clear buffer for next infer + atomicExch(mask + voxel_index, 0); +} + +// create 4 channels +cudaError_t PreprocessCuda::generateBaseFeatures_launch( + unsigned int * mask, float * voxels, unsigned int * pillar_num, float * voxel_features, + unsigned int * voxel_num, unsigned int * voxel_idxs) +{ + dim3 threads = {32, 32}; + dim3 blocks = {divup(config_.grid_x_size_, threads.x), divup(config_.grid_y_size_, threads.y)}; + + generateBaseFeatures_kernel<<>>( + mask, voxels, config_.grid_y_size_, config_.grid_x_size_, config_.points_per_voxel_, + config_.max_voxels_, pillar_num, voxel_features, voxel_num, voxel_idxs); + cudaError_t err = cudaGetLastError(); + return err; +} + +__global__ void generateSweepPoints_kernel( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, int num_features, float * output_points) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + const float input_x = input_points[point_idx * input_point_step + 0]; + const float input_y = input_points[point_idx * input_point_step + 1]; + const float input_z = input_points[point_idx * input_point_step + 2]; + const float intensity = input_points[point_idx * input_point_step + 3]; + + output_points[point_idx * num_features] = transform_array[0] * input_x + + transform_array[4] * input_y + + transform_array[8] * input_z + transform_array[12]; + output_points[point_idx * num_features + 1] = transform_array[1] * input_x + + transform_array[5] * input_y + + transform_array[9] * input_z + transform_array[13]; + output_points[point_idx * num_features + 2] = transform_array[2] * input_x + + transform_array[6] * input_y + + transform_array[10] * input_z + transform_array[14]; + output_points[point_idx * num_features + 3] = intensity; + output_points[point_idx * num_features + 4] = time_lag; +} + +cudaError_t PreprocessCuda::generateSweepPoints_launch( + const float * input_points, size_t points_size, int input_point_step, float time_lag, + const float * transform_array, float * output_points) +{ + int threadNum = config_.threads_for_voxel_; + dim3 blocks((points_size + threadNum - 1) / threadNum); + dim3 threads(threadNum); + + generateSweepPoints_kernel<<>>( + input_points, points_size, input_point_step, time_lag, transform_array, + config_.num_point_feature_size_, output_points); + + cudaError_t err = cudaGetLastError(); + return err; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp new file mode 100644 index 0000000000000..7072e8491af66 --- /dev/null +++ b/perception/lidar_transfusion/lib/preprocess/voxel_generator.cpp @@ -0,0 +1,142 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/preprocess/voxel_generator.hpp" + +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" + +#include + +#include + +namespace lidar_transfusion +{ + +std::ostream & operator<<(std::ostream & os, const CloudInfo & info) +{ + os << "x_offset: " << static_cast(info.x_offset) << std::endl; + os << "y_offset: " << static_cast(info.y_offset) << std::endl; + os << "z_offset: " << static_cast(info.z_offset) << std::endl; + os << "intensity_offset: " << static_cast(info.intensity_offset) << std::endl; + os << "x_datatype: " << static_cast(info.x_datatype) << std::endl; + os << "y_datatype: " << static_cast(info.y_datatype) << std::endl; + os << "z_datatype: " << static_cast(info.z_datatype) << std::endl; + os << "intensity_datatype: " << static_cast(info.intensity_datatype) << std::endl; + os << "x_count: " << static_cast(info.x_count) << std::endl; + os << "y_count: " << static_cast(info.y_count) << std::endl; + os << "z_count: " << static_cast(info.z_count) << std::endl; + os << "intensity_count: " << static_cast(info.intensity_count) << std::endl; + os << "point_step: " << static_cast(info.point_step) << std::endl; + os << "is_bigendian: " << static_cast(info.is_bigendian) << std::endl; + return os; +} + +VoxelGenerator::VoxelGenerator( + const DensificationParam & densification_param, const TransfusionConfig & config, + cudaStream_t & stream) +: config_(config), stream_(stream) +{ + pd_ptr_ = std::make_unique(densification_param, stream_); + pre_ptr_ = std::make_unique(config_, stream_); + cloud_data_d_ = cuda::make_unique(config_.cloud_capacity_ * MAX_CLOUD_STEP_SIZE); + affine_past2current_d_ = cuda::make_unique(AFF_MAT_SIZE); +} + +bool VoxelGenerator::enqueuePointCloud( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer) +{ + return pd_ptr_->enqueuePointCloud(msg, tf_buffer); +} + +std::size_t VoxelGenerator::generateSweepPoints( + const sensor_msgs::msg::PointCloud2 & msg, cuda::unique_ptr & points_d) +{ + if (!is_initialized_) { + initCloudInfo(msg); + std::stringstream ss; + ss << "Input point cloud information: " << std::endl << cloud_info_; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_transfusion"), ss.str()); + + CloudInfo default_cloud_info; + if (cloud_info_ != default_cloud_info) { + ss << "Expected point cloud information: " << std::endl << default_cloud_info; + RCLCPP_ERROR_STREAM(rclcpp::get_logger("lidar_transfusion"), ss.str()); + throw std::runtime_error("Input point cloud has unsupported format"); + } + is_initialized_ = true; + } + + size_t point_counter{0}; + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter); + pc_cache_iter++) { + auto sweep_num_points = pc_cache_iter->num_points; + if (point_counter + sweep_num_points >= config_.cloud_capacity_) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Exceeding cloud capacity. Used " + << pd_ptr_->getIdx(pc_cache_iter) << " out of " + << pd_ptr_->getCacheSize() << " sweep(s)"); + break; + } + auto shift = point_counter * config_.num_point_feature_size_; + + auto affine_past2current = + pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; + static_assert(std::is_same::value); + static_assert(!Eigen::Matrix4f::IsRowMajor, "matrices should be col-major."); + + float time_lag = static_cast( + pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_cache_iter->header.stamp).seconds()); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + affine_past2current_d_.get(), affine_past2current.data(), AFF_MAT_SIZE * sizeof(float), + cudaMemcpyHostToDevice, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + pre_ptr_->generateSweepPoints_launch( + pc_cache_iter->points_d.get(), sweep_num_points, cloud_info_.point_step / sizeof(float), + time_lag, affine_past2current_d_.get(), points_d.get() + shift); + point_counter += sweep_num_points; + } + + return point_counter; +} + +void VoxelGenerator::initCloudInfo(const sensor_msgs::msg::PointCloud2 & msg) +{ + std::tie(cloud_info_.x_offset, cloud_info_.x_datatype, cloud_info_.x_count) = + getFieldInfo(msg, "x"); + std::tie(cloud_info_.y_offset, cloud_info_.y_datatype, cloud_info_.y_count) = + getFieldInfo(msg, "y"); + std::tie(cloud_info_.z_offset, cloud_info_.z_datatype, cloud_info_.z_count) = + getFieldInfo(msg, "z"); + std::tie( + cloud_info_.intensity_offset, cloud_info_.intensity_datatype, cloud_info_.intensity_count) = + getFieldInfo(msg, "intensity"); + cloud_info_.point_step = msg.point_step; + cloud_info_.is_bigendian = msg.is_bigendian; +} + +std::tuple VoxelGenerator::getFieldInfo( + const sensor_msgs::msg::PointCloud2 & msg, const std::string & field_name) +{ + for (const auto & field : msg.fields) { + if (field.name == field_name) { + return std::make_tuple(field.offset, field.datatype, field.count); + } + } + throw std::runtime_error("Missing field: " + field_name); +} +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/ros_utils.cpp b/perception/lidar_transfusion/lib/ros_utils.cpp new file mode 100644 index 0000000000000..0ecb34faf732d --- /dev/null +++ b/perception/lidar_transfusion/lib/ros_utils.cpp @@ -0,0 +1,83 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/ros_utils.hpp" + +#include +#include +#include + +namespace lidar_transfusion +{ + +using Label = autoware_perception_msgs::msg::ObjectClassification; + +void box3DToDetectedObject( + const Box3D & box3d, const std::vector & class_names, + autoware_perception_msgs::msg::DetectedObject & obj) +{ + obj.existence_probability = box3d.score; + + // classification + autoware_perception_msgs::msg::ObjectClassification classification; + classification.probability = 1.0f; + if (box3d.label >= 0 && static_cast(box3d.label) < class_names.size()) { + classification.label = getSemanticType(class_names[box3d.label]); + } else { + classification.label = Label::UNKNOWN; + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Unexpected label: UNKNOWN is set."); + } + + if (object_recognition_utils::isCarLikeVehicle(classification.label)) { + obj.kinematics.orientation_availability = + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + } + + obj.classification.emplace_back(classification); + + // pose and shape + // mmdet3d yaw format to ros yaw format + float yaw = box3d.yaw + tier4_autoware_utils::pi / 2; + obj.kinematics.pose_with_covariance.pose.position = + tier4_autoware_utils::createPoint(box3d.x, box3d.y, box3d.z); + obj.kinematics.pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw); + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.dimensions = + tier4_autoware_utils::createTranslation(box3d.length, box3d.width, box3d.height); +} + +uint8_t getSemanticType(const std::string & class_name) +{ + if (class_name == "CAR") { + return Label::CAR; + } else if (class_name == "TRUCK") { + return Label::TRUCK; + } else if (class_name == "BUS") { + return Label::BUS; + } else if (class_name == "TRAILER") { + return Label::TRAILER; + } else if (class_name == "MOTORCYCLE") { + return Label::MOTORCYCLE; + } else if (class_name == "BICYCLE") { + return Label::BICYCLE; + } else if (class_name == "PEDESTRIAN") { + return Label::PEDESTRIAN; + } else { // CONSTRUCTION_VEHICLE, BARRIER, TRAFFIC_CONE + return Label::UNKNOWN; + } +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/lib/transfusion_trt.cpp b/perception/lidar_transfusion/lib/transfusion_trt.cpp new file mode 100644 index 0000000000000..c876c0906162b --- /dev/null +++ b/perception/lidar_transfusion/lib/transfusion_trt.cpp @@ -0,0 +1,205 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/transfusion_trt.hpp" + +#include "lidar_transfusion/preprocess/preprocess_kernel.hpp" +#include "lidar_transfusion/transfusion_config.hpp" + +#include + +#include +#include +#include +#include + +namespace lidar_transfusion +{ + +TransfusionTRT::TransfusionTRT( + const NetworkParam & network_param, const DensificationParam & densification_param, + const TransfusionConfig & config) +: config_(config) +{ + network_trt_ptr_ = std::make_unique(config_); + + network_trt_ptr_->init( + network_param.onnx_path(), network_param.engine_path(), network_param.trt_precision()); + vg_ptr_ = std::make_unique(densification_param, config_, stream_); + stop_watch_ptr_ = std::make_unique>(); + stop_watch_ptr_->tic("processing/inner"); + initPtr(); + + CHECK_CUDA_ERROR(cudaStreamCreate(&stream_)); +} + +TransfusionTRT::~TransfusionTRT() +{ + if (stream_) { + cudaStreamSynchronize(stream_); + cudaStreamDestroy(stream_); + } +} + +void TransfusionTRT::initPtr() +{ + // point cloud to voxels + voxel_features_size_ = + config_.max_voxels_ * config_.max_num_points_per_pillar_ * config_.num_point_feature_size_; + voxel_num_size_ = config_.max_voxels_; + voxel_idxs_size_ = config_.max_voxels_ * config_.num_point_values_; + + // output of TRT -- input of post-process + cls_size_ = config_.num_proposals_ * config_.num_classes_; + box_size_ = config_.num_proposals_ * config_.num_box_values_; + dir_cls_size_ = config_.num_proposals_ * 2; // x, y + cls_output_d_ = cuda::make_unique(cls_size_); + box_output_d_ = cuda::make_unique(box_size_); + dir_cls_output_d_ = cuda::make_unique(dir_cls_size_); + + params_input_d_ = cuda::make_unique(); + voxel_features_d_ = cuda::make_unique(voxel_features_size_); + voxel_num_d_ = cuda::make_unique(voxel_num_size_); + voxel_idxs_d_ = cuda::make_unique(voxel_idxs_size_); + points_d_ = cuda::make_unique(config_.cloud_capacity_ * config_.num_point_feature_size_); + pre_ptr_ = std::make_unique(config_, stream_); + post_ptr_ = std::make_unique(config_, stream_); +} + +bool TransfusionTRT::detect( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer, + std::vector & det_boxes3d, std::unordered_map & proc_timing) +{ + stop_watch_ptr_->toc("processing/inner", true); + if (!preprocess(msg, tf_buffer)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to preprocess and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/preprocess_ms", stop_watch_ptr_->toc("processing/inner", true)); + + if (!inference()) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to inference and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/inference_ms", stop_watch_ptr_->toc("processing/inner", true)); + + if (!postprocess(det_boxes3d)) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to postprocess and skip to detect."); + return false; + } + proc_timing.emplace( + "debug/processing_time/postprocess_ms", stop_watch_ptr_->toc("processing/inner", true)); + + return true; +} + +bool TransfusionTRT::preprocess( + const sensor_msgs::msg::PointCloud2 & msg, const tf2_ros::Buffer & tf_buffer) +{ + if (!vg_ptr_->enqueuePointCloud(msg, tf_buffer)) { + return false; + } + + cuda::clear_async(cls_output_d_.get(), cls_size_, stream_); + cuda::clear_async(box_output_d_.get(), box_size_, stream_); + cuda::clear_async(dir_cls_output_d_.get(), dir_cls_size_, stream_); + cuda::clear_async(voxel_features_d_.get(), voxel_features_size_, stream_); + cuda::clear_async(voxel_num_d_.get(), voxel_num_size_, stream_); + cuda::clear_async(voxel_idxs_d_.get(), voxel_idxs_size_, stream_); + cuda::clear_async(params_input_d_.get(), 1, stream_); + cuda::clear_async( + points_d_.get(), config_.cloud_capacity_ * config_.num_point_feature_size_, stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + const auto count = vg_ptr_->generateSweepPoints(msg, points_d_); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_transfusion"), "Generated sweep points: " << count); + + pre_ptr_->generateVoxels( + points_d_.get(), count, params_input_d_.get(), voxel_features_d_.get(), voxel_num_d_.get(), + voxel_idxs_d_.get()); + unsigned int params_input; + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + CHECK_CUDA_ERROR(cudaMemcpyAsync( + ¶ms_input, params_input_d_.get(), sizeof(unsigned int), cudaMemcpyDeviceToHost, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + if (params_input < config_.min_voxel_size_) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "Too few voxels (" << params_input << ") for actual optimization profile (" + << config_.min_voxel_size_ << ")"); + return false; + } + + if (params_input > config_.max_voxels_) { + params_input = config_.max_voxels_; + } + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Generated input voxels: " << params_input); + + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::voxels), voxel_features_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::voxels), + nvinfer1::Dims3{ + static_cast(params_input), static_cast(config_.max_num_points_per_pillar_), + static_cast(config_.num_point_feature_size_)}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::num_points), voxel_num_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::num_points), + nvinfer1::Dims{1, {static_cast(params_input)}}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::coors), voxel_idxs_d_.get()); + network_trt_ptr_->context->setInputShape( + network_trt_ptr_->getTensorName(NetworkIO::coors), + nvinfer1::Dims2{ + static_cast(params_input), static_cast(config_.num_point_values_)}); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::cls_score), cls_output_d_.get()); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::bbox_pred), box_output_d_.get()); + network_trt_ptr_->context->setTensorAddress( + network_trt_ptr_->getTensorName(NetworkIO::dir_pred), dir_cls_output_d_.get()); + return true; +} + +bool TransfusionTRT::inference() +{ + auto status = network_trt_ptr_->context->enqueueV3(stream_); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + + if (!status) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), "Fail to enqueue and skip to detect."); + return false; + } + return true; +} + +bool TransfusionTRT::postprocess(std::vector & det_boxes3d) +{ + CHECK_CUDA_ERROR(post_ptr_->generateDetectedBoxes3D_launch( + cls_output_d_.get(), box_output_d_.get(), dir_cls_output_d_.get(), det_boxes3d, stream_)); + CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + return true; +} + +} // namespace lidar_transfusion diff --git a/perception/lidar_transfusion/package.xml b/perception/lidar_transfusion/package.xml new file mode 100644 index 0000000000000..29643d0957eb1 --- /dev/null +++ b/perception/lidar_transfusion/package.xml @@ -0,0 +1,34 @@ + + + + lidar_transfusion + 1.0.0 + The lidar_transfusion package + Amadeusz Szymko + Kenzo Lobos-Tsunekawa + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_perception_msgs + launch_ros + object_recognition_utils + pcl_ros + rclcpp + rclcpp_components + tensorrt_common + tf2_eigen + tf2_geometry_msgs + tf2_sensor_msgs + tier4_autoware_utils + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/perception/lidar_transfusion/schema/detection_class_remapper.schema.json b/perception/lidar_transfusion/schema/detection_class_remapper.schema.json new file mode 100644 index 0000000000000..8aaefa295fcc0 --- /dev/null +++ b/perception/lidar_transfusion/schema/detection_class_remapper.schema.json @@ -0,0 +1,72 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Detection Class Remapper", + "type": "object", + "definitions": { + "detection_class_remapper": { + "type": "object", + "properties": { + "allow_remapping_by_area_matrix": { + "type": "array", + "items": { + "type": "integer" + }, + "description": "Whether to allow remapping of classes. The order of 8x8 matrix classes comes from ObjectClassification msg.", + "default": [ + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0 + ], + "minItems": 64, + "maxItems": 64 + }, + "min_area_matrix": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Minimum area for specific class to consider class remapping.", + "default": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 12.1, 0.0, 36.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 36.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ], + "minItems": 64, + "maxItems": 64 + }, + "max_area_matrix": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Maximum area for specific class to consider class remapping.", + "default": [ + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 36.0, 0.0, 999.999, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 999.999, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 999.999, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 + ], + "minItems": 64, + "maxItems": 64 + } + }, + "required": ["allow_remapping_by_area_matrix", "min_area_matrix", "max_area_matrix"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/detection_class_remapper" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/lidar_transfusion/schema/transfusion.schema.json b/perception/lidar_transfusion/schema/transfusion.schema.json new file mode 100644 index 0000000000000..41d8d887236a8 --- /dev/null +++ b/perception/lidar_transfusion/schema/transfusion.schema.json @@ -0,0 +1,160 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for LiDAR TransFusion Node", + "type": "object", + "definitions": { + "transfusion": { + "type": "object", + "properties": { + "class_names": { + "type": "array", + "items": { + "type": "string" + }, + "description": "An array of class names will be predicted.", + "default": ["CAR", "TRUCK", "BUS", "BICYCLE", "PEDESTRIAN"], + "uniqueItems": true + }, + "trt_precision": { + "type": "string", + "description": "A precision of TensorRT engine.", + "default": "fp16", + "enum": ["fp16", "fp32"] + }, + "voxels_num": { + "type": "array", + "items": { + "type": "integer" + }, + "description": "A maximum number of voxels [min, opt, max].", + "default": [5000, 30000, 60000] + }, + "point_cloud_range": { + "type": "array", + "items": { + "type": "number" + }, + "description": "An array of distance ranges of each class.", + "default": [-76.8, -76.8, -3.0, 76.8, 76.8, 5.0], + "minItems": 6, + "maxItems": 6 + }, + "voxel_size": { + "type": "array", + "items": { + "type": "number" + }, + "description": "Voxels size [x, y, z].", + "default": [0.3, 0.3, 8.0], + "minItems": 3, + "maxItems": 3 + }, + "onnx_path": { + "type": "string", + "description": "A path to ONNX model file.", + "default": "$(var model_path)/transfusion.onnx", + "pattern": "\\.onnx$" + }, + "engine_path": { + "type": "string", + "description": "A path to TensorRT engine file.", + "default": "$(var model_path)/transfusion.engine", + "pattern": "\\.engine$" + }, + "down_sample_factor": { + "type": "integer", + "description": "A scale factor of downsampling points", + "default": 1, + "minimum": 1 + }, + "densification_num_past_frames": { + "type": "integer", + "description": "A number of past frames to be considered as same input frame.", + "default": 1, + "minimum": 0 + }, + "densification_world_frame_id": { + "type": "string", + "description": "A name of frame id where world coordinates system is defined with respect to.", + "default": "map" + }, + "circle_nms_dist_threshold": { + "type": "number", + "description": "A distance threshold between detections in NMS.", + "default": 0.5, + "minimum": 0.0 + }, + "iou_nms_target_class_names": { + "type": "array", + "items": { + "type": "string" + }, + "description": "An array of class names to be target in NMS.", + "default": ["CAR"], + "uniqueItems": true + }, + "iou_nms_search_distance_2d": { + "type": "number", + "description": "A maximum distance value to search the nearest objects.", + "default": 10.0, + "minimum": 0.0 + }, + "iou_nms_threshold": { + "type": "number", + "description": "A threshold value of NMS using IoU score.", + "default": 0.1, + "minimum": 0.0, + "maximum": 1.0 + }, + "yaw_norm_thresholds": { + "type": "array", + "items": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0 + }, + "description": "A thresholds array of direction vectors norm, all of objects with vector norm less than this threshold are ignored.", + "default": [0.3, 0.3, 0.3, 0.3, 0.0] + }, + "score_threshold": { + "type": "number", + "description": "A threshold value of confidence score, all of objects with score less than this threshold are ignored.", + "default": 0.2, + "minimum": 0.0 + } + }, + "required": [ + "class_names", + "trt_precision", + "voxels_num", + "point_cloud_range", + "voxel_size", + "onnx_path", + "engine_path", + "densification_num_past_frames", + "densification_world_frame_id", + "circle_nms_dist_threshold", + "iou_nms_target_class_names", + "iou_nms_search_distance_2d", + "iou_nms_threshold", + "yaw_norm_thresholds", + "score_threshold" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/transfusion" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp new file mode 100644 index 0000000000000..9313015002973 --- /dev/null +++ b/perception/lidar_transfusion/src/lidar_transfusion_node.cpp @@ -0,0 +1,178 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +//    http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_transfusion/lidar_transfusion_node.hpp" + +#include "lidar_transfusion/utils.hpp" + +namespace lidar_transfusion +{ + +LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) +: Node("lidar_transfusion", options), tf_buffer_(this->get_clock()) +{ + auto descriptor = rcl_interfaces::msg::ParameterDescriptor{}.set__read_only(true); + // network + class_names_ = this->declare_parameter>("class_names", descriptor); + const std::string trt_precision = + this->declare_parameter("trt_precision", descriptor); + const auto voxels_num = this->declare_parameter>("voxels_num", descriptor); + const auto point_cloud_range = + this->declare_parameter>("point_cloud_range", descriptor); + const auto voxel_size = this->declare_parameter>("voxel_size", descriptor); + const std::string onnx_path = this->declare_parameter("onnx_path", descriptor); + const std::string engine_path = this->declare_parameter("engine_path", descriptor); + + if (point_cloud_range.size() != 6) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "The size of point_cloud_range != 6: use the default parameters."); + } + if (voxel_size.size() != 3) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger("lidar_transfusion"), + "The size of voxel_size != 3: use the default parameters."); + } + + // pre-process + const std::string densification_world_frame_id = + this->declare_parameter("densification_world_frame_id", descriptor); + const int densification_num_past_frames = + this->declare_parameter("densification_num_past_frames", descriptor); + + // post-process + const float circle_nms_dist_threshold = + static_cast(this->declare_parameter("circle_nms_dist_threshold", descriptor)); + { // IoU NMS + NMSParams p; + p.nms_type_ = NMS_TYPE::IoU_BEV; + p.target_class_names_ = + this->declare_parameter>("iou_nms_target_class_names", descriptor); + p.search_distance_2d_ = + this->declare_parameter("iou_nms_search_distance_2d", descriptor); + p.iou_threshold_ = this->declare_parameter("iou_nms_threshold", descriptor); + iou_bev_nms_.setParameters(p); + } + const auto yaw_norm_thresholds = + this->declare_parameter>("yaw_norm_thresholds", descriptor); + const float score_threshold = + static_cast(this->declare_parameter("score_threshold", descriptor)); + + NetworkParam network_param(onnx_path, engine_path, trt_precision); + DensificationParam densification_param( + densification_world_frame_id, densification_num_past_frames); + TransfusionConfig config( + voxels_num, point_cloud_range, voxel_size, circle_nms_dist_threshold, yaw_norm_thresholds, + score_threshold); + + const auto allow_remapping_by_area_matrix = + this->declare_parameter>("allow_remapping_by_area_matrix", descriptor); + const auto min_area_matrix = + this->declare_parameter>("min_area_matrix", descriptor); + const auto max_area_matrix = + this->declare_parameter>("max_area_matrix", descriptor); + detection_class_remapper_.setParameters( + allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + detector_ptr_ = std::make_unique(network_param, densification_param, config); + + cloud_sub_ = this->create_subscription( + "~/input/pointcloud", rclcpp::SensorDataQoS{}.keep_last(1), + std::bind(&LidarTransfusionNode::cloudCallback, this, std::placeholders::_1)); + + objects_pub_ = this->create_publisher( + "~/output/objects", rclcpp::QoS(1)); + + published_time_pub_ = std::make_unique(this); + + // initialize debug tool + { + using tier4_autoware_utils::DebugPublisher; + using tier4_autoware_utils::StopWatch; + stop_watch_ptr_ = std::make_unique>(); + debug_publisher_ptr_ = std::make_unique(this, this->get_name()); + stop_watch_ptr_->tic("cyclic"); + stop_watch_ptr_->tic("processing/total"); + } + + if (this->declare_parameter("build_only", false, descriptor)) { + RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); + rclcpp::shutdown(); + } +} + +void LidarTransfusionNode::cloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + const auto objects_sub_count = + objects_pub_->get_subscription_count() + objects_pub_->get_intra_process_subscription_count(); + if (objects_sub_count < 1) { + return; + } + + if (stop_watch_ptr_) { + stop_watch_ptr_->toc("processing/total", true); + } + + std::vector det_boxes3d; + std::unordered_map proc_timing; + bool is_success = detector_ptr_->detect(*msg, tf_buffer_, det_boxes3d, proc_timing); + if (!is_success) { + return; + } + + std::vector raw_objects; + raw_objects.reserve(det_boxes3d.size()); + for (const auto & box3d : det_boxes3d) { + autoware_perception_msgs::msg::DetectedObject obj; + box3DToDetectedObject(box3d, class_names_, obj); + raw_objects.emplace_back(obj); + } + + autoware_perception_msgs::msg::DetectedObjects output_msg; + output_msg.header = msg->header; + output_msg.objects = iou_bev_nms_.apply(raw_objects); + + detection_class_remapper_.mapClasses(output_msg); + + if (objects_sub_count > 0) { + objects_pub_->publish(output_msg); + published_time_pub_->publish_if_subscribed(objects_pub_, output_msg.header.stamp); + } + + // add processing time for debug + if (debug_publisher_ptr_ && stop_watch_ptr_) { + const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic", true); + const double processing_time_ms = stop_watch_ptr_->toc("processing/total", true); + const double pipeline_latency_ms = + std::chrono::duration( + std::chrono::nanoseconds( + (this->get_clock()->now() - output_msg.header.stamp).nanoseconds())) + .count(); + debug_publisher_ptr_->publish( + "debug/cyclic_time_ms", cyclic_time_ms); + debug_publisher_ptr_->publish( + "debug/pipeline_latency_ms", pipeline_latency_ms); + debug_publisher_ptr_->publish( + "debug/processing_time/total_ms", processing_time_ms); + for (const auto & [topic, time_ms] : proc_timing) { + debug_publisher_ptr_->publish(topic, time_ms); + } + } +} + +} // namespace lidar_transfusion + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(lidar_transfusion::LidarTransfusionNode) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 838d7b1c8e316..52e104f572f76 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -157,7 +157,7 @@ PredictedPath PathGenerator::generatePathForOnLaneVehicle( return generateStraightPath(object, duration); } - return generatePolynomialPath(object, ref_paths, speed_limit, duration, lateral_duration); + return generatePolynomialPath(object, ref_paths, duration, lateral_duration, speed_limit); } PredictedPath PathGenerator::generateStraightPath( 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 4966ff7dc0bb3..c144941662a5d 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/behavior_path_side_shift_module - - 'Start Planner': planning/behavior_path_start_planner_module + - 'Side Shift': planning/autoware_behavior_path_side_shift_module + - 'Start Planner': planning/autoware_behavior_path_start_planner_module - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner @@ -25,17 +25,16 @@ 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 - 'Run Out': planning/autoware_behavior_velocity_run_out_module - 'Speed Bump': planning/behavior_velocity_speed_bump_module - 'Stop Line': planning/behavior_velocity_stop_line_module - 'Traffic Light': planning/behavior_velocity_traffic_light_module - 'Virtual Traffic Light': planning/autoware_behavior_velocity_virtual_traffic_light_module - - 'Walkway': planning/behavior_velocity_walkway_module + - 'Walkway': planning/autoware_behavior_velocity_walkway_module - 'Parking': - 'Freespace Planner': - 'About Freespace Planner': planning/freespace_planner @@ -53,16 +52,16 @@ 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/surround_obstacle_checker - - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja + - 'About Surround Obstacle Checker': planning/autoware_surround_obstacle_checker + - 'About Surround Obstacle Checker (Japanese)': planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja - 'Motion Velocity Planner': - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ - 'Available Modules': @@ -77,7 +76,7 @@ nav: - '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_side_shift_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 86% rename from planning/behavior_path_side_shift_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 db1f5ee3560b1..ea7979958e49e 100644 --- a/planning/behavior_path_side_shift_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() @@ -48,9 +48,11 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_path_planner_dir = ament_index_cpp::get_package_share_directory("behavior_path_planner"); + const auto behavior_path_lane_change_module_dir = + ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); + module_names.emplace_back("autoware::behavior_path_planner::AvoidanceByLaneChangeModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -63,8 +65,13 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_side_shift_module") + - "/config/side_shift.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/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/package.xml b/planning/autoware_behavior_path_planner_common/package.xml index 21da8dcef1c2d..b6fb6a352e91e 100644 --- a/planning/autoware_behavior_path_planner_common/package.xml +++ b/planning/autoware_behavior_path_planner_common/package.xml @@ -43,12 +43,13 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_lane_departure_checker autoware_perception_msgs autoware_planning_msgs + autoware_rtc_interface freespace_planning_algorithms geometry_msgs interpolation - lane_departure_checker lanelet2_extension magic_enum motion_utils @@ -56,7 +57,6 @@ objects_of_interest_marker_interface rclcpp route_handler - rtc_interface tf2 tier4_autoware_utils tier4_planning_msgs 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/behavior_path_side_shift_module/CMakeLists.txt b/planning/autoware_behavior_path_side_shift_module/CMakeLists.txt similarity index 92% rename from planning/behavior_path_side_shift_module/CMakeLists.txt rename to planning/autoware_behavior_path_side_shift_module/CMakeLists.txt index 28009e48a4cc4..c36a4ffd73254 100644 --- a/planning/behavior_path_side_shift_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_side_shift_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_side_shift_module) +project(autoware_behavior_path_side_shift_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_side_shift_module/README.md b/planning/autoware_behavior_path_side_shift_module/README.md similarity index 100% rename from planning/behavior_path_side_shift_module/README.md rename to planning/autoware_behavior_path_side_shift_module/README.md diff --git a/planning/behavior_path_side_shift_module/config/side_shift.param.yaml b/planning/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml similarity index 100% rename from planning/behavior_path_side_shift_module/config/side_shift.param.yaml rename to planning/autoware_behavior_path_side_shift_module/config/side_shift.param.yaml diff --git a/planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg b/planning/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg similarity index 100% rename from planning/behavior_path_side_shift_module/images/side_shift_status.drawio.svg rename to planning/autoware_behavior_path_side_shift_module/images/side_shift_status.drawio.svg diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp similarity index 87% rename from planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp rename to planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp index 8059c46423dac..227b81e3d006f 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/data_structs.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ -#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -51,4 +51,4 @@ struct SideShiftDebugData }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp similarity index 85% rename from planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp rename to planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp index 21621e1c7128c..7e03c77c757e8 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/manager.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ #include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "behavior_path_side_shift_module/scene.hpp" +#include "autoware_behavior_path_side_shift_module/scene.hpp" #include @@ -50,4 +50,4 @@ class SideShiftModuleManager : public SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp similarity index 94% rename from planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp rename to planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp index accba00b81439..5c8442284e513 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/scene.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ #include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_side_shift_module/data_structs.hpp" +#include "autoware_behavior_path_side_shift_module/data_structs.hpp" #include @@ -131,4 +131,4 @@ class SideShiftModule : public SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp similarity index 88% rename from planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp rename to planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp index f53bd42363b55..e5973948af9b3 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp +++ b/planning/autoware_behavior_path_side_shift_module/include/autoware_behavior_path_side_shift_module/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ -#define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #include #include @@ -41,4 +41,4 @@ Point transformToGrid( } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ diff --git a/planning/behavior_path_side_shift_module/package.xml b/planning/autoware_behavior_path_side_shift_module/package.xml similarity index 91% rename from planning/behavior_path_side_shift_module/package.xml rename to planning/autoware_behavior_path_side_shift_module/package.xml index af2b3c121f090..98d765fbe2fc7 100644 --- a/planning/behavior_path_side_shift_module/package.xml +++ b/planning/autoware_behavior_path_side_shift_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_side_shift_module + autoware_behavior_path_side_shift_module 0.1.0 - The behavior_path_side_shift_module package + The autoware_behavior_path_side_shift_module package Satoshi Ota Fumiya Watanabe diff --git a/planning/behavior_path_side_shift_module/plugins.xml b/planning/autoware_behavior_path_side_shift_module/plugins.xml similarity index 71% rename from planning/behavior_path_side_shift_module/plugins.xml rename to planning/autoware_behavior_path_side_shift_module/plugins.xml index 2688e3a17c2b6..7be57efc71739 100644 --- a/planning/behavior_path_side_shift_module/plugins.xml +++ b/planning/autoware_behavior_path_side_shift_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_path_side_shift_module/src/manager.cpp b/planning/autoware_behavior_path_side_shift_module/src/manager.cpp similarity index 97% rename from planning/behavior_path_side_shift_module/src/manager.cpp rename to planning/autoware_behavior_path_side_shift_module/src/manager.cpp index fa8d579da3d09..bee9d3c78bafc 100644 --- a/planning/behavior_path_side_shift_module/src/manager.cpp +++ b/planning/autoware_behavior_path_side_shift_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_side_shift_module/manager.hpp" +#include "autoware_behavior_path_side_shift_module/manager.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" diff --git a/planning/behavior_path_side_shift_module/src/scene.cpp b/planning/autoware_behavior_path_side_shift_module/src/scene.cpp similarity index 99% rename from planning/behavior_path_side_shift_module/src/scene.cpp rename to planning/autoware_behavior_path_side_shift_module/src/scene.cpp index c2b239b5dd4fa..e5ea0e4cca29c 100644 --- a/planning/behavior_path_side_shift_module/src/scene.cpp +++ b/planning/autoware_behavior_path_side_shift_module/src/scene.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_side_shift_module/scene.hpp" +#include "autoware_behavior_path_side_shift_module/scene.hpp" #include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_side_shift_module/utils.hpp" +#include "autoware_behavior_path_side_shift_module/utils.hpp" #include diff --git a/planning/behavior_path_side_shift_module/src/utils.cpp b/planning/autoware_behavior_path_side_shift_module/src/utils.cpp similarity index 97% rename from planning/behavior_path_side_shift_module/src/utils.cpp rename to planning/autoware_behavior_path_side_shift_module/src/utils.cpp index 888a2e9e24d9f..507259d9e6e23 100644 --- a/planning/behavior_path_side_shift_module/src/utils.cpp +++ b/planning/autoware_behavior_path_side_shift_module/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_side_shift_module/utils.hpp" +#include "autoware_behavior_path_side_shift_module/utils.hpp" #include "autoware_behavior_path_planner_common/utils/utils.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_side_shift_module/test/test_behavior_path_planner_node_interface.cpp similarity index 88% rename from planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp index 69ce87aabc7b4..244c59bf5e6f9 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -48,11 +48,9 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto behavior_path_planner_dir = ament_index_cpp::get_package_share_directory("behavior_path_planner"); - const auto behavior_path_lane_change_module_dir = - ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); + module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); @@ -66,12 +64,8 @@ std::shared_ptr generateNode() 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"}); + ament_index_cpp::get_package_share_directory("autoware_behavior_path_side_shift_module") + + "/config/side_shift.param.yaml"}); return std::make_shared(node_options); } 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..bcb3b1bec1fd9 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` 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 95% 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..95c98c3c89733 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,8 +13,8 @@ // 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" @@ -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 83% 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..df640b04e5a72 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,10 +12,10 @@ // 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 @@ -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 89% 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..8cbe3569d13fe 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 @@ -102,4 +102,4 @@ class PullOutPlannerBase }; } // 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 94% 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..7a98a43efec5b 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,13 +21,13 @@ #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 @@ -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 @@ -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 97% 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..1dfacd45b5218 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 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..b30af17eaa898 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 @@ -65,7 +65,7 @@ StartPlannerModule::StartPlannerModule( { 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..e3699306df1f3 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,7 @@ autoware_behavior_path_planner_common autoware_perception_msgs + autoware_rtc_interface behavior_path_planner geometry_msgs lanelet2_extension @@ -32,7 +33,6 @@ pluginlib rclcpp route_handler - rtc_interface sensor_msgs signal_processing tf2 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/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 86% rename from planning/behavior_velocity_intersection_module/package.xml rename to planning/autoware_behavior_velocity_intersection_module/package.xml index 3694a395b3a53..0655867951ed8 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,10 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common + autoware_rtc_interface fmt geometry_msgs interpolation @@ -32,7 +33,6 @@ pluginlib rclcpp route_handler - rtc_interface tf2_geometry_msgs tier4_autoware_utils tier4_planning_msgs 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/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index bcef46e90c1ec..acf4c1ce78e56 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -9,9 +9,9 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Blind Spot](../behavior_velocity_blind_spot_module/README.md) - [Crosswalk](../behavior_velocity_crosswalk_module/README.md) -- [Walkway](../behavior_velocity_walkway_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) - [Virtual Traffic Light](../autoware_behavior_velocity_virtual_traffic_light_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 fff1fb8ff91c0..70fd92fd9e072 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,21 +65,20 @@ ament_cmake_ros ament_lint_auto + autoware_behavior_velocity_intersection_module autoware_behavior_velocity_run_out_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 - behavior_velocity_walkway_module rosidl_interface_packages diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index ffbc4ef9174dc..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 e741d054afbaf..cf8b918418c53 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -68,21 +68,20 @@ std::shared_ptr generateNode() }; std::vector module_names; - module_names.emplace_back("behavior_velocity_planner::CrosswalkModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::WalkwayModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::TrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::IntersectionModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::MergeFromPrivateModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::BlindSpotModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::DetectionAreaModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); + module_names.emplace_back("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); @@ -98,9 +97,9 @@ std::shared_ptr generateNode() behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", get_behavior_velocity_module_config_no_prefix("blind_spot"), get_behavior_velocity_module_config_no_prefix("crosswalk"), - get_behavior_velocity_module_config_no_prefix("walkway"), + get_behavior_velocity_module_config("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("run_out"), @@ -108,7 +107,6 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config_no_prefix("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 92% 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..c69f23215a369 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,13 +12,13 @@ // 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 @@ -45,7 +45,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class BehaviorVelocityPlannerNode; struct PlannerData @@ -88,7 +88,7 @@ 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 @@ -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 91% rename from planning/behavior_velocity_planner_common/package.xml rename to planning/autoware_behavior_velocity_planner_common/package.xml index ba847d8b1f853..7ab991a247a63 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,7 @@ autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_rtc_interface autoware_velocity_smoother diagnostic_msgs eigen @@ -36,7 +37,6 @@ rclcpp rclcpp_components route_handler - rtc_interface sensor_msgs tf2 tf2_eigen 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/package.xml b/planning/autoware_behavior_velocity_run_out_module/package.xml index cb2c2df58ffe2..330880b77d3da 100644 --- a/planning/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/autoware_behavior_velocity_run_out_module/package.xml @@ -19,10 +19,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs behavior_velocity_crosswalk_module - behavior_velocity_planner_common eigen geometry_msgs libboost-dev 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..002a49abae611 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -22,7 +22,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_autoware_utils::getOrDeclareParameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -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..28bfa9569c66d 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -17,8 +17,8 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -38,8 +38,6 @@ 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; 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..cda3abbd0eee9 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,8 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs lanelet2_extension motion_utils 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..9775e64145529 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,7 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#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/behavior_velocity_walkway_module/CMakeLists.txt b/planning/autoware_behavior_velocity_walkway_module/CMakeLists.txt similarity index 86% rename from planning/behavior_velocity_walkway_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_walkway_module/CMakeLists.txt index 351a240743402..11504d9c8999c 100644 --- a/planning/behavior_velocity_walkway_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_walkway_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_walkway_module) +project(autoware_behavior_velocity_walkway_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_walkway_module/README.md b/planning/autoware_behavior_velocity_walkway_module/README.md similarity index 100% rename from planning/behavior_velocity_walkway_module/README.md rename to planning/autoware_behavior_velocity_walkway_module/README.md diff --git a/planning/behavior_velocity_walkway_module/config/walkway.param.yaml b/planning/autoware_behavior_velocity_walkway_module/config/walkway.param.yaml similarity index 100% rename from planning/behavior_velocity_walkway_module/config/walkway.param.yaml rename to planning/autoware_behavior_velocity_walkway_module/config/walkway.param.yaml diff --git a/planning/behavior_velocity_walkway_module/package.xml b/planning/autoware_behavior_velocity_walkway_module/package.xml similarity index 86% rename from planning/behavior_velocity_walkway_module/package.xml rename to planning/autoware_behavior_velocity_walkway_module/package.xml index ea6c7803717f9..bbe87ffb8b2cd 100644 --- a/planning/behavior_velocity_walkway_module/package.xml +++ b/planning/autoware_behavior_velocity_walkway_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_walkway_module + autoware_behavior_velocity_walkway_module 0.1.0 - The behavior_velocity_walkway_module package + The autoware_behavior_velocity_walkway_module package Satoshi Ota Tomoya Kimura @@ -17,9 +17,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_planning_msgs behavior_velocity_crosswalk_module - behavior_velocity_planner_common geometry_msgs lanelet2_extension libboost-dev diff --git a/planning/autoware_behavior_velocity_walkway_module/plugins.xml b/planning/autoware_behavior_velocity_walkway_module/plugins.xml new file mode 100644 index 0000000000000..9a5803807ae8d --- /dev/null +++ b/planning/autoware_behavior_velocity_walkway_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_walkway_module/src/debug.cpp b/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp similarity index 95% rename from planning/behavior_velocity_walkway_module/src/debug.cpp rename to planning/autoware_behavior_velocity_walkway_module/src/debug.cpp index 8814fa8271866..80badf59434da 100644 --- a/planning/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 @@ -22,7 +22,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; @@ -98,4 +98,4 @@ visualization_msgs::msg::MarkerArray WalkwayModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp similarity index 92% rename from planning/behavior_velocity_walkway_module/src/manager.cpp rename to planning/autoware_behavior_velocity_walkway_module/src/manager.cpp index d427e57009cf6..cf3efe3700a05 100644 --- a/planning/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 @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::Crosswalk; @@ -100,8 +100,9 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) return walkway_id_set.count(scene_module->getModuleId()) == 0; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::WalkwayModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::WalkwayModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp similarity index 82% rename from planning/behavior_velocity_walkway_module/src/manager.hpp rename to planning/autoware_behavior_velocity_walkway_module/src/manager.hpp index 5228445bb4e03..592181dd3ac75 100644 --- a/planning/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 @@ -31,9 +31,8 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { - using tier4_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface @@ -56,6 +55,6 @@ class WalkwayModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp similarity index 97% rename from planning/behavior_velocity_walkway_module/src/scene_walkway.cpp rename to planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 4a777d49f4fe6..73ae5a52e0183 100644 --- a/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -14,12 +14,12 @@ #include "scene_walkway.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcLongitudinalOffsetPose; @@ -165,4 +165,4 @@ bool WalkwayModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_ return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp similarity index 92% rename from planning/behavior_velocity_walkway_module/src/scene_walkway.hpp rename to planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index c372a5835f795..e6c8f61ba656e 100644 --- a/planning/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 @@ -31,9 +31,8 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { - class WalkwayModule : public SceneModuleInterface { public: @@ -77,6 +76,6 @@ class WalkwayModule : public SceneModuleInterface // flag to use regulatory element const bool use_regulatory_element_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_WALKWAY_HPP_ 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/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..edca8d706047b 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 @@ -25,11 +25,11 @@ #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 +} // 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..8c207a9a3830f 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 @@ -34,7 +34,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct Bounds { @@ -308,5 +308,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..30c95debe11cb 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp @@ -33,7 +33,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { class PathOptimizer : public rclcpp::Node { @@ -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..588b68f52a094 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 @@ -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 { @@ -68,5 +68,5 @@ bool isOutsideDrivableAreaFromRectangleFootprint( const vehicle_info_util::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/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index 3127d521160c2..7c644f4448a0c 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; @@ -432,4 +432,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..2ab622c6b4b58 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -29,7 +29,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace { @@ -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..816c0d459d95f 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 { @@ -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..45302c0b729a9 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; @@ -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/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..26eb41ca1de23 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -18,6 +18,7 @@ autoware_behavior_path_planner_common autoware_map_msgs autoware_path_optimizer + autoware_path_smoother autoware_perception_msgs autoware_planning_msgs geography_utils @@ -30,7 +31,6 @@ mission_planner motion_utils osqp_interface - path_smoother rclcpp rclcpp_components route_handler 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/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/surround_obstacle_checker/CMakeLists.txt b/planning/autoware_surround_obstacle_checker/CMakeLists.txt similarity index 82% rename from planning/surround_obstacle_checker/CMakeLists.txt rename to planning/autoware_surround_obstacle_checker/CMakeLists.txt index 0d7b636646783..141b2c7ce4aa2 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/autoware_surround_obstacle_checker/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(surround_obstacle_checker) +project(autoware_surround_obstacle_checker) find_package(autoware_cmake REQUIRED) autoware_package() @@ -23,7 +23,7 @@ target_link_libraries(${PROJECT_NAME} ) rclcpp_components_register_node(${PROJECT_NAME} - PLUGIN "surround_obstacle_checker::SurroundObstacleCheckerNode" + PLUGIN "autoware::surround_obstacle_checker::SurroundObstacleCheckerNode" EXECUTABLE ${PROJECT_NAME}_node ) diff --git a/planning/surround_obstacle_checker/README.md b/planning/autoware_surround_obstacle_checker/README.md similarity index 98% rename from planning/surround_obstacle_checker/README.md rename to planning/autoware_surround_obstacle_checker/README.md index 5cf7c8a419b61..1f4bf77145624 100644 --- a/planning/surround_obstacle_checker/README.md +++ b/planning/autoware_surround_obstacle_checker/README.md @@ -101,7 +101,7 @@ As mentioned in stop condition section, it prevents chattering by changing thres ## Parameters -{{ json_to_markdown("planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json") }} +{{ json_to_markdown("planning/autoware_surround_obstacle_checker/schema/surround_obstacle_checker.schema.json") }} | Name | Type | Description | Default value | | :----------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------- | :------------------------------------------- | diff --git a/planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml b/planning/autoware_surround_obstacle_checker/config/surround_obstacle_checker.param.yaml similarity index 100% rename from planning/surround_obstacle_checker/config/surround_obstacle_checker.param.yaml rename to planning/autoware_surround_obstacle_checker/config/surround_obstacle_checker.param.yaml diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp similarity index 93% rename from planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp rename to planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp index 07a23a4ac2e67..d2e055bbccaa2 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/debug_marker.hpp +++ b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ -#define SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +#define AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ #include #include @@ -31,7 +31,7 @@ #include #include -namespace surround_obstacle_checker +namespace autoware::surround_obstacle_checker { using autoware_adapi_v1_msgs::msg::PlanningBehavior; @@ -98,5 +98,5 @@ class SurroundObstacleCheckerDebugNode std::shared_ptr stop_pose_ptr_; rclcpp::Clock::SharedPtr clock_; }; -} // namespace surround_obstacle_checker -#endif // SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ +} // namespace autoware::surround_obstacle_checker +#endif // AUTOWARE_SURROUND_OBSTACLE_CHECKER__DEBUG_MARKER_HPP_ diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp similarity index 94% rename from planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp rename to planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp index d5b2c49df51cb..4ccd097b40837 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/autoware_surround_obstacle_checker/include/autoware_surround_obstacle_checker/node.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SURROUND_OBSTACLE_CHECKER__NODE_HPP_ -#define SURROUND_OBSTACLE_CHECKER__NODE_HPP_ +#ifndef AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ +#define AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ -#include "surround_obstacle_checker/debug_marker.hpp" +#include "autoware_surround_obstacle_checker/debug_marker.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/polling_subscriber.hpp" @@ -43,7 +43,7 @@ #include #include -namespace surround_obstacle_checker +namespace autoware::surround_obstacle_checker { using autoware_perception_msgs::msg::PredictedObjects; @@ -149,6 +149,6 @@ class SurroundObstacleCheckerNode : public rclcpp::Node std::unordered_map label_map_; }; -} // namespace surround_obstacle_checker +} // namespace autoware::surround_obstacle_checker -#endif // SURROUND_OBSTACLE_CHECKER__NODE_HPP_ +#endif // AUTOWARE_SURROUND_OBSTACLE_CHECKER__NODE_HPP_ diff --git a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml b/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml similarity index 80% rename from planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml rename to planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml index 53f26a05c87c3..0ba7d29090916 100644 --- a/planning/surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml +++ b/planning/autoware_surround_obstacle_checker/launch/surround_obstacle_checker.launch.xml @@ -1,5 +1,5 @@ - + @@ -7,7 +7,7 @@ - + diff --git a/planning/surround_obstacle_checker/media/check_distance.drawio.svg b/planning/autoware_surround_obstacle_checker/media/check_distance.drawio.svg similarity index 100% rename from planning/surround_obstacle_checker/media/check_distance.drawio.svg rename to planning/autoware_surround_obstacle_checker/media/check_distance.drawio.svg diff --git a/planning/surround_obstacle_checker/package.xml b/planning/autoware_surround_obstacle_checker/package.xml similarity index 91% rename from planning/surround_obstacle_checker/package.xml rename to planning/autoware_surround_obstacle_checker/package.xml index a1ef2c408ce3b..3bb961766e9f3 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -1,9 +1,9 @@ - surround_obstacle_checker + autoware_surround_obstacle_checker 0.1.0 - The surround_obstacle_checker package + The autoware_surround_obstacle_checker package Satoshi Ota Go Sakayori Apache License 2.0 diff --git a/planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json b/planning/autoware_surround_obstacle_checker/schema/surround_obstacle_checker.schema.json similarity index 99% rename from planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json rename to planning/autoware_surround_obstacle_checker/schema/surround_obstacle_checker.schema.json index 00a8ae6a12991..85458fb7cdbc6 100644 --- a/planning/surround_obstacle_checker/schema/surround_obstacle_checker.schema.json +++ b/planning/autoware_surround_obstacle_checker/schema/surround_obstacle_checker.schema.json @@ -3,7 +3,7 @@ "title": "Parameters for Surround Obstacle Checker Node", "type": "object", "definitions": { - "surround_obstacle_checker": { + "autoware_surround_obstacle_checker": { "type": "object", "properties": { "pointcloud": { @@ -385,7 +385,7 @@ "type": "object", "properties": { "ros__parameters": { - "$ref": "#/definitions/surround_obstacle_checker" + "$ref": "#/definitions/autoware_surround_obstacle_checker" } }, "required": ["ros__parameters"], diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp similarity index 98% rename from planning/surround_obstacle_checker/src/debug_marker.cpp rename to planning/autoware_surround_obstacle_checker/src/debug_marker.cpp index a0ca15d490e9c..ba561a4b8cf35 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/autoware_surround_obstacle_checker/src/debug_marker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "surround_obstacle_checker/debug_marker.hpp" +#include "autoware_surround_obstacle_checker/debug_marker.hpp" #include #include @@ -25,7 +25,7 @@ #include -namespace surround_obstacle_checker +namespace autoware::surround_obstacle_checker { namespace { @@ -245,4 +245,4 @@ void SurroundObstacleCheckerDebugNode::updateFootprintMargin( surround_check_back_distance_ = back_distance; } -} // namespace surround_obstacle_checker +} // namespace autoware::surround_obstacle_checker diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/autoware_surround_obstacle_checker/src/node.cpp similarity index 98% rename from planning/surround_obstacle_checker/src/node.cpp rename to planning/autoware_surround_obstacle_checker/src/node.cpp index af6dba8fcc0c9..0744ebecd370c 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/autoware_surround_obstacle_checker/src/node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "surround_obstacle_checker/node.hpp" +#include "autoware_surround_obstacle_checker/node.hpp" #include #include @@ -46,7 +46,7 @@ #include #include -namespace surround_obstacle_checker +namespace autoware::surround_obstacle_checker { namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; @@ -541,7 +541,7 @@ bool SurroundObstacleCheckerNode::isStopRequired( return false; } -} // namespace surround_obstacle_checker +} // namespace autoware::surround_obstacle_checker #include -RCLCPP_COMPONENTS_REGISTER_NODE(surround_obstacle_checker::SurroundObstacleCheckerNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::surround_obstacle_checker::SurroundObstacleCheckerNode) diff --git a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md similarity index 100% rename from planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md rename to planning/autoware_surround_obstacle_checker/surround_obstacle_checker-design.ja.md 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/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 0ea10534eb126..1541b19d70a1c 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -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) @@ -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/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..b7c4a24e52974 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 @@ -50,9 +50,9 @@ 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; 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..d6c9215e495ae 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 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/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_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_planner/README.md b/planning/behavior_path_planner/README.md index 2e79589c9f1bd..70190731ac1a7 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -33,8 +33,8 @@ 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) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_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..f51fa99578d7b 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -38,23 +38,23 @@ autoware_adapi_v1_msgs autoware_behavior_path_planner_common + autoware_frenet_planner + autoware_lane_departure_checker + autoware_path_sampler autoware_perception_msgs autoware_planning_msgs autoware_planning_test_manager 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 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..0a5dec8d07b4f 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,19 +21,19 @@ #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 "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" @@ -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; + 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..0583056db4293 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -14,17 +14,17 @@ 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_msgs - bezier_sampler - frenet_planner geometry_msgs interpolation lanelet2_extension motion_utils - path_sampler pluginlib rclcpp rclcpp_components 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..22187652bd663 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 @@ -37,21 +37,22 @@ SamplingPlannerModule::SamplingPlannerModule( : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT vehicle_info_{vehicle_info_util::VehicleInfoUtil(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..eebf9c4d4a14f 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,9 +21,9 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs grid_map_core 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..91893c5224550 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; @@ -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..13ecddafb6490 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; @@ -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..6b9db657d6d8e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -12,9 +12,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs libboost-dev motion_utils 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..ddb8c842fb92d 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; @@ -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..baa51d6681490 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,9 +17,9 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs interpolation 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..6aeac1e9d8dd3 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; @@ -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/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml index 5817c2203cc5d..ceddd48b045a0 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_occlusion_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 grid_map_ros grid_map_utils diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml index a38900d1a893b..2d85752cf2b3d 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_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index 1170afc490a75..40e5b8f91e6b9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/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/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index c8adb324b9055..427a6f27be3c4 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/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 { @@ -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/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 710f671df609f..5c8d77fc831df 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/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; @@ -118,6 +118,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/behavior_velocity_occlusion_spot_module/src/manager.cpp index 64adc112aab2f..b1757c4142212 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/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; @@ -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/behavior_velocity_occlusion_spot_module/src/manager.hpp index 0955e4ae9aab5..123d0ef32afc1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/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/package.xml b/planning/behavior_velocity_stop_line_module/package.xml index c71ecb83098fb..3132f60d1f342 100644 --- a/planning/behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -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/behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_stop_line_module/plugins.xml index 51fb225fbebad..4b8ce9852fcec 100644 --- a/planning/behavior_velocity_stop_line_module/plugins.xml +++ b/planning/behavior_velocity_stop_line_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index f63bdca5068a2..bfcd5a5828556 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/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/behavior_velocity_stop_line_module/src/manager.cpp index b37b3b119178c..80f48fa65efaf 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/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/behavior_velocity_stop_line_module/src/manager.hpp index af9dbaa083c33..eca5132a75c5f 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/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/behavior_velocity_stop_line_module/src/scene.cpp index 976acd98fe42b..1bf777338e43c 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/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/behavior_velocity_stop_line_module/src/scene.hpp index 70c67df623c85..0ae0a885f036f 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/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/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/behavior_velocity_walkway_module/plugins.xml b/planning/behavior_velocity_walkway_module/plugins.xml deleted file mode 100644 index 971a49f2cb044..0000000000000 --- a/planning/behavior_velocity_walkway_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - 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..87aa4ac483feb 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 @@ -81,7 +81,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_{}; // parameters vehicle_info_util::VehicleInfo vehicle_info_; 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..262c32fe1ea47 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 @@ -241,7 +241,7 @@ void MotionVelocityPlannerNode::on_acceleration( void MotionVelocityPlannerNode::set_velocity_smoother_params() { planner_data_.velocity_smoother_ = - std::make_shared(*this); + std::make_shared(*this); } void MotionVelocityPlannerNode::on_lanelet_map( @@ -399,7 +399,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/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..3aea741e3b2f4 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -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/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 416c36b116059..7a060657e16af 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 @@ -42,7 +44,8 @@ class PlannerInterface 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 @@ -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/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/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index c5f0c73a13b78..41ae29b7d470f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -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/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/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/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 91% 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..113c70fecb62c 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp @@ -12,11 +12,11 @@ // 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 "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -33,14 +33,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; @@ -141,6 +141,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..7fe57e68928d9 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 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..aac50e24c5d5c 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; @@ -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/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 77% 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..e44c38aed2b53 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_ +#ifndef AUTOWARE_PATH_SAMPLER__NODE_HPP_ +#define AUTOWARE_PATH_SAMPLER__NODE_HPP_ -#include "path_sampler/common_structs.hpp" -#include "path_sampler/parameters.hpp" -#include "path_sampler/type_alias.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 "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 { @@ -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 81% 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..abd2fa367bc7b 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,16 +12,16 @@ // 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 "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" @@ -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 92% rename from planning/sampling_based_planner/path_sampler/package.xml rename to planning/sampling_based_planner/autoware_path_sampler/package.xml index a57ff6e6033f6..011da8d469607 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,10 @@ autoware_cmake + autoware_bezier_sampler + autoware_frenet_planner autoware_perception_msgs autoware_planning_msgs - bezier_sampler - frenet_planner geometry_msgs interpolation motion_utils 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..a82c1b6a52a26 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 { @@ -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/CMakeLists.txt b/sensing/pointcloud_preprocessor/CMakeLists.txt index 0cbb04345477e..ecb84c8a7f3b3 100644 --- a/sensing/pointcloud_preprocessor/CMakeLists.txt +++ b/sensing/pointcloud_preprocessor/CMakeLists.txt @@ -221,6 +221,14 @@ ament_auto_package(INSTALL_TO_SHARE ) if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest) + ament_lint_auto_find_test_dependencies() + ament_add_gtest(test_utilities + test/test_utilities.cpp + ) + target_link_libraries(test_utilities pointcloud_preprocessor_filter) + add_ros_test( test/test_distortion_corrector.py TIMEOUT "30" diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md index bd2f7d98919d5..8e31dfb203d2b 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md +++ b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md @@ -10,6 +10,7 @@ The `vector_map_inside_area_filter` is a node that removes points inside the vec - Extract the vector map area that intersects with the bounding box of input points to reduce the calculation cost - Create the 2D polygon from the extracted vector map area - Remove input points inside the polygon +- If the z value is used for filtering, remove points that are below the z threshold ![vector_map_inside_area_filter_figure](./image/vector_map_inside_area_filter_overview.svg) @@ -35,5 +36,7 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, so please | Name | Type | Description | | -------------- | ------ | --------------------------- | | `polygon_type` | string | polygon type to be filtered | +| `use_z` | bool | use z value for filtering | +| `z_threshold` | float | z threshold for filtering | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp index 5de644fd8ffd1..654057e7bb8d7 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/utility/utilities.hpp @@ -26,6 +26,7 @@ #include #include +#include #include using K = CGAL::Exact_predicates_inexact_constructions_kernel; @@ -49,28 +50,30 @@ void to_cgal_polygon(const lanelet::BasicPolygon2d & polygon_in, PolygonCgal & p */ void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon, - sensor_msgs::msg::PointCloud2 & cloud_out); + sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygon */ void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out); + pcl::PointCloud & cloud_out, const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygons */ void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, - const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out); + const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, + const std::optional & max_z = std::nullopt); /** * @brief remove points in the given polygons */ void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out); + const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, + const std::optional & max_z = std::nullopt); /** * @brief return true if the given point is inside the at least one of the polygons diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 894768d385438..760ffbe1eb1dd 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -43,6 +43,8 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte // parameter std::string polygon_type_; + bool use_z_filter_ = false; + float z_threshold_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp index fe9fde7301fd7..ec84a2467db78 100644 --- a/sensing/pointcloud_preprocessor/src/utility/utilities.cpp +++ b/sensing/pointcloud_preprocessor/src/utility/utilities.cpp @@ -46,18 +46,19 @@ void to_cgal_polygon(const lanelet::BasicPolygon2d & polygon_in, PolygonCgal & p void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, const PolygonCgal & polyline_polygon, - sensor_msgs::msg::PointCloud2 & cloud_out) + sensor_msgs::msg::PointCloud2 & cloud_out, const std::optional & max_z) { pcl::PointCloud pcl_output; for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - // check if the point is inside the polygon - if ( - CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(*iter_x, *iter_y), K()) == - CGAL::ON_UNBOUNDED_SIDE) { + const bool within_max_z = max_z ? *iter_z <= *max_z : true; + const bool within_polygon = CGAL::bounded_side_2( + polyline_polygon.begin(), polyline_polygon.end(), + PointCgal(*iter_x, *iter_y), K()) == CGAL::ON_BOUNDED_SIDE; + // remove points within the polygon and max_z + if (!(within_max_z && within_polygon)) { pcl::PointXYZ p; p.x = *iter_x; p.y = *iter_y; @@ -72,17 +73,18 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, const PolygonCgal & polyline_polygon, - pcl::PointCloud & cloud_out) + pcl::PointCloud & cloud_out, const std::optional & max_z) { cloud_out.clear(); cloud_out.header = cloud_in.header; for (const auto & p : cloud_in) { - // check if the point is inside the polygon - if ( - CGAL::bounded_side_2( - polyline_polygon.begin(), polyline_polygon.end(), PointCgal(p.x, p.y), K()) == - CGAL::ON_UNBOUNDED_SIDE) { + const bool within_max_z = max_z ? p.z <= *max_z : true; + const bool within_polygon = CGAL::bounded_side_2( + polyline_polygon.begin(), polyline_polygon.end(), + PointCgal(p.x, p.y), K()) == CGAL::ON_BOUNDED_SIDE; + // remove points within the polygon and max_z + if (!(within_max_z && within_polygon)) { cloud_out.emplace_back(p); } } @@ -90,7 +92,8 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const sensor_msgs::msg::PointCloud2 & cloud_in, - const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out) + const std::vector & polyline_polygons, sensor_msgs::msg::PointCloud2 & cloud_out, + const std::optional & max_z) { if (polyline_polygons.empty()) { cloud_out = cloud_in; @@ -101,9 +104,11 @@ void remove_polygon_cgal_from_cloud( for (sensor_msgs::PointCloud2ConstIterator iter_x(cloud_in, "x"), iter_y(cloud_in, "y"), iter_z(cloud_in, "z"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) { - // if the point is inside the polygon, skip inserting and check the next point - pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); - if (point_within_cgal_polys(p, polyline_polygons)) { + const bool within_max_z = max_z ? *iter_z <= *max_z : true; + const pcl::PointXYZ p(*iter_x, *iter_y, *iter_z); + const bool within_polygon = point_within_cgal_polys(p, polyline_polygons); + // remove points within the polygon and max_z + if (within_max_z && within_polygon) { continue; } filtered_cloud.emplace_back(p); @@ -115,7 +120,8 @@ void remove_polygon_cgal_from_cloud( void remove_polygon_cgal_from_cloud( const pcl::PointCloud & cloud_in, - const std::vector & polyline_polygons, pcl::PointCloud & cloud_out) + const std::vector & polyline_polygons, pcl::PointCloud & cloud_out, + const std::optional & max_z) { if (polyline_polygons.empty()) { cloud_out = cloud_in; @@ -124,8 +130,10 @@ void remove_polygon_cgal_from_cloud( pcl::PointCloud filtered_cloud; for (const auto & p : cloud_in) { - // if the point is inside the polygon, skip inserting and check the next point - if (point_within_cgal_polys(p, polyline_polygons)) { + const bool within_max_z = max_z ? p.z <= *max_z : true; + const bool within_polygon = point_within_cgal_polys(p, polyline_polygons); + // remove points within the polygon and max_z + if (within_max_z && within_polygon) { continue; } filtered_cloud.emplace_back(p); diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index 8fcf15326829f..6b4e90a697cd6 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -40,7 +40,8 @@ lanelet::ConstPolygons3d calcIntersectedPolygons( } pcl::PointCloud removePointsWithinPolygons( - const pcl::PointCloud::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons) + const pcl::PointCloud::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons, + const std::optional & z_threshold_) { std::vector cgal_polys; @@ -53,7 +54,7 @@ pcl::PointCloud removePointsWithinPolygons( pcl::PointCloud filtered_cloud; pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( - *cloud_in, cgal_polys, filtered_cloud); + *cloud_in, cgal_polys, filtered_cloud, z_threshold_); return filtered_cloud; } @@ -74,6 +75,10 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1)); + + // Set parameters + use_z_filter_ = declare_parameter("use_z_filter", false); + z_threshold_ = declare_parameter("z_threshold", 0.0f); // defined in the base_link frame } void VectorMapInsideAreaFilterComponent::filter( @@ -99,7 +104,25 @@ void VectorMapInsideAreaFilterComponent::filter( const auto intersected_lanelets = calcIntersectedPolygons(bounding_box, polygon_lanelets_); // filter pointcloud by lanelet - const auto filtered_pc = removePointsWithinPolygons(pc_input, intersected_lanelets); + std::optional z_threshold_in_base_link = std::nullopt; + if (use_z_filter_) { + // assume z_max is defined in the base_link frame + const std::string base_link_frame = "base_link"; + z_threshold_in_base_link = z_threshold_; + if (input->header.frame_id != base_link_frame) { + try { + // get z difference from baselink to input frame + const auto transform = + tf_buffer_->lookupTransform(input->header.frame_id, base_link_frame, input->header.stamp); + *z_threshold_in_base_link += transform.transform.translation.z; + } catch (const tf2::TransformException & e) { + RCLCPP_WARN(get_logger(), "Failed to transform z_threshold to base_link frame"); + z_threshold_in_base_link = std::nullopt; + } + } + } + const auto filtered_pc = + removePointsWithinPolygons(pc_input, intersected_lanelets, z_threshold_in_base_link); // convert to ROS message pcl::toROSMsg(filtered_pc, output); diff --git a/sensing/pointcloud_preprocessor/test/test_utilities.cpp b/sensing/pointcloud_preprocessor/test/test_utilities.cpp new file mode 100644 index 0000000000000..68c86dfbd0505 --- /dev/null +++ b/sensing/pointcloud_preprocessor/test/test_utilities.cpp @@ -0,0 +1,125 @@ +// 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 "pointcloud_preprocessor/utility/utilities.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +#include + +constexpr double EPSILON = 1e-6; + +class RemovePolygonCgalFromCloudTest : public ::testing::Test +{ +protected: + PolygonCgal polygon; + sensor_msgs::msg::PointCloud2 cloud_out; + + virtual void SetUp() + { + // Set up a simple square polygon + polygon.push_back(PointCgal(0.0, 0.0)); + polygon.push_back(PointCgal(0.0, 1.0)); + polygon.push_back(PointCgal(1.0, 1.0)); + polygon.push_back(PointCgal(1.0, 0.0)); + polygon.push_back(PointCgal(0.0, 0.0)); + } + + void CreatePointCloud2(sensor_msgs::msg::PointCloud2 & cloud, double x, double y, double z) + { + pcl::PointCloud pcl_cloud; + pcl_cloud.push_back(pcl::PointXYZ(x, y, z)); + pcl::toROSMsg(pcl_cloud, cloud); + cloud.header.frame_id = "map"; + } +}; + +// Test case 1: without max_z, points inside the polygon are removed +TEST_F(RemovePolygonCgalFromCloudTest, PointsInsidePolygonAreRemoved) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon + + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 0); +} + +// Test case 2: without max_z, points outside the polygon remain +TEST_F(RemovePolygonCgalFromCloudTest, PointsOutsidePolygonRemain) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 1.5, 1.5, 0.1); // point outside the polygon + + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(cloud_in, polygon, cloud_out); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 1); + EXPECT_NEAR(pcl_output.points[0].x, 1.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].y, 1.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].z, 0.1, EPSILON); +} + +// Test case 3: with max_z, points inside the polygon and below max_z are removed +TEST_F(RemovePolygonCgalFromCloudTest, PointsBelowMaxZAreRemoved) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 0.1); // point inside the polygon, below max_z + + std::optional max_z = 1.0f; + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out, max_z); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 0); +} + +// Test case 4: with max_z, points inside the polygon but above max_z remain +TEST_F(RemovePolygonCgalFromCloudTest, PointsAboveMaxZRemain) +{ + sensor_msgs::msg::PointCloud2 cloud_in; + CreatePointCloud2(cloud_in, 0.5, 0.5, 1.5); // point inside the polygon, above max_z + + std::optional max_z = 1.0f; + pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud( + cloud_in, polygon, cloud_out, max_z); + + pcl::PointCloud pcl_output; + pcl::fromROSMsg(cloud_out, pcl_output); + + EXPECT_EQ(pcl_output.size(), 1); + EXPECT_NEAR(pcl_output.points[0].x, 0.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].y, 0.5, EPSILON); + EXPECT_NEAR(pcl_output.points[0].z, 1.5, EPSILON); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} 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/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 6b94b7301540c..2fa234ff2e937 100644 --- a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -23,6 +23,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" #include @@ -55,7 +56,6 @@ namespace accel_brake_map_calibrator { - using autoware_vehicle_msgs::msg::SteeringReport; using autoware_vehicle_msgs::msg::VelocityReport; using geometry_msgs::msg::TwistStamped; @@ -107,18 +107,22 @@ class AccelBrakeMapCalibrator : public rclcpp::Node rclcpp::Publisher::SharedPtr map_error_ratio_pub_; rclcpp::Publisher::SharedPtr calibration_status_pub_; - rclcpp::Subscription::SharedPtr velocity_sub_; - rclcpp::Subscription::SharedPtr steer_sub_; - rclcpp::Subscription::SharedPtr actuation_status_sub_; - rclcpp::Subscription::SharedPtr actuation_cmd_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber steer_sub_{ + this, "~/input/steer"}; + tier4_autoware_utils::InterProcessPollingSubscriber actuation_status_sub_{ + this, "~/input/actuation_status"}; + tier4_autoware_utils::InterProcessPollingSubscriber actuation_cmd_sub_{ + this, "~/input/actuation_cmd"}; + tier4_autoware_utils::InterProcessPollingSubscriber velocity_sub_{ + this, "~/input/velocity"}; // Service rclcpp::Service::SharedPtr update_map_dir_server_; rclcpp::TimerBase::SharedPtr timer_; rclcpp::TimerBase::SharedPtr timer_output_csv_; - void initTimer(double period_s); - void initOutputCSVTimer(double period_s); + void init_timer(double period_s); + void init_output_csv_timer(double period_s); TwistStamped::ConstSharedPtr twist_ptr_; std::vector> twist_vec_; @@ -230,78 +234,79 @@ class AccelBrakeMapCalibrator : public rclcpp::Node // output log std::ofstream output_log_; - bool getCurrentPitchFromTF(double * pitch); - void timerCallback(); - void timerCallbackOutputCSV(); - void executeUpdate( + bool get_current_pitch_from_tf(double * pitch); + void fetch_data(); + void timer_callback_output_csv(); + void execute_update( const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, const int brake_pedal_index, const int brake_vel_index); - bool updateFourCellAroundOffset( + bool update_four_cell_around_offset( const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, const int brake_pedal_index, const int brake_vel_index, const double measured_acc); - bool updateEachValOffset( + bool update_each_val_offset( const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, const int brake_pedal_index, const int brake_vel_index, const double measured_acc, const double map_acc); - void updateTotalMapOffset(const double measured_acc, const double map_acc); - void callbackActuation( - const std_msgs::msg::Header header, const double accel, const double brake); - void callbackActuationCommand(const ActuationCommandStamped::ConstSharedPtr msg); - void callbackActuationStatus(const ActuationStatusStamped::ConstSharedPtr msg); - void callbackVelocity(const VelocityReport::ConstSharedPtr msg); - void callbackSteer(const SteeringReport::ConstSharedPtr msg); - bool callbackUpdateMapService( + void update_total_map_offset(const double measured_acc, const double map_acc); + + void take_actuation(const std_msgs::msg::Header & header, const double accel, const double brake); + void take_actuation_command(const ActuationCommandStamped::ConstSharedPtr msg); + void take_actuation_status(const ActuationStatusStamped::ConstSharedPtr msg); + void take_velocity(const VelocityReport::ConstSharedPtr msg); + bool fetch_data(); + + bool callback_update_map_service( const std::shared_ptr request_header, UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res); - bool getAccFromMap(const double velocity, const double pedal); - double lowpass(const double original, const double current, const double gain = 0.8); - double getPedalSpeed( + bool get_acc_from_map(const double velocity, const double pedal); + static double lowpass(const double original, const double current, const double gain = 0.8); + static double get_pedal_speed( const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, const double prev_pedal_speed); - double getAccel( + double get_accel( const TwistStamped::ConstSharedPtr & prev_twist, - const TwistStamped::ConstSharedPtr & current_twist); - double getJerk(); - bool indexValueSearch( - const std::vector value_index, const double value, const double value_thresh, - int * searched_index); - int nearestValueSearch(const std::vector value_index, const double value); - int nearestPedalSearch(); - int nearestVelSearch(); - void takeConsistencyOfAccelMap(); - void takeConsistencyOfBrakeMap(); - bool updateAccelBrakeMap(); - void publishFloat32(const std::string publish_type, const double val); - void publishUpdateSuggestFlag(); - double getPitchCompensatedAcceleration(); - void executeEvaluation(); - double calculateEstimatedAcc( + const TwistStamped::ConstSharedPtr & current_twist) const; + double get_jerk(); + bool index_value_search( + const std::vector & value_index, const double value, const double value_thresh, + int * searched_index) const; + static int nearest_value_search(const std::vector & value_index, const double value); + int nearest_pedal_search(); + int nearest_vel_search(); + void take_consistency_of_accel_map(); + void take_consistency_of_brake_map(); + bool update_accel_brake_map(); + void publish_float32(const std::string & publish_type, const double val); + void publish_update_suggest_flag(); + double get_pitch_compensated_acceleration() const; + void execute_evaluation(); + static double calculate_estimated_acc( const double throttle, const double brake, const double vel, AccelMap & accel_map, BrakeMap & brake_map); - double calculateAccelSquaredError( + double calculate_accel_squared_error( const double throttle, const double brake, const double vel, AccelMap & accel_map, BrakeMap & brake_map); - double calculateAccelErrorL1Norm( + double calculate_accel_error_l1_norm( const double throttle, const double brake, const double vel, AccelMap & accel_map, BrakeMap & brake_map); - std::vector getMapColumnFromUnifiedIndex( + static std::vector get_map_column_from_unified_index( const Map & accel_map_value, const Map & brake_map_value, const std::size_t index); - double getPedalValueFromUnifiedIndex(const std::size_t index); - int getUnifiedIndexFromAccelBrakeIndex(const bool accel_map, const std::size_t index); - void pushDataToQue( + double get_pedal_value_from_unified_index(const std::size_t index); + int get_unified_index_from_accel_brake_index(const bool accel_map, const std::size_t index); + static void push_data_to_que( const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, std::queue * que); template - void pushDataToVec(const T data, const std::size_t max_size, std::vector * vec); + void push_data_to_vec(const T data, const std::size_t max_size, std::vector * vec); template - T getNearestTimeDataFromVec( + static T get_nearest_time_data_from_vec( const T base_data, const double back_time, const std::vector & vec); - DataStampedPtr getNearestTimeDataFromVec( + DataStampedPtr get_nearest_time_data_from_vec( DataStampedPtr base_data, const double back_time, const std::vector & vec); - double getAverage(const std::vector & vec); - double getStandardDeviation(const std::vector & vec); - bool isTimeout(const builtin_interfaces::msg::Time & stamp, const double timeout_sec); - bool isTimeout(const DataStampedPtr & data_stamped, const double timeout_sec); + static double get_average(const std::vector & vec); + double get_standard_deviation(const std::vector & vec); + bool is_timeout(const builtin_interfaces::msg::Time & stamp, const double timeout_sec); + bool is_timeout(const DataStampedPtr & data_stamped, const double timeout_sec); /* for covariance calculation */ // mean value on each cell (counting method depends on the update algorithm) @@ -314,24 +319,24 @@ class AccelBrakeMapCalibrator : public rclcpp::Node Eigen::MatrixXd accel_data_num_; Eigen::MatrixXd brake_data_num_; - OccupancyGrid getOccMsg( - const std::string frame_id, const double height, const double width, const double resolution, + OccupancyGrid get_occ_msg( + const std::string & frame_id, const double height, const double width, const double resolution, const std::vector & map_value); /* Diag*/ - void checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat); + void check_update_suggest(diagnostic_updater::DiagnosticStatusWrapper & stat); /* Debug */ - void publishMap( - const Map accel_map_value, const Map brake_map_value, const std::string publish_type); - void publishOffsetCovMap(const Map accel_map_value, const Map brake_map_value); - void publishCountMap(); - void publishIndex(); - bool writeMapToCSV( + void publish_map( + const Map & accel_map_value, const Map & brake_map_value, const std::string & publish_type); + void publish_offset_cov_map(const Map accel_map_value, const Map brake_map_value); + void publish_count_map(); + void publish_index(); + bool write_map_to_csv( std::vector vel_index, std::vector pedal_index, Map value_map, std::string filename); - void addIndexToCSV(std::ofstream * csv_file); - void addLogToCSV( + static void add_index_to_csv(std::ofstream * csv_file); + static void add_log_to_csv( std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, const double pitched_accel, const double accel_pedal, const double brake_pedal, const double accel_pedal_speed, const double brake_pedal_speed, const double pitch, @@ -378,4 +383,4 @@ class AccelBrakeMapCalibrator : public rclcpp::Node } // namespace accel_brake_map_calibrator -#endif // ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ +#endif // ACCEL_BRAKE_MAP_CALIBRATOR__ACCEL_BRAKE_MAP_CALIBRATOR_NODE_HPP_ \ No newline at end of file diff --git a/vehicle/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/package.xml index 4c4eadea5e1ce..e9b2d7b6bf690 100644 --- a/vehicle/accel_brake_map_calibrator/package.xml +++ b/vehicle/accel_brake_map_calibrator/package.xml @@ -7,6 +7,7 @@ Tomoya Kimura Taiki Tanaka Takeshi Miura + Eiki Nagata Apache License 2.0 diff --git a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp b/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp index 37f0a11165df3..59e160596e64f 100644 --- a/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp +++ b/vehicle/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp @@ -46,10 +46,10 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod max_accel_ = declare_parameter("max_accel", 5.0); min_accel_ = declare_parameter("min_accel", -5.0); pedal_to_accel_delay_ = declare_parameter("pedal_to_accel_delay", 0.3); - max_data_count_ = declare_parameter("max_data_count", 200); + max_data_count_ = static_cast(declare_parameter("max_data_count", 200)); pedal_accel_graph_output_ = declare_parameter("pedal_accel_graph_output", false); progress_file_output_ = declare_parameter("progress_file_output", false); - precision_ = declare_parameter("precision", 3); + precision_ = static_cast(declare_parameter("precision", 3)); const auto get_pitch_method_str = declare_parameter("get_pitch_method", std::string("tf")); const auto get_pitch_method_str = declare_parameter("get_pitch_method", std::string("tf")); if (get_pitch_method_str == std::string("tf")) { get_pitch_method_ = GET_PITCH_METHOD::TF; @@ -95,7 +95,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod static constexpr std::size_t queue_size = 1; rclcpp::QoS durable_qos(queue_size); - // Publisher for checkUpdateSuggest + // Publisher for check_update_suggest calibration_status_pub_ = create_publisher( "/accel_brake_map_calibrator/output/calibration_status", durable_qos); @@ -103,7 +103,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod updater_ptr_ = std::make_shared(this, 1.0 / update_hz_); updater_ptr_->setHardwareID("accel_brake_map_calibrator"); updater_ptr_->add( - "accel_brake_map_calibrator", this, &AccelBrakeMapCalibrator::checkUpdateSuggest); + "accel_brake_map_calibrator", this, &AccelBrakeMapCalibrator::check_update_suggest); { csv_default_map_dir_ = declare_parameter("csv_default_map_dir", std::string("")); @@ -132,7 +132,7 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod std::string output_log_file = declare_parameter("output_log_file", std::string("")); output_log_.open(output_log_file); - addIndexToCSV(&output_log_); + add_index_to_csv(&output_log_); debug_values_.data.resize(num_debug_values_); @@ -169,15 +169,15 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod // initialize matrix for covariance calculation { - const auto genConstMat = [](const Map & map, const auto val) { + const auto gen_const_mat = [](const Map & map, const auto val) { return Eigen::MatrixXd::Constant(map.size(), map.at(0).size(), val); }; - accel_data_mean_mat_ = genConstMat(accel_map_value_, map_offset_); - brake_data_mean_mat_ = genConstMat(brake_map_value_, map_offset_); - accel_data_covariance_mat_ = genConstMat(accel_map_value_, covariance_); - brake_data_covariance_mat_ = genConstMat(brake_map_value_, covariance_); - accel_data_num_ = genConstMat(accel_map_value_, 1); - brake_data_num_ = genConstMat(brake_map_value_, 1); + accel_data_mean_mat_ = gen_const_mat(accel_map_value_, map_offset_); + brake_data_mean_mat_ = gen_const_mat(brake_map_value_, map_offset_); + accel_data_covariance_mat_ = gen_const_mat(accel_map_value_, covariance_); + brake_data_covariance_mat_ = gen_const_mat(brake_map_value_, covariance_); + accel_data_num_ = gen_const_mat(accel_map_value_, 1); + brake_data_num_ = gen_const_mat(brake_map_value_, 1); } // publisher @@ -208,52 +208,36 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod using std::placeholders::_2; using std::placeholders::_3; - velocity_sub_ = create_subscription( - "~/input/velocity", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackVelocity, this, _1)); - steer_sub_ = create_subscription( - "~/input/steer", queue_size, std::bind(&AccelBrakeMapCalibrator::callbackSteer, this, _1)); - if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::STATUS) { - actuation_status_sub_ = create_subscription( - "~/input/actuation_status", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackActuationStatus, this, _1)); - } - if (accel_brake_value_source_ == ACCEL_BRAKE_SOURCE::COMMAND) { - actuation_cmd_sub_ = create_subscription( - "~/input/actuation_cmd", queue_size, - std::bind(&AccelBrakeMapCalibrator::callbackActuationCommand, this, _1)); - } - // Service update_map_dir_server_ = create_service( "~/input/update_map_dir", - std::bind(&AccelBrakeMapCalibrator::callbackUpdateMapService, this, _1, _2, _3)); + std::bind(&AccelBrakeMapCalibrator::callback_update_map_service, this, _1, _2, _3)); // timer - initTimer(1.0 / update_hz_); - initOutputCSVTimer(30.0); + init_timer(1.0 / update_hz_); + init_output_csv_timer(30.0); logger_configure_ = std::make_unique(this); } -void AccelBrakeMapCalibrator::initOutputCSVTimer(double period_s) +void AccelBrakeMapCalibrator::init_output_csv_timer(double period_s) { const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); timer_output_csv_ = rclcpp::create_timer( this, get_clock(), period_ns, - std::bind(&AccelBrakeMapCalibrator::timerCallbackOutputCSV, this)); + std::bind(&AccelBrakeMapCalibrator::timer_callback_output_csv, this)); } -void AccelBrakeMapCalibrator::initTimer(double period_s) +void AccelBrakeMapCalibrator::init_timer(double period_s) { const auto period_ns = std::chrono::duration_cast(std::chrono::duration(period_s)); timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&AccelBrakeMapCalibrator::timerCallback, this)); + this, get_clock(), period_ns, std::bind(&AccelBrakeMapCalibrator::fetch_data, this)); } -bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) +bool AccelBrakeMapCalibrator::get_current_pitch_from_tf(double * pitch) { if (get_pitch_method_ == GET_PITCH_METHOD::NONE) { // do not get pitch from tf @@ -269,15 +253,53 @@ bool AccelBrakeMapCalibrator::getCurrentPitchFromTF(double * pitch) get_logger(), *get_clock(), 5000, "cannot get map to base_link transform. "); return false; } - double roll, raw_pitch, yaw; + double roll = 0.0; + double raw_pitch = 0.0; + double yaw = 0.0; tf2::getEulerYPR(transform->transform.rotation, roll, raw_pitch, yaw); - debug_values_.data.at(CURRENT_RAW_PITCH) = raw_pitch; + debug_values_.data.at(CURRENT_RAW_PITCH) = static_cast(raw_pitch); *pitch = lowpass(*pitch, raw_pitch, 0.2); - debug_values_.data.at(CURRENT_PITCH) = *pitch; + debug_values_.data.at(CURRENT_PITCH) = static_cast(*pitch); + return true; +} + +bool AccelBrakeMapCalibrator::fetch_data() +{ + // take data from subscribers + // take actuation data + ActuationStatusStamped::ConstSharedPtr actuation_status_ptr = actuation_status_sub_.takeData(); + ActuationCommandStamped::ConstSharedPtr actuation_cmd_ptr = actuation_cmd_sub_.takeData(); + if (actuation_status_ptr) { + take_actuation_status(actuation_status_ptr); + } else if (actuation_cmd_ptr) { + take_actuation_command(actuation_cmd_ptr); + } else { + return false; + } + + // take velocity data + VelocityReport::ConstSharedPtr velocity_ptr = velocity_sub_.takeData(); + if (!velocity_ptr) return false; + take_velocity(velocity_ptr); + + // take steer data + steer_ptr_ = steer_sub_.takeData(); + + /* valid check */ + // data check + if ( + !twist_ptr_ || !steer_ptr_ || !accel_pedal_ptr_ || !brake_pedal_ptr_ || + !delayed_accel_pedal_ptr_ || !delayed_brake_pedal_ptr_) { + // lack of data + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), 5000, "lack of topics (twist, steer, accel, brake)"); + lack_of_data_count_++; + return false; + } return true; } -void AccelBrakeMapCalibrator::timerCallback() +void AccelBrakeMapCalibrator::fetch_data() { update_count_++; @@ -296,24 +318,16 @@ void AccelBrakeMapCalibrator::timerCallback() << "\n\t" << "update_fail_count_: " << update_fail_count_ << "\n"); - /* valid check */ + // if cannot get data, return this callback + if (!fetch_data()) return; - // data check - if ( - !twist_ptr_ || !steer_ptr_ || !accel_pedal_ptr_ || !brake_pedal_ptr_ || - !delayed_accel_pedal_ptr_ || !delayed_brake_pedal_ptr_) { - // lack of data - RCLCPP_WARN_STREAM_THROTTLE( - get_logger(), *get_clock(), 5000, "lack of topics (twist, steer, accel, brake)"); - lack_of_data_count_++; - return; - } + debug_values_.data.at(CURRENT_STEER) = steer_ptr_->steering_tire_angle; // data check 2 if ( - isTimeout(twist_ptr_->header.stamp, timeout_sec_) || - isTimeout(steer_ptr_->stamp, timeout_sec_) || isTimeout(accel_pedal_ptr_, timeout_sec_) || - isTimeout(brake_pedal_ptr_, timeout_sec_)) { + is_timeout(twist_ptr_->header.stamp, timeout_sec_) || + is_timeout(steer_ptr_->stamp, timeout_sec_) || is_timeout(accel_pedal_ptr_, timeout_sec_) || + is_timeout(brake_pedal_ptr_, timeout_sec_)) { RCLCPP_WARN_STREAM_THROTTLE( get_logger(), *get_clock(), 5000, "timeout of topics (twist, steer, accel, brake)"); lack_of_data_count_++; @@ -321,7 +335,7 @@ void AccelBrakeMapCalibrator::timerCallback() } // data check 3 - if (!getCurrentPitchFromTF(&pitch_)) { + if (!get_current_pitch_from_tf(&pitch_)) { // cannot get pitch RCLCPP_WARN_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot get pitch"); failed_to_get_pitch_count_++; @@ -330,25 +344,25 @@ void AccelBrakeMapCalibrator::timerCallback() /* write data to log */ if (pedal_accel_graph_output_) { - addLogToCSV( + add_log_to_csv( &output_log_, rclcpp::Time(twist_ptr_->header.stamp).seconds(), twist_ptr_->twist.linear.x, - acceleration_, getPitchCompensatedAcceleration(), delayed_accel_pedal_ptr_->data, + acceleration_, get_pitch_compensated_acceleration(), delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, accel_pedal_speed_, brake_pedal_speed_, pitch_, steer_ptr_->steering_tire_angle, jerk_, full_original_accel_rmse_, part_original_accel_rmse_, new_accel_rmse_); } /* publish map & debug_values*/ - publishMap(accel_map_value_, brake_map_value_, "original"); - publishMap(update_accel_map_value_, update_brake_map_value_, "update"); - publishOffsetCovMap(accel_offset_covariance_value_, brake_offset_covariance_value_); - publishCountMap(); - publishIndex(); - publishUpdateSuggestFlag(); + publish_map(accel_map_value_, brake_map_value_, "original"); + publish_map(update_accel_map_value_, update_brake_map_value_, "update"); + publish_offset_cov_map(accel_offset_covariance_value_, brake_offset_covariance_value_); + publish_count_map(); + publish_index(); + publish_update_suggest_flag(); debug_pub_->publish(debug_values_); - publishFloat32("current_map_error", part_original_accel_rmse_); - publishFloat32("updated_map_error", new_accel_rmse_); - publishFloat32( + publish_float32("current_map_error", part_original_accel_rmse_); + publish_float32("updated_map_error", new_accel_rmse_); + publish_float32( "map_error_ratio", part_original_accel_rmse_ != 0.0 ? new_accel_rmse_ / part_original_accel_rmse_ : 1.0); @@ -366,7 +380,7 @@ void AccelBrakeMapCalibrator::timerCallback() } // accel / brake map evaluation (do not evaluate when the car stops) - executeEvaluation(); + execute_evaluation(); // pitch check if (std::fabs(pitch_) > max_pitch_threshold_) { @@ -411,7 +425,7 @@ void AccelBrakeMapCalibrator::timerCallback() } /* update map */ - if (updateAccelBrakeMap()) { + if (update_accel_brake_map()) { update_success_count_++; debug_values_.data.at(SUCCESS_TO_UPDATE) = true; update_success_ = true; @@ -420,22 +434,24 @@ void AccelBrakeMapCalibrator::timerCallback() } } -void AccelBrakeMapCalibrator::timerCallbackOutputCSV() +void AccelBrakeMapCalibrator::timer_callback_output_csv() { // write accel/ brake map to file const auto ros_time = std::to_string(this->now().seconds()); - writeMapToCSV(accel_vel_index_, accel_pedal_index_, update_accel_map_value_, output_accel_file_); - writeMapToCSV(brake_vel_index_, brake_pedal_index_, update_brake_map_value_, output_brake_file_); + write_map_to_csv( + accel_vel_index_, accel_pedal_index_, update_accel_map_value_, output_accel_file_); + write_map_to_csv( + brake_vel_index_, brake_pedal_index_, update_brake_map_value_, output_brake_file_); if (progress_file_output_) { - writeMapToCSV( + write_map_to_csv( accel_vel_index_, accel_pedal_index_, update_accel_map_value_, output_accel_file_ + "_" + ros_time); - writeMapToCSV( + write_map_to_csv( brake_vel_index_, brake_pedal_index_, update_brake_map_value_, output_brake_file_ + "_" + ros_time); - writeMapToCSV( + write_map_to_csv( accel_vel_index_, accel_pedal_index_, accel_map_value_, output_accel_file_ + "_original"); - writeMapToCSV( + write_map_to_csv( brake_vel_index_, brake_pedal_index_, brake_map_value_, output_brake_file_ + "_original"); } @@ -458,7 +474,7 @@ void AccelBrakeMapCalibrator::timerCallbackOutputCSV() } } -void AccelBrakeMapCalibrator::callbackVelocity(const VelocityReport::ConstSharedPtr msg) +void AccelBrakeMapCalibrator::take_velocity(const VelocityReport::ConstSharedPtr msg) { // convert velocity-report to twist-stamped auto twist_msg = std::make_shared(); @@ -468,12 +484,12 @@ void AccelBrakeMapCalibrator::callbackVelocity(const VelocityReport::ConstShared twist_msg->twist.angular.z = msg->heading_rate; if (!twist_vec_.empty()) { - const auto past_msg = getNearestTimeDataFromVec(twist_msg, dif_twist_time_, twist_vec_); - const double raw_acceleration = getAccel(past_msg, twist_msg); + const auto past_msg = get_nearest_time_data_from_vec(twist_msg, dif_twist_time_, twist_vec_); + const double raw_acceleration = get_accel(past_msg, twist_msg); acceleration_ = lowpass(acceleration_, raw_acceleration, 0.25); acceleration_time_ = rclcpp::Time(msg->header.stamp).seconds(); - debug_values_.data.at(CURRENT_RAW_ACCEL) = raw_acceleration; - debug_values_.data.at(CURRENT_ACCEL) = acceleration_; + debug_values_.data.at(CURRENT_RAW_ACCEL) = static_cast(raw_acceleration); + debug_values_.data.at(CURRENT_ACCEL) = static_cast(acceleration_); // calculate jerk if ( @@ -482,81 +498,75 @@ void AccelBrakeMapCalibrator::callbackVelocity(const VelocityReport::ConstShared RCLCPP_DEBUG_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, "cannot calculate jerk"); // does not update jerk } else { - const double raw_jerk = getJerk(); + const double raw_jerk = get_jerk(); // to avoid to get delayed jerk, set high gain jerk_ = lowpass(jerk_, raw_jerk, 0.5); } - debug_values_.data.at(CURRENT_JERK) = jerk_; + debug_values_.data.at(CURRENT_JERK) = static_cast(jerk_); pre_acceleration_ = acceleration_; pre_acceleration_time_ = acceleration_time_; } - debug_values_.data.at(CURRENT_SPEED) = twist_msg->twist.linear.x; + debug_values_.data.at(CURRENT_SPEED) = static_cast(twist_msg->twist.linear.x); twist_ptr_ = twist_msg; - pushDataToVec(twist_msg, twist_vec_max_size_, &twist_vec_); -} - -void AccelBrakeMapCalibrator::callbackSteer(const SteeringReport::ConstSharedPtr msg) -{ - debug_values_.data.at(CURRENT_STEER) = msg->steering_tire_angle; - steer_ptr_ = msg; + push_data_to_vec(twist_msg, twist_vec_max_size_, &twist_vec_); } -void AccelBrakeMapCalibrator::callbackActuation( - const std_msgs::msg::Header header, const double accel, const double brake) +void AccelBrakeMapCalibrator::take_actuation( + const std_msgs::msg::Header & header, const double accel, const double brake) { // get accel data accel_pedal_ptr_ = std::make_shared(accel, rclcpp::Time(header.stamp)); if (!accel_pedal_vec_.empty()) { const auto past_accel_ptr = - getNearestTimeDataFromVec(accel_pedal_ptr_, dif_pedal_time_, accel_pedal_vec_); + get_nearest_time_data_from_vec(accel_pedal_ptr_, dif_pedal_time_, accel_pedal_vec_); const double raw_accel_pedal_speed = - getPedalSpeed(past_accel_ptr, accel_pedal_ptr_, accel_pedal_speed_); + get_pedal_speed(past_accel_ptr, accel_pedal_ptr_, accel_pedal_speed_); accel_pedal_speed_ = lowpass(accel_pedal_speed_, raw_accel_pedal_speed, 0.5); - debug_values_.data.at(CURRENT_RAW_ACCEL_SPEED) = raw_accel_pedal_speed; - debug_values_.data.at(CURRENT_ACCEL_SPEED) = accel_pedal_speed_; + debug_values_.data.at(CURRENT_RAW_ACCEL_SPEED) = static_cast(raw_accel_pedal_speed); + debug_values_.data.at(CURRENT_ACCEL_SPEED) = static_cast(accel_pedal_speed_); } - debug_values_.data.at(CURRENT_ACCEL_PEDAL) = accel_pedal_ptr_->data; - pushDataToVec(accel_pedal_ptr_, pedal_vec_max_size_, &accel_pedal_vec_); + debug_values_.data.at(CURRENT_ACCEL_PEDAL) = static_cast(accel_pedal_ptr_->data); + push_data_to_vec(accel_pedal_ptr_, pedal_vec_max_size_, &accel_pedal_vec_); delayed_accel_pedal_ptr_ = - getNearestTimeDataFromVec(accel_pedal_ptr_, pedal_to_accel_delay_, accel_pedal_vec_); + get_nearest_time_data_from_vec(accel_pedal_ptr_, pedal_to_accel_delay_, accel_pedal_vec_); // get brake data brake_pedal_ptr_ = std::make_shared(brake, rclcpp::Time(header.stamp)); if (!brake_pedal_vec_.empty()) { const auto past_brake_ptr = - getNearestTimeDataFromVec(brake_pedal_ptr_, dif_pedal_time_, brake_pedal_vec_); + get_nearest_time_data_from_vec(brake_pedal_ptr_, dif_pedal_time_, brake_pedal_vec_); const double raw_brake_pedal_speed = - getPedalSpeed(past_brake_ptr, brake_pedal_ptr_, brake_pedal_speed_); + get_pedal_speed(past_brake_ptr, brake_pedal_ptr_, brake_pedal_speed_); brake_pedal_speed_ = lowpass(brake_pedal_speed_, raw_brake_pedal_speed, 0.5); - debug_values_.data.at(CURRENT_RAW_BRAKE_SPEED) = raw_brake_pedal_speed; - debug_values_.data.at(CURRENT_BRAKE_SPEED) = brake_pedal_speed_; + debug_values_.data.at(CURRENT_RAW_BRAKE_SPEED) = static_cast(raw_brake_pedal_speed); + debug_values_.data.at(CURRENT_BRAKE_SPEED) = static_cast(brake_pedal_speed_); } - debug_values_.data.at(CURRENT_BRAKE_PEDAL) = brake_pedal_ptr_->data; - pushDataToVec(brake_pedal_ptr_, pedal_vec_max_size_, &brake_pedal_vec_); + debug_values_.data.at(CURRENT_BRAKE_PEDAL) = static_cast(brake_pedal_ptr_->data); + push_data_to_vec(brake_pedal_ptr_, pedal_vec_max_size_, &brake_pedal_vec_); delayed_brake_pedal_ptr_ = - getNearestTimeDataFromVec(brake_pedal_ptr_, pedal_to_accel_delay_, brake_pedal_vec_); + get_nearest_time_data_from_vec(brake_pedal_ptr_, pedal_to_accel_delay_, brake_pedal_vec_); } -void AccelBrakeMapCalibrator::callbackActuationCommand( +void AccelBrakeMapCalibrator::take_actuation_command( const ActuationCommandStamped::ConstSharedPtr msg) { const auto header = msg->header; const auto accel = msg->actuation.accel_cmd; const auto brake = msg->actuation.brake_cmd; - callbackActuation(header, accel, brake); + take_actuation(header, accel, brake); } -void AccelBrakeMapCalibrator::callbackActuationStatus( +void AccelBrakeMapCalibrator::take_actuation_status( const ActuationStatusStamped::ConstSharedPtr msg) { const auto header = msg->header; const auto accel = msg->status.accel_status; const auto brake = msg->status.brake_status; - callbackActuation(header, accel, brake); + take_actuation(header, accel, brake); } -bool AccelBrakeMapCalibrator::callbackUpdateMapService( +bool AccelBrakeMapCalibrator::callback_update_map_service( [[maybe_unused]] const std::shared_ptr request_header, UpdateAccelBrakeMap::Request::SharedPtr req, UpdateAccelBrakeMap::Response::SharedPtr res) { @@ -567,8 +577,10 @@ bool AccelBrakeMapCalibrator::callbackUpdateMapService( const auto accel_map_file = update_map_dir + "/accel_map.csv"; const auto brake_map_file = update_map_dir + "/brake_map.csv"; if ( - writeMapToCSV(accel_vel_index_, accel_pedal_index_, update_accel_map_value_, accel_map_file) && - writeMapToCSV(brake_vel_index_, brake_pedal_index_, update_brake_map_value_, brake_map_file)) { + write_map_to_csv( + accel_vel_index_, accel_pedal_index_, update_accel_map_value_, accel_map_file) && + write_map_to_csv( + brake_vel_index_, brake_pedal_index_, update_brake_map_value_, brake_map_file)) { res->success = true; res->message = "Data has been successfully saved on " + update_map_dir + "/(accel/brake)_map.csv"; @@ -585,7 +597,7 @@ double AccelBrakeMapCalibrator::lowpass( return current * gain + original * (1.0 - gain); } -double AccelBrakeMapCalibrator::getPedalSpeed( +double AccelBrakeMapCalibrator::get_pedal_speed( const DataStampedPtr & prev_pedal, const DataStampedPtr & current_pedal, const double prev_pedal_speed) { @@ -599,9 +611,9 @@ double AccelBrakeMapCalibrator::getPedalSpeed( return d_pedal / dt; } -double AccelBrakeMapCalibrator::getAccel( +double AccelBrakeMapCalibrator::get_accel( const TwistStamped::ConstSharedPtr & prev_twist, - const TwistStamped::ConstSharedPtr & current_twist) + const TwistStamped::ConstSharedPtr & current_twist) const { const double dt = (rclcpp::Time(current_twist->header.stamp) - rclcpp::Time(prev_twist->header.stamp)).seconds(); @@ -613,16 +625,16 @@ double AccelBrakeMapCalibrator::getAccel( return std::min(std::max(min_accel_, dv / dt), max_accel_); } -double AccelBrakeMapCalibrator::getJerk() +double AccelBrakeMapCalibrator::get_jerk() { const double jerk = (acceleration_ - pre_acceleration_) / (acceleration_time_ - pre_acceleration_time_); return std::min(std::max(-max_jerk_, jerk), max_jerk_); } -bool AccelBrakeMapCalibrator::indexValueSearch( - const std::vector value_index, const double value, const double value_thresh, - int * searched_index) +bool AccelBrakeMapCalibrator::index_value_search( + const std::vector & value_index, const double value, const double value_thresh, + int * searched_index) const { if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { /* return lower left index. @@ -639,7 +651,7 @@ bool AccelBrakeMapCalibrator::indexValueSearch( for (std::size_t i = 0; i < value_index.size(); i++) { const double diff_value = value_index.at(i) - value; if (diff_value <= 0 && old_diff_value < diff_value) { - *searched_index = i; + *searched_index = static_cast(i); old_diff_value = diff_value; } else { return true; @@ -649,7 +661,7 @@ bool AccelBrakeMapCalibrator::indexValueSearch( for (std::size_t i = 0; i < value_index.size(); i++) { const double diff_value = std::fabs(value_index.at(i) - value); if (diff_value <= value_thresh) { - *searched_index = i; + *searched_index = static_cast(i); return true; } } @@ -657,8 +669,8 @@ bool AccelBrakeMapCalibrator::indexValueSearch( return false; } -int AccelBrakeMapCalibrator::nearestValueSearch( - const std::vector value_index, const double value) +int AccelBrakeMapCalibrator::nearest_value_search( + const std::vector & value_index, const double value) { double max_dist = std::numeric_limits::max(); int nearest_idx = 0; @@ -666,40 +678,34 @@ int AccelBrakeMapCalibrator::nearestValueSearch( for (std::size_t i = 0; i < value_index.size(); i++) { const double dist = std::fabs(value - value_index.at(i)); if (max_dist > dist) { - nearest_idx = i; + nearest_idx = static_cast(i); max_dist = dist; } } return nearest_idx; } -int AccelBrakeMapCalibrator::nearestPedalSearch() +int AccelBrakeMapCalibrator::nearest_pedal_search() { - bool accel_mode; - - if (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()) { - accel_mode = true; - } else { - accel_mode = false; - } + bool accel_mode = (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()); - int nearest_idx; + int nearest_idx = 0; if (accel_mode) { - nearest_idx = nearestValueSearch(accel_pedal_index_, delayed_accel_pedal_ptr_->data); + nearest_idx = nearest_value_search(accel_pedal_index_, delayed_accel_pedal_ptr_->data); } else { - nearest_idx = nearestValueSearch(brake_pedal_index_, delayed_brake_pedal_ptr_->data); + nearest_idx = nearest_value_search(brake_pedal_index_, delayed_brake_pedal_ptr_->data); } - return getUnifiedIndexFromAccelBrakeIndex(accel_mode, nearest_idx); + return get_unified_index_from_accel_brake_index(accel_mode, nearest_idx); } -int AccelBrakeMapCalibrator::nearestVelSearch() +int AccelBrakeMapCalibrator::nearest_vel_search() { const double current_vel = twist_ptr_->twist.linear.x; - return nearestValueSearch(accel_vel_index_, current_vel); + return nearest_value_search(accel_vel_index_, current_vel); } -void AccelBrakeMapCalibrator::takeConsistencyOfAccelMap() +void AccelBrakeMapCalibrator::take_consistency_of_accel_map() { const double bit = std::pow(1e-01, precision_); for (std::size_t ped_idx = 0; ped_idx < update_accel_map_value_.size() - 1; ped_idx++) { @@ -723,7 +729,7 @@ void AccelBrakeMapCalibrator::takeConsistencyOfAccelMap() } } -void AccelBrakeMapCalibrator::takeConsistencyOfBrakeMap() +void AccelBrakeMapCalibrator::take_consistency_of_brake_map() { const double bit = std::pow(1e-01, precision_); for (std::size_t ped_idx = 0; ped_idx < update_brake_map_value_.size() - 1; ped_idx++) { @@ -745,7 +751,7 @@ void AccelBrakeMapCalibrator::takeConsistencyOfBrakeMap() } } -bool AccelBrakeMapCalibrator::updateAccelBrakeMap() +bool AccelBrakeMapCalibrator::update_accel_brake_map() { // get pedal index bool accel_mode = false; @@ -754,14 +760,10 @@ bool AccelBrakeMapCalibrator::updateAccelBrakeMap() int accel_vel_index = 0; int brake_vel_index = 0; - if (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()) { - accel_mode = true; - } else { - accel_mode = false; - } + accel_mode = (delayed_accel_pedal_ptr_->data > std::numeric_limits::epsilon()); if ( - accel_mode && !indexValueSearch( + accel_mode && !index_value_search( accel_pedal_index_, delayed_accel_pedal_ptr_->data, pedal_diff_threshold_, &accel_pedal_index)) { // not match accel pedal output to pedal value in index @@ -769,7 +771,7 @@ bool AccelBrakeMapCalibrator::updateAccelBrakeMap() } if ( - !accel_mode && !indexValueSearch( + !accel_mode && !index_value_search( brake_pedal_index_, delayed_brake_pedal_ptr_->data, pedal_diff_threshold_, &brake_pedal_index)) { // not match accel pedal output to pedal value in index @@ -778,7 +780,7 @@ bool AccelBrakeMapCalibrator::updateAccelBrakeMap() if ( accel_mode && - !indexValueSearch( + !index_value_search( accel_vel_index_, twist_ptr_->twist.linear.x, velocity_diff_threshold_, &accel_vel_index)) { // not match current velocity to velocity value in index return false; @@ -786,14 +788,15 @@ bool AccelBrakeMapCalibrator::updateAccelBrakeMap() if ( !accel_mode && - !indexValueSearch( + !index_value_search( brake_vel_index_, twist_ptr_->twist.linear.x, velocity_diff_threshold_, &brake_vel_index)) { // not match current velocity to velocity value in index return false; } // update map - executeUpdate(accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index); + execute_update( + accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index); // when update 0 pedal index, update another map if (accel_mode && accel_pedal_index == 0) { @@ -815,57 +818,58 @@ bool AccelBrakeMapCalibrator::updateAccelBrakeMap() } // take consistency of map - takeConsistencyOfAccelMap(); - takeConsistencyOfBrakeMap(); + take_consistency_of_accel_map(); + take_consistency_of_brake_map(); return true; } -void AccelBrakeMapCalibrator::executeUpdate( +void AccelBrakeMapCalibrator::execute_update( const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, const int brake_pedal_index, const int brake_vel_index) { - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); + const double measured_acc = acceleration_ - get_pitch_compensated_acceleration(); const double map_acc = accel_mode ? update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) : update_brake_map_value_.at(brake_pedal_index).at(brake_vel_index); RCLCPP_DEBUG_STREAM(get_logger(), "measured_acc: " << measured_acc << ", map_acc: " << map_acc); if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_EACH_CELL) { - updateEachValOffset( + update_each_val_offset( accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index, measured_acc, map_acc); } else if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_TOTAL) { - updateTotalMapOffset(measured_acc, map_acc); + update_total_map_offset(measured_acc, map_acc); } else if (update_method_ == UPDATE_METHOD::UPDATE_OFFSET_FOUR_CELL_AROUND) { - updateFourCellAroundOffset( + update_four_cell_around_offset( accel_mode, accel_pedal_index, accel_vel_index, brake_pedal_index, brake_vel_index, measured_acc); } // add accel data to map - accel_mode ? map_value_data_.at(getUnifiedIndexFromAccelBrakeIndex(true, accel_pedal_index)) - .at(accel_vel_index) - .emplace_back(measured_acc) - : map_value_data_.at(getUnifiedIndexFromAccelBrakeIndex(false, brake_pedal_index)) - .at(brake_vel_index) - .emplace_back(measured_acc); + accel_mode + ? map_value_data_.at(get_unified_index_from_accel_brake_index(true, accel_pedal_index)) + .at(accel_vel_index) + .emplace_back(measured_acc) + : map_value_data_.at(get_unified_index_from_accel_brake_index(false, brake_pedal_index)) + .at(brake_vel_index) + .emplace_back(measured_acc); } -bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( +bool AccelBrakeMapCalibrator::update_four_cell_around_offset( const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, const int brake_pedal_index, const int brake_vel_index, const double measured_acc) { // pre-defined - static Map accel_map_offset_vec_( + static Map accelMapOffsetVec_( accel_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); - static Map brake_map_offset_vec_( + static Map brakeMapOffsetVec_( brake_map_value_.size(), std::vector(accel_map_value_.at(0).size(), map_offset_)); - static std::vector> accel_covariance_mat_( + static std::vector> accelCovarianceMat_( accel_map_value_.size() - 1, std::vector( accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); - static std::vector> brake_covariance_mat_( + static std::vector> brakeCovarianceMat_( brake_map_value_.size() - 1, std::vector( accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_)); @@ -880,8 +884,8 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( const auto & vel_index_ = accel_mode ? accel_vel_index_ : brake_vel_index_; const auto & delayed_pedal = accel_mode ? delayed_accel_pedal_ptr_->data : delayed_brake_pedal_ptr_->data; - auto & map_offset_vec = accel_mode ? accel_map_offset_vec_ : brake_map_offset_vec_; - auto & covariance_mat = accel_mode ? accel_covariance_mat_ : brake_covariance_mat_; + auto & map_offset_vec = accel_mode ? accelMapOffsetVec_ : brakeMapOffsetVec_; + auto & covariance_mat = accel_mode ? accelCovarianceMat_ : brakeCovarianceMat_; auto & data_mean_mat = accel_mode ? accel_data_mean_mat_ : brake_data_mean_mat_; auto & data_covariance_mat = accel_mode ? accel_data_covariance_mat_ : brake_data_covariance_mat_; auto & data_weighted_num = accel_mode ? accel_data_num_ : brake_data_num_; @@ -937,18 +941,18 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( Eigen::MatrixXd covariance = covariance_mat.at(ped_idx_l).at(vel_idx_l); /* calculate adaptive map offset */ - Eigen::MatrixXd G(4, 4); - Eigen::RowVectorXd phiT(4); - phiT = phi.transpose(); - double rk = phiT * covariance * phi; + Eigen::MatrixXd matrixG(4, 4); + Eigen::RowVectorXd phi_t(4); + phi_t = phi.transpose(); + double rk = phi_t * covariance * phi; - G = covariance * phi / (forgetting_factor_ + rk); + matrixG = covariance * phi / (forgetting_factor_ + rk); double beta = rk > 0 ? (forgetting_factor_ - (1 - forgetting_factor_) / rk) : 1; - covariance = covariance - covariance * phi * phiT * covariance / (1 / beta + rk); // anti-windup - double eta_hat = phiT * theta; + covariance = covariance - covariance * phi * phi_t * covariance / (1 / beta + rk); // anti-windup + double eta_hat = phi_t * theta; const double error_map_offset = measured_acc - eta_hat; - updated_map_offset = map_offset + G * error_map_offset; + updated_map_offset = map_offset + matrixG * error_map_offset; for (int i = 0; i < 4; i++) { const double pre_mean = mean(i); @@ -1000,24 +1004,24 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset( return true; } -bool AccelBrakeMapCalibrator::updateEachValOffset( +bool AccelBrakeMapCalibrator::update_each_val_offset( const bool accel_mode, const int accel_pedal_index, const int accel_vel_index, const int brake_pedal_index, const int brake_vel_index, const double measured_acc, const double map_acc) { // pre-defined - static Map map_offset_vec_( + static Map map_offset_vec( accel_map_value_.size() + brake_map_value_.size() - 1, std::vector(accel_map_value_.at(0).size(), map_offset_)); - static Map covariance_vec_( + static Map covariance_vec( accel_map_value_.size() + brake_map_value_.size() - 1, std::vector(accel_map_value_.at(0).size(), covariance_)); const int vel_idx = accel_mode ? accel_vel_index : brake_vel_index; int ped_idx = accel_mode ? accel_pedal_index : brake_pedal_index; - ped_idx = getUnifiedIndexFromAccelBrakeIndex(accel_mode, ped_idx); - double map_offset = map_offset_vec_.at(ped_idx).at(vel_idx); - double covariance = covariance_vec_.at(ped_idx).at(vel_idx); + ped_idx = get_unified_index_from_accel_brake_index(accel_mode, ped_idx); + double map_offset = map_offset_vec.at(ped_idx).at(vel_idx); + double covariance = covariance_vec.at(ped_idx).at(vel_idx); /* calculate adaptive map offset */ const double phi = 1.0; @@ -1032,13 +1036,13 @@ bool AccelBrakeMapCalibrator::updateEachValOffset( RCLCPP_DEBUG_STREAM( get_logger(), "index: " << ped_idx << ", " << vel_idx - << ": map_offset_ = " << map_offset_vec_.at(ped_idx).at(vel_idx) + << ": map_offset_ = " << map_offset_vec.at(ped_idx).at(vel_idx) << " -> " << map_offset << "\t" << " covariance = " << covariance); /* input calculated result and update map */ - map_offset_vec_.at(ped_idx).at(vel_idx) = map_offset; - covariance_vec_.at(ped_idx).at(vel_idx) = covariance; + map_offset_vec.at(ped_idx).at(vel_idx) = map_offset; + covariance_vec.at(ped_idx).at(vel_idx) = covariance; if (accel_mode) { update_accel_map_value_.at(accel_pedal_index).at(accel_vel_index) = accel_map_value_.at(accel_pedal_index).at(accel_vel_index) + map_offset; @@ -1049,7 +1053,8 @@ bool AccelBrakeMapCalibrator::updateEachValOffset( return true; } -void AccelBrakeMapCalibrator::updateTotalMapOffset(const double measured_acc, const double map_acc) +void AccelBrakeMapCalibrator::update_total_map_offset( + const double measured_acc, const double map_acc) { /* calculate adaptive map offset */ const double phi = 1.0; @@ -1080,19 +1085,18 @@ void AccelBrakeMapCalibrator::updateTotalMapOffset(const double measured_acc, co } } -std::vector AccelBrakeMapCalibrator::getMapColumnFromUnifiedIndex( +std::vector AccelBrakeMapCalibrator::get_map_column_from_unified_index( const Map & accel_map_value, const Map & brake_map_value, const std::size_t index) { if (index < brake_map_value.size()) { // input brake map value return brake_map_value.at(brake_map_value.size() - index - 1); - } else { - // input accel map value - return accel_map_value.at(index - brake_map_value.size() + 1); } + // input accel map value + return accel_map_value.at(index - brake_map_value.size() + 1); } -double AccelBrakeMapCalibrator::getPedalValueFromUnifiedIndex(const std::size_t index) +double AccelBrakeMapCalibrator::get_pedal_value_from_unified_index(const std::size_t index) { if (index < brake_pedal_index_.size()) { // brake index ( minus ) @@ -1103,64 +1107,64 @@ double AccelBrakeMapCalibrator::getPedalValueFromUnifiedIndex(const std::size_t } } -int AccelBrakeMapCalibrator::getUnifiedIndexFromAccelBrakeIndex( +int AccelBrakeMapCalibrator::get_unified_index_from_accel_brake_index( const bool accel_map, const std::size_t index) { // accel_map=true: accel_map; accel_map=false: brake_map if (accel_map) { // accel map - return index + brake_map_value_.size() - 1; - } else { - // brake map - return brake_map_value_.size() - index - 1; + return static_cast(index) + static_cast(brake_map_value_.size()) - 1; } + // brake map + return static_cast(brake_map_value_.size()) - static_cast(index) - 1; } -double AccelBrakeMapCalibrator::getPitchCompensatedAcceleration() +double AccelBrakeMapCalibrator::get_pitch_compensated_acceleration() const { // It works correctly only when the velocity is positive. constexpr double gravity = 9.80665; return gravity * std::sin(pitch_); } -void AccelBrakeMapCalibrator::executeEvaluation() +void AccelBrakeMapCalibrator::execute_evaluation() { - const double full_orig_accel_sq_error = calculateAccelSquaredError( + const double full_orig_accel_sq_error = calculate_accel_squared_error( delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, accel_map_, brake_map_); - pushDataToVec(full_orig_accel_sq_error, full_mse_que_size_, &full_original_accel_mse_que_); - full_original_accel_rmse_ = getAverage(full_original_accel_mse_que_); + push_data_to_vec(full_orig_accel_sq_error, full_mse_que_size_, &full_original_accel_mse_que_); + full_original_accel_rmse_ = get_average(full_original_accel_mse_que_); // std::cerr << "rmse : " << sqrt(full_original_accel_rmse_) << std::endl; - const double full_orig_accel_l1_error = calculateAccelErrorL1Norm( + const double full_orig_accel_l1_error = calculate_accel_error_l1_norm( delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, accel_map_, brake_map_); const double full_orig_accel_sq_l1_error = full_orig_accel_l1_error * full_orig_accel_l1_error; - pushDataToVec(full_orig_accel_l1_error, full_mse_que_size_, &full_original_accel_l1_que_); - pushDataToVec(full_orig_accel_sq_l1_error, full_mse_que_size_, &full_original_accel_sq_l1_que_); - full_original_accel_error_l1norm_ = getAverage(full_original_accel_l1_que_); + push_data_to_vec(full_orig_accel_l1_error, full_mse_que_size_, &full_original_accel_l1_que_); + push_data_to_vec( + full_orig_accel_sq_l1_error, full_mse_que_size_, &full_original_accel_sq_l1_que_); + full_original_accel_error_l1norm_ = get_average(full_original_accel_l1_que_); /*calculate l1norm_covariance*/ - // const double full_original_accel_error_sql1_ = getAverage(full_original_accel_sq_l1_que_); + // const double full_original_accel_error_sql1_ = get_average(full_original_accel_sq_l1_que_); // std::cerr << "error_l1norm : " << full_original_accel_error_l1norm_ << std::endl; // std::cerr << "error_l1_cov : " << // full_original_accel_error_sql1_-full_original_accel_error_l1norm_*full_original_accel_error_l1norm_ // << std::endl; - const double part_orig_accel_sq_error = calculateAccelSquaredError( + const double part_orig_accel_sq_error = calculate_accel_squared_error( delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, accel_map_, brake_map_); - pushDataToVec(part_orig_accel_sq_error, part_mse_que_size_, &part_original_accel_mse_que_); - part_original_accel_rmse_ = getAverage(part_original_accel_mse_que_); + push_data_to_vec(part_orig_accel_sq_error, part_mse_que_size_, &part_original_accel_mse_que_); + part_original_accel_rmse_ = get_average(part_original_accel_mse_que_); - const double new_accel_sq_error = calculateAccelSquaredError( + const double new_accel_sq_error = calculate_accel_squared_error( delayed_accel_pedal_ptr_->data, delayed_brake_pedal_ptr_->data, twist_ptr_->twist.linear.x, new_accel_map_, new_brake_map_); - pushDataToVec(new_accel_sq_error, part_mse_que_size_, &new_accel_mse_que_); - new_accel_rmse_ = getAverage(new_accel_mse_que_); + push_data_to_vec(new_accel_sq_error, part_mse_que_size_, &new_accel_mse_que_); + new_accel_rmse_ = get_average(new_accel_mse_que_); } -double AccelBrakeMapCalibrator::calculateEstimatedAcc( +double AccelBrakeMapCalibrator::calculate_estimated_acc( const double throttle, const double brake, const double vel, AccelMap & accel_map, BrakeMap & brake_map) { @@ -1176,27 +1180,27 @@ double AccelBrakeMapCalibrator::calculateEstimatedAcc( return estimated_acc; } -double AccelBrakeMapCalibrator::calculateAccelSquaredError( +double AccelBrakeMapCalibrator::calculate_accel_squared_error( const double throttle, const double brake, const double vel, AccelMap & accel_map, BrakeMap & brake_map) { - const double estimated_acc = calculateEstimatedAcc(throttle, brake, vel, accel_map, brake_map); - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); + const double estimated_acc = calculate_estimated_acc(throttle, brake, vel, accel_map, brake_map); + const double measured_acc = acceleration_ - get_pitch_compensated_acceleration(); const double dif_acc = measured_acc - estimated_acc; return dif_acc * dif_acc; } -double AccelBrakeMapCalibrator::calculateAccelErrorL1Norm( +double AccelBrakeMapCalibrator::calculate_accel_error_l1_norm( const double throttle, const double brake, const double vel, AccelMap & accel_map, BrakeMap & brake_map) { - const double estimated_acc = calculateEstimatedAcc(throttle, brake, vel, accel_map, brake_map); - const double measured_acc = acceleration_ - getPitchCompensatedAcceleration(); + const double estimated_acc = calculate_estimated_acc(throttle, brake, vel, accel_map, brake_map); + const double measured_acc = acceleration_ - get_pitch_compensated_acceleration(); const double dif_acc = measured_acc - estimated_acc; return abs(dif_acc); } -void AccelBrakeMapCalibrator::pushDataToQue( +void AccelBrakeMapCalibrator::push_data_to_que( const TwistStamped::ConstSharedPtr & data, const std::size_t max_size, std::queue * que) { @@ -1207,7 +1211,7 @@ void AccelBrakeMapCalibrator::pushDataToQue( } template -void AccelBrakeMapCalibrator::pushDataToVec( +void AccelBrakeMapCalibrator::push_data_to_vec( const T data, const std::size_t max_size, std::vector * vec) { vec->emplace_back(data); @@ -1217,7 +1221,7 @@ void AccelBrakeMapCalibrator::pushDataToVec( } template -T AccelBrakeMapCalibrator::getNearestTimeDataFromVec( +T AccelBrakeMapCalibrator::get_nearest_time_data_from_vec( const T base_data, const double back_time, const std::vector & vec) { double nearest_time = std::numeric_limits::max(); @@ -1234,7 +1238,7 @@ T AccelBrakeMapCalibrator::getNearestTimeDataFromVec( return nearest_time_data; } -DataStampedPtr AccelBrakeMapCalibrator::getNearestTimeDataFromVec( +DataStampedPtr AccelBrakeMapCalibrator::get_nearest_time_data_from_vec( DataStampedPtr base_data, const double back_time, const std::vector & vec) { double nearest_time = std::numeric_limits::max(); @@ -1251,7 +1255,7 @@ DataStampedPtr AccelBrakeMapCalibrator::getNearestTimeDataFromVec( return nearest_time_data; } -double AccelBrakeMapCalibrator::getAverage(const std::vector & vec) +double AccelBrakeMapCalibrator::get_average(const std::vector & vec) { if (vec.empty()) { return 0.0; @@ -1261,47 +1265,47 @@ double AccelBrakeMapCalibrator::getAverage(const std::vector & vec) for (const auto num : vec) { sum += num; } - return sum / vec.size(); + return sum / static_cast(vec.size()); } -double AccelBrakeMapCalibrator::getStandardDeviation(const std::vector & vec) +double AccelBrakeMapCalibrator::get_standard_deviation(const std::vector & vec) { if (vec.empty()) { return 0.0; } - const double ave = getAverage(vec); + const double ave = get_average(vec); double sum = 0.0; for (const auto num : vec) { sum += std::pow(num - ave, 2); } - return std::sqrt(sum / vec.size()); + return std::sqrt(sum / static_cast(vec.size())); } -bool AccelBrakeMapCalibrator::isTimeout( +bool AccelBrakeMapCalibrator::is_timeout( const builtin_interfaces::msg::Time & stamp, const double timeout_sec) { const double dt = this->now().seconds() - rclcpp::Time(stamp).seconds(); return dt > timeout_sec; } -bool AccelBrakeMapCalibrator::isTimeout( +bool AccelBrakeMapCalibrator::is_timeout( const DataStampedPtr & data_stamped, const double timeout_sec) { const double dt = (this->now() - data_stamped->data_time).seconds(); return dt > timeout_sec; } -OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( - const std::string frame_id, const double height, const double width, const double resolution, +OccupancyGrid AccelBrakeMapCalibrator::get_occ_msg( + const std::string & frame_id, const double height, const double width, const double resolution, const std::vector & map_value) { OccupancyGrid occ; occ.header.frame_id = frame_id; occ.header.stamp = this->now(); - occ.info.height = height; - occ.info.width = width; + occ.info.height = static_cast(height); + occ.info.width = static_cast(width); occ.info.map_load_time = this->now(); occ.info.origin.position.x = 0; occ.info.origin.position.y = 0; @@ -1310,22 +1314,23 @@ OccupancyGrid AccelBrakeMapCalibrator::getOccMsg( occ.info.origin.orientation.y = 0; occ.info.origin.orientation.z = 0; occ.info.origin.orientation.w = 1; - occ.info.resolution = resolution; + occ.info.resolution = static_cast(resolution); occ.data = map_value; return occ; } // function for diagnostics -void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticStatusWrapper & stat) +void AccelBrakeMapCalibrator::check_update_suggest( + diagnostic_updater::DiagnosticStatusWrapper & stat) { using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; using CalibrationStatus = CalibrationStatus; CalibrationStatus accel_brake_map_status; - int8_t level; + int8_t level = DiagStatus::OK; std::string msg; accel_brake_map_status.target = CalibrationStatus::ACCEL_BRAKE_MAP; - if (is_default_map_ == true) { + if (is_default_map_) { accel_brake_map_status.status = CalibrationStatus::NORMAL; level = DiagStatus::OK; msg = "OK"; @@ -1357,8 +1362,8 @@ void AccelBrakeMapCalibrator::checkUpdateSuggest(diagnostic_updater::DiagnosticS // function for debug -void AccelBrakeMapCalibrator::publishMap( - const Map accel_map_value, const Map brake_map_value, const std::string publish_type) +void AccelBrakeMapCalibrator::publish_map( + const Map & accel_map_value, const Map & brake_map_value, const std::string & publish_type) { if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { RCLCPP_ERROR_STREAM( @@ -1366,29 +1371,30 @@ void AccelBrakeMapCalibrator::publishMap( "Invalid map. The number of velocity index of accel map and brake map is different."); return; } - const double h = accel_map_value.size() + brake_map_value.size() - + const double h = static_cast(accel_map_value.size()) + brake_map_value.size() - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value.at(0).size(); // velocity + const double w = static_cast(accel_map_value.at(0).size()); // velocity // publish occupancy map - const int8_t MAX_OCC_VALUE = 100; + const int8_t max_occ_value = 100; std::vector int_map_value; - int_map_value.resize(h * w); + int_map_value.resize(static_cast::size_type>(h * w)); for (int i = 0; i < h; i++) { for (int j = 0; j < w; j++) { const double value = - getMapColumnFromUnifiedIndex(accel_map_value_, brake_map_value_, i).at(j); + get_map_column_from_unified_index(accel_map_value_, brake_map_value_, i).at(j); // convert acc to 0~100 int value int8_t int_value = - static_cast(MAX_OCC_VALUE * ((value - min_accel_) / (max_accel_ - min_accel_))); - int_map_value.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_value), (int8_t)0); + static_cast(max_occ_value * ((value - min_accel_) / (max_accel_ - min_accel_))); + int_map_value.at(static_cast::size_type>(i * w + j)) = + std::max(std::min(max_occ_value, int_value), (int8_t)0); } } if (publish_type == "original") { - original_map_occ_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, int_map_value)); + original_map_occ_pub_->publish(get_occ_msg("base_link", h, w, map_resolution_, int_map_value)); } else { - update_map_occ_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, int_map_value)); + update_map_occ_pub_->publish(get_occ_msg("base_link", h, w, map_resolution_, int_map_value)); } // publish raw map @@ -1403,11 +1409,12 @@ void AccelBrakeMapCalibrator::publishMap( float_map.layout.dim[0].stride = h * w; float_map.layout.dim[1].stride = w; float_map.layout.data_offset = 0; - std::vector vec(h * w, 0); + std::vector::size_type vec_size = static_cast::size_type>(h * w); + std::vector vec(vec_size, 0); for (int i = 0; i < h; i++) { for (int j = 0; j < w; j++) { vec[i * w + j] = static_cast( - getMapColumnFromUnifiedIndex(accel_map_value_, brake_map_value_, i).at(j)); + get_map_column_from_unified_index(accel_map_value_, brake_map_value_, i).at(j)); } } float_map.data = vec; @@ -1418,7 +1425,7 @@ void AccelBrakeMapCalibrator::publishMap( } } -void AccelBrakeMapCalibrator::publishOffsetCovMap( +void AccelBrakeMapCalibrator::publish_offset_cov_map( const Map accel_map_value, const Map brake_map_value) { if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) { @@ -1446,19 +1453,20 @@ void AccelBrakeMapCalibrator::publishOffsetCovMap( std::vector vec(h * w, 0); for (int i = 0; i < h; i++) { for (int j = 0; j < w; j++) { - vec[i * w + j] = - static_cast(getMapColumnFromUnifiedIndex(accel_map_value, brake_map_value, i).at(j)); + std::vector::size_type index = static_cast::size_type>(i * w + j); + vec[index] = static_cast( + get_map_column_from_unified_index(accel_map_value, brake_map_value, i).at(j)); } } float_map.data = vec; offset_covariance_pub_->publish(float_map); } -void AccelBrakeMapCalibrator::publishFloat32(const std::string publish_type, const double val) +void AccelBrakeMapCalibrator::publish_float32(const std::string & publish_type, const double val) { Float32Stamped msg; msg.stamp = this->now(); - msg.data = val; + msg.data = static_cast(val); if (publish_type == "current_map_error") { current_map_error_pub_->publish(msg); } else if (publish_type == "updated_map_error") { @@ -1468,7 +1476,7 @@ void AccelBrakeMapCalibrator::publishFloat32(const std::string publish_type, con } } -void AccelBrakeMapCalibrator::publishCountMap() +void AccelBrakeMapCalibrator::publish_count_map() { if (accel_map_value_.at(0).size() != brake_map_value_.at(0).size()) { RCLCPP_ERROR_STREAM( @@ -1476,76 +1484,84 @@ void AccelBrakeMapCalibrator::publishCountMap() "Invalid map. The number of velocity index of accel map and brake map is different."); return; } - const double h = accel_map_value_.size() + brake_map_value_.size() - - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value_.at(0).size(); // velocity - const int8_t MAX_OCC_VALUE = 100; + const double h = static_cast( + accel_map_value_.size() + brake_map_value_.size() - + 1); // pedal (accel_map_value(0) and brake_map_value(0) is same.) + + const double w = static_cast(accel_map_value_.at(0).size()); // velocity + const int8_t max_occ_value = 100; - std::vector count_map, ave_map, std_map; - count_map.resize(h * w); - ave_map.resize(h * w); - std_map.resize(h * w); + std::vector count_map; + std::vector ave_map; + std::vector std_map; + count_map.resize(static_cast::size_type>(h * w)); + ave_map.resize(static_cast::size_type>(h * w)); + std_map.resize(static_cast::size_type>(h * w)); for (int i = 0; i < h; i++) { for (int j = 0; j < w; j++) { const auto data_vec = map_value_data_.at(i).at(j); if (data_vec.empty()) { // input *UNKNOWN* value - count_map.at(i * w + j) = -1; - ave_map.at(i * w + j) = -1; + count_map.at(static_cast::size_type>(i * w + j)) = -1; + ave_map.at(static_cast::size_type>(i * w + j)) = -1; } else { const auto count_rate = - MAX_OCC_VALUE * (static_cast(data_vec.size()) / max_data_count_); - count_map.at(i * w + j) = static_cast( - std::max(std::min(static_cast(MAX_OCC_VALUE), static_cast(count_rate)), 0)); + max_occ_value * (static_cast(data_vec.size()) / max_data_count_); + count_map.at(static_cast::size_type>(i * w + j)) = static_cast( + std::max(std::min(static_cast(max_occ_value), static_cast(count_rate)), 0)); // calculate average { - const double average = getAverage(data_vec); - int8_t int_average = static_cast( - MAX_OCC_VALUE * ((average - min_accel_) / (max_accel_ - min_accel_))); - ave_map.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_average), (int8_t)0); + const double average = get_average(data_vec); + int8_t int_average = static_cast( + max_occ_value * ((average - min_accel_) / (max_accel_ - min_accel_))); + ave_map.at(static_cast::size_type>(i * w + j)) = + std::max(std::min(max_occ_value, int_average), static_cast(0)); } // calculate standard deviation { - const double std_dev = getStandardDeviation(data_vec); + const double std_dev = get_standard_deviation(data_vec); const double max_std_dev = 0.2; const double min_std_dev = 0.0; int8_t int_std_dev = static_cast( - MAX_OCC_VALUE * ((std_dev - min_std_dev) / (max_std_dev - min_std_dev))); - std_map.at(i * w + j) = std::max(std::min(MAX_OCC_VALUE, int_std_dev), (int8_t)0); + max_occ_value * ((std_dev - min_std_dev) / (max_std_dev - min_std_dev))); + std_map.at(i * w + j) = std::max(std::min(max_occ_value, int_std_dev), (int8_t)0); } } } } // publish average map / standard dev map / count map - data_ave_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, ave_map)); - data_std_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, std_map)); - data_count_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, count_map)); + data_ave_pub_->publish(get_occ_msg("base_link", h, w, map_resolution_, ave_map)); + data_std_pub_->publish(get_occ_msg("base_link", h, w, map_resolution_, std_map)); + data_count_pub_->publish(get_occ_msg("base_link", h, w, map_resolution_, count_map)); // publish count with self pos map - int nearest_pedal_idx = nearestPedalSearch(); - int nearest_vel_idx = nearestVelSearch(); + int nearest_pedal_idx = nearest_pedal_search(); + int nearest_vel_idx = nearest_vel_search(); // input self pose (to max value / min value ) and publish this update_success_ - ? count_map.at(nearest_pedal_idx * w + nearest_vel_idx) = std::numeric_limits::max() - : count_map.at(nearest_pedal_idx * w + nearest_vel_idx) = std::numeric_limits::min(); - data_count_with_self_pose_pub_->publish(getOccMsg("base_link", h, w, map_resolution_, count_map)); + ? count_map.at(static_cast::size_type>( + nearest_pedal_idx * w + nearest_vel_idx)) = std::numeric_limits::max() + : count_map.at(static_cast::size_type>( + nearest_pedal_idx * w + nearest_vel_idx)) = std::numeric_limits::min(); + data_count_with_self_pose_pub_->publish( + get_occ_msg("base_link", h, w, map_resolution_, count_map)); } -void AccelBrakeMapCalibrator::publishIndex() +void AccelBrakeMapCalibrator::publish_index() { MarkerArray markers; - const double h = accel_map_value_.size() + brake_map_value_.size() - + const double h = static_cast(accel_map_value_.size()) + brake_map_value_.size() - 1; // pedal (accel_map_value(0) and brake_map_value(0) is same.) - const double w = accel_map_value_.at(0).size(); // velocity + const double w = static_cast(accel_map_value_.at(0).size()); // velocity visualization_msgs::msg::Marker marker; marker.header.frame_id = "base_link"; marker.header.stamp = this->now(); - marker.type = marker.TEXT_VIEW_FACING; - marker.action = marker.ADD; + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; std_msgs::msg::ColorRGBA color; color.a = 0.999; color.b = 0.1; @@ -1561,7 +1577,7 @@ void AccelBrakeMapCalibrator::publishIndex() // pedal value for (int pedal_idx = 0; pedal_idx < h; pedal_idx++) { - const double pedal_value = getPedalValueFromUnifiedIndex(pedal_idx); + const double pedal_value = get_pedal_value_from_unified_index(pedal_idx); marker.ns = "occ_pedal_index"; marker.id = pedal_idx; marker.pose.position.x = -map_resolution_ * 0.5; @@ -1575,7 +1591,7 @@ void AccelBrakeMapCalibrator::publishIndex() // pedal index name marker.ns = "occ_pedal_index"; - marker.id = h; + marker.id = static_cast(h); marker.pose.position.x = -map_resolution_ * 0.5; marker.pose.position.y = map_resolution_ * (0.5 + h); marker.scale.z = 0.03; @@ -1598,7 +1614,7 @@ void AccelBrakeMapCalibrator::publishIndex() // velocity index name marker.ns = "occ_vel_index"; - marker.id = w; + marker.id = static_cast(w); marker.pose.position.x = map_resolution_ * (0.5 + w); marker.pose.position.y = -map_resolution_ * 0.2; marker.scale.z = 0.02; @@ -1608,7 +1624,7 @@ void AccelBrakeMapCalibrator::publishIndex() index_pub_->publish(markers); } -void AccelBrakeMapCalibrator::publishUpdateSuggestFlag() +void AccelBrakeMapCalibrator::publish_update_suggest_flag() { std_msgs::msg::Bool update_suggest; @@ -1628,7 +1644,7 @@ void AccelBrakeMapCalibrator::publishUpdateSuggestFlag() update_suggest_pub_->publish(update_suggest); } -bool AccelBrakeMapCalibrator::writeMapToCSV( +bool AccelBrakeMapCalibrator::write_map_to_csv( std::vector vel_index, std::vector pedal_index, Map value_map, std::string filename) { @@ -1667,7 +1683,7 @@ bool AccelBrakeMapCalibrator::writeMapToCSV( return true; } -void AccelBrakeMapCalibrator::addIndexToCSV(std::ofstream * csv_file) +void AccelBrakeMapCalibrator::add_index_to_csv(std::ofstream * csv_file) { *csv_file << "timestamp,velocity,accel,pitch_comp_accel,final_accel,accel_pedal,brake_pedal," << "accel_pedal_speed,brake_pedal_speed,pitch,steer,jerk,full_original_accel_rmse, " @@ -1675,7 +1691,7 @@ void AccelBrakeMapCalibrator::addIndexToCSV(std::ofstream * csv_file) << std::endl; } -void AccelBrakeMapCalibrator::addLogToCSV( +void AccelBrakeMapCalibrator::add_log_to_csv( std::ofstream * csv_file, const double & timestamp, const double velocity, const double accel, const double pitched_accel, const double accel_pedal, const double brake_pedal, const double accel_pedal_speed, const double brake_pedal_speed, const double pitch, @@ -1691,4 +1707,4 @@ void AccelBrakeMapCalibrator::addLogToCSV( << ", " << new_accel_mse << "," << rmse_rate << std::endl; } -} // namespace accel_brake_map_calibrator +} // namespace accel_brake_map_calibrator \ No newline at end of file diff --git a/vehicle/steer_offset_estimator/CMakeLists.txt b/vehicle/autoware_steer_offset_estimator/CMakeLists.txt similarity index 86% rename from vehicle/steer_offset_estimator/CMakeLists.txt rename to vehicle/autoware_steer_offset_estimator/CMakeLists.txt index eec8b2e24bec7..80ae467ed1f85 100644 --- a/vehicle/steer_offset_estimator/CMakeLists.txt +++ b/vehicle/autoware_steer_offset_estimator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(steer_offset_estimator) +project(autoware_steer_offset_estimator) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) @@ -18,7 +18,7 @@ ament_auto_add_library(steer_offset_estimator_node SHARED ) rclcpp_components_register_node(steer_offset_estimator_node - PLUGIN "steer_offset_estimator::SteerOffsetEstimatorNode" + PLUGIN "autoware::steer_offset_estimator::SteerOffsetEstimatorNode" EXECUTABLE steer_offset_estimator ) diff --git a/vehicle/steer_offset_estimator/Readme.md b/vehicle/autoware_steer_offset_estimator/Readme.md similarity index 100% rename from vehicle/steer_offset_estimator/Readme.md rename to vehicle/autoware_steer_offset_estimator/Readme.md diff --git a/vehicle/steer_offset_estimator/config/steer_offset_estimator.param.yaml b/vehicle/autoware_steer_offset_estimator/config/steer_offset_estimator.param.yaml similarity index 100% rename from vehicle/steer_offset_estimator/config/steer_offset_estimator.param.yaml rename to vehicle/autoware_steer_offset_estimator/config/steer_offset_estimator.param.yaml diff --git a/vehicle/steer_offset_estimator/image/kinematic_constraints.png b/vehicle/autoware_steer_offset_estimator/image/kinematic_constraints.png similarity index 100% rename from vehicle/steer_offset_estimator/image/kinematic_constraints.png rename to vehicle/autoware_steer_offset_estimator/image/kinematic_constraints.png diff --git a/vehicle/steer_offset_estimator/image/steer_offset.png b/vehicle/autoware_steer_offset_estimator/image/steer_offset.png similarity index 100% rename from vehicle/steer_offset_estimator/image/steer_offset.png rename to vehicle/autoware_steer_offset_estimator/image/steer_offset.png diff --git a/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp b/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp similarity index 87% rename from vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp rename to vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp index afd1d0520e951..4e03cbe0162fe 100644 --- a/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp +++ b/vehicle/autoware_steer_offset_estimator/include/autoware_steer_offset_estimator/steer_offset_estimator_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ -#define STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ +#ifndef AUTOWARE_STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ +#define AUTOWARE_STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" @@ -24,7 +24,7 @@ #include -namespace steer_offset_estimator +namespace autoware::steer_offset_estimator { using geometry_msgs::msg::TwistStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -64,6 +64,6 @@ class SteerOffsetEstimatorNode : public rclcpp::Node public: explicit SteerOffsetEstimatorNode(const rclcpp::NodeOptions & node_options); }; -} // namespace steer_offset_estimator +} // namespace autoware::steer_offset_estimator -#endif // STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ +#endif // AUTOWARE_STEER_OFFSET_ESTIMATOR__STEER_OFFSET_ESTIMATOR_NODE_HPP_ diff --git a/vehicle/steer_offset_estimator/launch/steer_offset_estimator.launch.xml b/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml similarity index 77% rename from vehicle/steer_offset_estimator/launch/steer_offset_estimator.launch.xml rename to vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml index 8b98537c75500..9ee05189a35d8 100644 --- a/vehicle/steer_offset_estimator/launch/steer_offset_estimator.launch.xml +++ b/vehicle/autoware_steer_offset_estimator/launch/steer_offset_estimator.launch.xml @@ -1,6 +1,6 @@ - + @@ -12,7 +12,7 @@ - + diff --git a/vehicle/steer_offset_estimator/package.xml b/vehicle/autoware_steer_offset_estimator/package.xml similarity index 95% rename from vehicle/steer_offset_estimator/package.xml rename to vehicle/autoware_steer_offset_estimator/package.xml index 636d901d71ef5..4246f8dee8a4e 100644 --- a/vehicle/steer_offset_estimator/package.xml +++ b/vehicle/autoware_steer_offset_estimator/package.xml @@ -1,6 +1,6 @@ - steer_offset_estimator + autoware_steer_offset_estimator 0.1.0 The steer_offset_estimator Taiki Tanaka diff --git a/vehicle/steer_offset_estimator/schema/steer_offset_estimator.schema.json b/vehicle/autoware_steer_offset_estimator/schema/steer_offset_estimator.schema.json similarity index 100% rename from vehicle/steer_offset_estimator/schema/steer_offset_estimator.schema.json rename to vehicle/autoware_steer_offset_estimator/schema/steer_offset_estimator.schema.json diff --git a/vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp b/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp similarity index 95% rename from vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp rename to vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp index 86c76dc6f2f26..fdb5fdb404c0c 100644 --- a/vehicle/steer_offset_estimator/src/steer_offset_estimator_node.cpp +++ b/vehicle/autoware_steer_offset_estimator/src/steer_offset_estimator_node.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "steer_offset_estimator/steer_offset_estimator_node.hpp" +#include "autoware_steer_offset_estimator/steer_offset_estimator_node.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include #include -namespace steer_offset_estimator +namespace autoware::steer_offset_estimator { SteerOffsetEstimatorNode::SteerOffsetEstimatorNode(const rclcpp::NodeOptions & node_options) : Node("steer_offset_estimator", node_options) @@ -137,7 +137,7 @@ void SteerOffsetEstimatorNode::onTimer() pub_steer_offset_cov_->publish(std::move(cov_msg)); } } -} // namespace steer_offset_estimator +} // namespace autoware::steer_offset_estimator #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(steer_offset_estimator::SteerOffsetEstimatorNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::steer_offset_estimator::SteerOffsetEstimatorNode)