diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 2cf484c9fbe34..36b1af9887ed9 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 @@ -144,22 +144,28 @@ 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_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_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/autoware_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp -planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/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/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 @@ -171,41 +177,38 @@ planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp s planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp +planning/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/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp -planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp planning/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 +243,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.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/cppcheck-all.yaml b/.github/workflows/cppcheck-daily.yaml similarity index 97% rename from .github/workflows/cppcheck-all.yaml rename to .github/workflows/cppcheck-daily.yaml index db3bd5d259895..6a18bfb988b08 100644 --- a/.github/workflows/cppcheck-all.yaml +++ b/.github/workflows/cppcheck-daily.yaml @@ -1,13 +1,12 @@ -name: cppcheck-all +name: cppcheck-daily on: - pull_request: schedule: - cron: 0 0 * * * workflow_dispatch: jobs: - cppcheck-all: + cppcheck-daily: runs-on: ubuntu-latest steps: diff --git a/.github/workflows/openai-pr-reviewer.yaml b/.github/workflows/openai-pr-reviewer.yaml deleted file mode 100644 index 4ecbd93198276..0000000000000 --- a/.github/workflows/openai-pr-reviewer.yaml +++ /dev/null @@ -1,39 +0,0 @@ -name: Code Review By ChatGPT - -permissions: - contents: read - pull-requests: write - -on: - pull_request_target: - types: - - labeled - pull_request_review_comment: - types: [created] - -concurrency: - group: ${{ github.repository }}-${{ github.event.number || github.head_ref || - github.sha }}-${{ github.workflow }}-${{ github.event_name == - 'pull_request_review_comment' && 'pr_comment' || 'pr' }} - cancel-in-progress: ${{ github.event_name != 'pull_request_review_comment' }} - -jobs: - prevent-no-label-execution: - uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 - with: - label: tag:openai-pr-reviewer - review: - needs: prevent-no-label-execution - if: ${{ needs.prevent-no-label-execution.outputs.run == 'true' }} - runs-on: ubuntu-latest - steps: - - uses: fluxninja/openai-pr-reviewer@latest - env: - GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - OPENAI_API_KEY: ${{ secrets.OPENAI_API_KEY }} - with: - debug: false - review_simple_changes: false - review_comment_lgtm: false - openai_light_model: gpt-3.5-turbo-0613 - openai_heavy_model: gpt-3.5-turbo-0613 # The default is to use GPT4, which needs to be changed to join ChatGPT Plus. diff --git a/.github/workflows/pr-agent.yaml b/.github/workflows/pr-agent.yaml index f4794b7497402..81b192a1c2095 100644 --- a/.github/workflows/pr-agent.yaml +++ b/.github/workflows/pr-agent.yaml @@ -26,6 +26,13 @@ jobs: env: OPENAI_KEY: ${{ secrets.OPENAI_KEY_PR_AGENT }} GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} - github_action_config.auto_review: "false" - github_action_config.auto_describe: "false" - github_action_config.auto_improve: "false" + github_action_config.auto_review: false + github_action_config.auto_describe: false + github_action_config.auto_improve: false + # https://github.com/Codium-ai/pr-agent/blob/main/pr_agent/settings/configuration.toml + pr_description.publish_labels: false + config.model: gpt-4o + config.model_turbo: gpt-4o + config.max_model_tokens: 64000 + pr_code_suggestions.max_context_tokens: 12000 + pr_code_suggestions.commitable_code_suggestions: true diff --git a/build_depends.repos b/build_depends.repos index a8f431f97df25..bd93897d81d68 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -41,3 +41,7 @@ repositories: type: git url: https://github.com/tier4/glog.git version: v0.6.0_t4-ros + universe/external/ament_cmake: # TODO(mitsudome-r): remove when https://github.com/ament/ament_cmake/pull/448 is merged + type: git + url: https://github.com/autowarefoundation/ament_cmake.git + version: feat/faster_ament_libraries_deduplicate diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 154a3019f1c17..4cf0c541510ed 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 @@ -106,7 +106,7 @@ /> - + @@ -98,7 +98,7 @@ - + @@ -232,7 +232,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index be85ee704ff95..6a7e00154256a 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 @@ -19,7 +19,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index cf96cd39043ce..7b03097e55e10 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,23 +58,23 @@ autoware_cmake autoware_behavior_velocity_planner + autoware_external_velocity_limit_selector autoware_path_optimizer + autoware_planning_topic_converter autoware_remaining_distance_time_calculator + autoware_surround_obstacle_checker autoware_velocity_smoother behavior_path_planner costmap_generator external_cmd_selector - external_velocity_limit_selector freespace_planner glog_component mission_planner obstacle_cruise_planner obstacle_stop_planner planning_evaluator - planning_topic_converter planning_validator scenario_selector - surround_obstacle_checker ament_lint_auto autoware_lint_common diff --git a/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/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 24145a7920d91..e8428788820b2 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -52,6 +52,6 @@ class StopFilter : public rclcpp::Node /** * @brief set odometry measurement */ - void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg); }; #endif // STOP_FILTER__STOP_FILTER_HPP_ diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 4d6b2c6240867..c1c4de2fb6b6e 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -31,13 +31,13 @@ StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( - "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); + "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, _1)); pub_odom_ = create_publisher("output/odom", 1); pub_stop_flag_ = create_publisher("debug/stop_flag", 1); } -void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { tier4_debug_msgs::msg::BoolStamped stop_flag_msg; stop_flag_msg.stamp = msg->header.stamp; diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index de675da5cc2d4..d4c8b71fa6b66 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -29,6 +29,7 @@ sophus std_msgs tf2_ros + tier4_autoware_utils visualization_msgs ament_lint_auto diff --git a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp index d0dfc87067f7f..1126d6260b911 100644 --- a/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp +++ b/localization/yabloc/yabloc_common/src/ll2_decomposer/ll2_decomposer_core.cpp @@ -15,7 +15,7 @@ #include "yabloc_common/ll2_decomposer/ll2_decomposer.hpp" #include -#include +#include #include #include @@ -198,7 +198,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_sign_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0.6f, 0.6f, 0.6f, 0.999f); + marker.color = tier4_autoware_utils::createMarkerColor(0.6f, 0.6f, 0.6f, 0.999f); marker.scale.x = 0.1; marker.ns = ns; marker.id = id++; @@ -228,7 +228,7 @@ Ll2Decomposer::MarkerArray Ll2Decomposer::make_polygon_marker_msg( marker.header.frame_id = "map"; marker.header.stamp = get_clock()->now(); marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0.4f, 0.4f, 0.8f, 0.999f); + marker.color = tier4_autoware_utils::createMarkerColor(0.4f, 0.4f, 0.8f, 0.999f); marker.scale.x = 0.2; marker.ns = ns; marker.id = id++; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp index 90cd62883e339..e6e59a836f6db 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp @@ -93,7 +93,8 @@ class ParticleVisualize : public rclcpp::Node marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(bound_weight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(bound_weight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp index 955ed84fdea2d..888baf0bea0ac 100644 --- a/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp @@ -45,7 +45,8 @@ void ParticleVisualizer::publish(const ParticleArray & msg) marker.scale.x = 0.3; marker.scale.y = 0.1; marker.scale.z = 0.1; - marker.color = common::color_scale::rainbow(boundWeight(p.weight)); + marker.color = + static_cast(common::color_scale::rainbow(boundWeight(p.weight))); marker.pose.orientation = p.pose.orientation; marker.pose.position.x = p.pose.position.x; marker.pose.position.y = p.pose.position.y; diff --git a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp index b321986240b69..e8c8890561a7f 100644 --- a/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp @@ -136,7 +136,7 @@ void GnssParticleCorrector::publish_marker(const Eigen::Vector3f & position, boo marker.pose.position.z = latest_height_.data; float prob = i / 4.0f; - marker.color = common::color_scale::rainbow(prob); + marker.color = static_cast(common::color_scale::rainbow(prob)); marker.color.a = 0.5f; marker.scale.x = 0.1; drawCircle(marker.points, weight_manager_.inverse_normal_pdf(prob, is_rtk_fixed)); diff --git a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp index ebd9c424edf5e..62d8ed826b373 100644 --- a/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp +++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -169,7 +169,7 @@ HierarchicalCostMap::MarkerArray HierarchicalCostMap::show_map_range() const marker.header.frame_id = "map"; marker.id = id++; marker.type = Marker::LINE_STRIP; - marker.color = common::Color(0, 0, 1.0f, 1.0f); + marker.color = tier4_autoware_utils::createMarkerColor(0, 0, 1.0f, 1.0f); marker.scale.x = 0.1; Eigen::Vector2f xy = area.real_scale(); marker.points.push_back(point_msg(xy.x(), xy.y())); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp index c6c34f0e28a42..8a5fc65e2fac3 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/marker_module.cpp @@ -42,7 +42,8 @@ void MarkerModule::publish_marker( marker.type = Marker::ARROW; marker.id = i; marker.ns = "arrow"; - marker.color = common::color_scale::rainbow(normalize(scores.at(i))); + marker.color = + static_cast(common::color_scale::rainbow(normalize(scores.at(i)))); marker.color.a = 0.5; marker.pose.position.x = position.x(); diff --git a/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/planning/.pages b/planning/.pages index 849d117b73b71..29b0f6a2cde56 100644 --- a/planning/.pages +++ b/planning/.pages @@ -14,7 +14,7 @@ nav: - '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 + - 'Side Shift': planning/autoware_behavior_path_side_shift_module - 'Start Planner': planning/behavior_path_start_planner_module - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': @@ -25,17 +25,17 @@ 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/behavior_velocity_run_out_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 @@ -56,13 +56,13 @@ nav: - 'About Path Smoother': planning/path_smoother - 'Elastic Band': planning/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': @@ -74,7 +74,7 @@ nav: - 'Static Centerline Generator': planning/autoware_static_centerline_generator - 'API and Library': - 'Costmap Generator': planning/costmap_generator - - 'External Velocity Limit Selector': planning/external_velocity_limit_selector + - '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 @@ -86,5 +86,5 @@ nav: - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md - 'Planning Test Utils': planning/planning_test_utils - 'Planning Test Manager': planning/autoware_planning_test_manager - - 'Planning Topic Converter': planning/planning_topic_converter + - 'Planning Topic Converter': planning/autoware_planning_topic_converter - 'Planning Validator': planning/planning_validator 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_side_shift_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp similarity index 87% rename from planning/behavior_path_side_shift_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 db1f5ee3560b1..244c59bf5e6f9 100644 --- a/planning/behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_side_shift_module/test/test_behavior_path_planner_node_interface.cpp @@ -57,14 +57,15 @@ std::shared_ptr generateNode() node_options.parameter_overrides(params); test_utils::updateNodeOptions( - node_options, {planning_test_utils_dir + "/config/test_common.param.yaml", - planning_test_utils_dir + "/config/test_nearest_search.param.yaml", - planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", - behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", - behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", - behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_side_shift_module") + - "/config/side_shift.param.yaml"}); + node_options, + {planning_test_utils_dir + "/config/test_common.param.yaml", + planning_test_utils_dir + "/config/test_nearest_search.param.yaml", + planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", + behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", + behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", + behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", + 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_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 91% rename from planning/behavior_velocity_intersection_module/package.xml rename to planning/autoware_behavior_velocity_intersection_module/package.xml index 3694a395b3a53..204a267594c03 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 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 100% rename from planning/behavior_velocity_intersection_module/src/debug.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/debug.cpp 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..0dc0e3aab4b87 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 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 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..bf0b5520f7a32 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 behavior_velocity_planner { /** @@ -172,6 +172,6 @@ using DecisionResult = std::variant< std::string formatDecisionResult(const DecisionResult & decision_result); -} // namespace behavior_velocity_planner::intersection +} // namespace 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..c855164e24d5b 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 behavior_velocity_planner { /** @@ -43,6 +43,6 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; -} // namespace behavior_velocity_planner::intersection +} // namespace 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..bf833ee07c155 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 behavior_velocity_planner { void IntersectionLanelets::update( @@ -79,4 +79,4 @@ void IntersectionLanelets::update( } } } -} // namespace behavior_velocity_planner::intersection +} // namespace 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..3682787237d42 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 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 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..85876ffc98380 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 behavior_velocity_planner { /** @@ -72,6 +72,6 @@ struct IntersectionStopLines */ size_t occlusion_wo_tl_pass_judge_line{0}; }; -} // namespace behavior_velocity_planner::intersection +} // namespace 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 100% rename from planning/behavior_velocity_intersection_module/src/manager.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/manager.cpp diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp similarity index 100% rename from planning/behavior_velocity_intersection_module/src/manager.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/manager.hpp diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/object_manager.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp index d2c905673cee9..8ca860e23c111 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.cpp @@ -64,7 +64,7 @@ tier4_autoware_utils::Polygon2d createOneStepPolygon( } // namespace -namespace behavior_velocity_planner::intersection +namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -201,7 +201,7 @@ std::shared_ptr ObjectInfoManager::registerObject( const bool belong_intersection_area, const bool is_parked_vehicle) { if (objects_info_.count(uuid) == 0) { - auto object = std::make_shared(uuid); + auto object = std::make_shared(uuid); objects_info_[uuid] = object; } auto object = objects_info_[uuid]; @@ -219,7 +219,7 @@ std::shared_ptr ObjectInfoManager::registerObject( void ObjectInfoManager::registerExistingObject( const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area, const bool belong_intersection_area, const bool is_parked_vehicle, - std::shared_ptr object) + std::shared_ptr object) { objects_info_[uuid] = object; if (belong_attention_area) { @@ -249,7 +249,7 @@ std::vector> ObjectInfoManager::allObjects() const return all_objects; } -std::optional findPassageInterval( +std::optional findPassageInterval( const autoware_perception_msgs::msg::PredictedPath & predicted_path, const autoware_perception_msgs::msg::Shape & shape, const lanelet::BasicPolygon2d & ego_lane_poly, const std::optional & first_attention_lane_opt, @@ -284,25 +284,25 @@ std::optional findPassageInterval( if (lanelet::geometry::inside( first_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return intersection::CollisionInterval::LanePosition::FIRST; + return CollisionInterval::LanePosition::FIRST; } } if (second_attention_lane_opt) { if (lanelet::geometry::inside( second_attention_lane_opt.value(), lanelet::BasicPoint2d(first_itr->position.x, first_itr->position.y))) { - return intersection::CollisionInterval::LanePosition::SECOND; + return CollisionInterval::LanePosition::SECOND; } } - return intersection::CollisionInterval::LanePosition::ELSE; + return CollisionInterval::LanePosition::ELSE; }(); std::vector path; for (const auto & pose : predicted_path.path) { path.push_back(pose); } - return intersection::CollisionInterval{ + return CollisionInterval{ lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; } -} // namespace behavior_velocity_planner::intersection +} // namespace 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..3d1113656c3e3 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 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 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..da89920e59a85 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 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 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 93% 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..8e50c140894e7 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -42,9 +42,6 @@ namespace 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; 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..b1cfcdee04fa6 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -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; /** @} */ 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 94% 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..48387bc6aa398 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -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 = 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 99% 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..0fb6042e5aadb 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -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(); 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 96% 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..d4466c1e9431b 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 @@ -161,12 +161,8 @@ namespace 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; 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 95% 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..c7de1805327c3 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -118,10 +118,9 @@ namespace 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 = [&]() { 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 99% 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..be8d94c3b306d 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 @@ -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; 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 100% 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 diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp similarity index 97% rename from planning/behavior_velocity_intersection_module/src/util.cpp rename to planning/autoware_behavior_velocity_intersection_module/src/util.cpp index 9c492e7a64cde..a083185786e42 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -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; } diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp similarity index 95% rename from planning/behavior_velocity_intersection_module/src/util.hpp rename to planning/autoware_behavior_velocity_intersection_module/src/util.hpp index 878253e6943a7..27200e663503d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -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,7 +135,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); std::vector getPolygon3dFromLanelets( diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index 0bbc2fad3c5a8..acf4c1ce78e56 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -9,16 +9,16 @@ 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) - [Traffic Light](../behavior_velocity_traffic_light_module/README.md) - [Occlusion Spot](../behavior_velocity_occlusion_spot_module/README.md) - [No Stopping Area](../behavior_velocity_no_stopping_area_module/README.md) -- [Run Out](../behavior_velocity_run_out_module/README.md) +- [Run Out](../autoware_behavior_velocity_run_out_module/README.md) - [Speed Bump](../behavior_velocity_speed_bump_module/README.md) - [Out of Lane](../behavior_velocity_out_of_lane_module/README.md) diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index 3d51c6b855eb7..c90d4d01cbca5 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -65,21 +65,21 @@ 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_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_traffic_light_module - behavior_velocity_walkway_module rosidl_interface_packages diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 7cbe0f65a1fc2..2e971ed238751 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 @@ -69,9 +69,9 @@ 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("autoware::behavior_velocity_planner::WalkwayModulePlugin"); module_names.emplace_back("behavior_velocity_planner::TrafficLightModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::IntersectionModulePlugin"); + module_names.emplace_back("autoware::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"); @@ -79,7 +79,7 @@ std::shared_ptr generateNode() module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::RunOutModulePlugin"); + module_names.emplace_back("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"); @@ -98,12 +98,12 @@ 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_no_prefix("run_out"), + get_behavior_velocity_module_config("run_out"), get_behavior_velocity_module_config_no_prefix("speed_bump"), get_behavior_velocity_module_config_no_prefix("stop_line"), get_behavior_velocity_module_config_no_prefix("traffic_light"), diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/autoware_behavior_velocity_run_out_module/CMakeLists.txt similarity index 89% rename from planning/behavior_velocity_run_out_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_run_out_module/CMakeLists.txt index 3ee589ba69740..bcbe4ea12a643 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_run_out_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_run_out_module) +project(autoware_behavior_velocity_run_out_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/autoware_behavior_velocity_run_out_module/README.md similarity index 100% rename from planning/behavior_velocity_run_out_module/README.md rename to planning/autoware_behavior_velocity_run_out_module/README.md diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/autoware_behavior_velocity_run_out_module/config/run_out.param.yaml similarity index 100% rename from planning/behavior_velocity_run_out_module/config/run_out.param.yaml rename to planning/autoware_behavior_velocity_run_out_module/config/run_out.param.yaml diff --git a/planning/behavior_velocity_run_out_module/docs/calculate_expected_target_velocity.svg b/planning/autoware_behavior_velocity_run_out_module/docs/calculate_expected_target_velocity.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/calculate_expected_target_velocity.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/calculate_expected_target_velocity.svg diff --git a/planning/behavior_velocity_run_out_module/docs/collision_detection_for_shape.svg b/planning/autoware_behavior_velocity_run_out_module/docs/collision_detection_for_shape.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/collision_detection_for_shape.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/collision_detection_for_shape.svg diff --git a/planning/behavior_velocity_run_out_module/docs/collision_points.svg b/planning/autoware_behavior_velocity_run_out_module/docs/collision_points.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/collision_points.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/collision_points.svg diff --git a/planning/behavior_velocity_run_out_module/docs/create_dynamic_obstacle.svg b/planning/autoware_behavior_velocity_run_out_module/docs/create_dynamic_obstacle.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/create_dynamic_obstacle.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/create_dynamic_obstacle.svg diff --git a/planning/behavior_velocity_run_out_module/docs/create_polygon_on_path_point.svg b/planning/autoware_behavior_velocity_run_out_module/docs/create_polygon_on_path_point.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/create_polygon_on_path_point.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/create_polygon_on_path_point.svg diff --git a/planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg b/planning/autoware_behavior_velocity_run_out_module/docs/ego_cut_line.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/ego_cut_line.svg diff --git a/planning/behavior_velocity_run_out_module/docs/exclude_obstacles_by_partition.svg b/planning/autoware_behavior_velocity_run_out_module/docs/exclude_obstacles_by_partition.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/exclude_obstacles_by_partition.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/exclude_obstacles_by_partition.svg diff --git a/planning/behavior_velocity_run_out_module/docs/insert_velocity.svg b/planning/autoware_behavior_velocity_run_out_module/docs/insert_velocity.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/insert_velocity.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/insert_velocity.svg diff --git a/planning/behavior_velocity_run_out_module/docs/insert_velocity_to_approach.svg b/planning/autoware_behavior_velocity_run_out_module/docs/insert_velocity_to_approach.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/insert_velocity_to_approach.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/insert_velocity_to_approach.svg diff --git a/planning/behavior_velocity_run_out_module/docs/run_out_overview.svg b/planning/autoware_behavior_velocity_run_out_module/docs/run_out_overview.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/run_out_overview.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/run_out_overview.svg diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/autoware_behavior_velocity_run_out_module/package.xml similarity index 92% rename from planning/behavior_velocity_run_out_module/package.xml rename to planning/autoware_behavior_velocity_run_out_module/package.xml index 40e55b3de1a17..cb2c2df58ffe2 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/autoware_behavior_velocity_run_out_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_run_out_module + autoware_behavior_velocity_run_out_module 0.1.0 - The behavior_velocity_run_out_module package + The autoware_behavior_velocity_run_out_module package Tomohito Ando Makoto Kurihara diff --git a/planning/autoware_behavior_velocity_run_out_module/plugins.xml b/planning/autoware_behavior_velocity_run_out_module/plugins.xml new file mode 100644 index 0000000000000..1ddcaf8e620e1 --- /dev/null +++ b/planning/autoware_behavior_velocity_run_out_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/autoware_behavior_velocity_run_out_module/src/debug.cpp similarity index 99% rename from planning/behavior_velocity_run_out_module/src/debug.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/debug.cpp index 3f6935365c1a4..129d5299385fd 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -28,7 +28,7 @@ using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -368,4 +368,4 @@ motion_utils::VirtualWalls RunOutModule::createVirtualWalls() return debug_ptr_->createVirtualWalls(); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp similarity index 97% rename from planning/behavior_velocity_run_out_module/src/debug.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/debug.hpp index fbf682279e363..3c6d475950e7c 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -24,8 +24,9 @@ #include #include #include -namespace behavior_velocity_planner +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; @@ -149,6 +150,6 @@ class RunOutDebug double height_{0}; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp similarity index 99% rename from planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index edf1d8cf48b8e..d204c7ec6f2fc 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -30,8 +30,10 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::Point2d; +using ::behavior_velocity_planner::splineInterpolate; namespace { // create quaternion facing to the nearest trajectory point @@ -600,4 +602,4 @@ void DynamicObstacleCreatorForPoints::onSynchronizedPointCloud( std::lock_guard lock(mutex_); obstacle_points_map_filtered_ = lateral_nearest_points; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp similarity index 96% rename from planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index c7d8ef674aafd..b7ed815829a2b 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -42,11 +42,12 @@ #include #include -namespace behavior_velocity_planner +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; @@ -55,6 +56,7 @@ 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 @@ -171,6 +173,6 @@ class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator pcl::PointCloud obstacle_points_map_filtered_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // DYNAMIC_OBSTACLE_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp similarity index 97% rename from planning/behavior_velocity_run_out_module/src/manager.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/manager.cpp index fb6a70070749a..5ee50863fd162 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -20,10 +20,10 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_autoware_utils::getOrDeclareParameter; - RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { @@ -198,8 +198,9 @@ void RunOutModuleManager::setDynamicObstacleCreator( break; } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::RunOutModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::RunOutModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_run_out_module/src/manager.hpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp similarity index 86% rename from planning/behavior_velocity_run_out_module/src/manager.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/manager.hpp index ad35caf98149a..f0c49b99c99e5 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -23,8 +23,11 @@ #include -namespace behavior_velocity_planner +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: @@ -49,6 +52,6 @@ class RunOutModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.cpp b/planning/autoware_behavior_velocity_run_out_module/src/path_utils.cpp similarity index 90% rename from planning/behavior_velocity_run_out_module/src/path_utils.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/path_utils.cpp index be7db13cd3ca3..346a10646e6bc 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/path_utils.cpp @@ -15,7 +15,7 @@ #include "path_utils.hpp" #include -namespace behavior_velocity_planner::run_out_utils +namespace autoware::behavior_velocity_planner::run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( const std::vector & points, @@ -36,4 +36,4 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( return min_dist_point; } -} // namespace behavior_velocity_planner::run_out_utils +} // namespace autoware::behavior_velocity_planner::run_out_utils diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/autoware_behavior_velocity_run_out_module/src/path_utils.hpp similarity index 92% rename from planning/behavior_velocity_run_out_module/src/path_utils.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/path_utils.hpp index 8867778520bc7..62591af27af11 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/path_utils.hpp @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace run_out_utils { @@ -36,5 +36,5 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( const std::vector & target_points); } // namespace run_out_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // PATH_UTILS_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_run_out_module/src/scene.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/scene.cpp index de50b3161ea6f..351ed8e7a09b8 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -34,10 +34,14 @@ #include #include -namespace behavior_velocity_planner +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; RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, @@ -1031,4 +1035,4 @@ bool RunOutModule::isMomentaryDetection() return elapsed_time_since_detection < planner_param_.ignore_momentary_detection.time_threshold; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp similarity index 95% rename from planning/behavior_velocity_run_out_module/src/scene.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/scene.hpp index 2f797a44d06f7..85da1340a5a5d 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; @@ -39,6 +39,11 @@ 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 { @@ -177,6 +182,6 @@ class RunOutModule : public SceneModuleInterface bool isMomentaryDetection(); }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/state_machine.cpp b/planning/autoware_behavior_velocity_run_out_module/src/state_machine.cpp similarity index 96% rename from planning/behavior_velocity_run_out_module/src/state_machine.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/state_machine.cpp index 322264cad8501..484a7cdcd95e3 100644 --- a/planning/behavior_velocity_run_out_module/src/state_machine.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/state_machine.cpp @@ -14,7 +14,7 @@ #include "state_machine.hpp" -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace run_out_utils { @@ -107,4 +107,4 @@ void StateMachine::updateState(const StateInput & state_input, rclcpp::Clock & c } } // namespace run_out_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/state_machine.hpp b/planning/autoware_behavior_velocity_run_out_module/src/state_machine.hpp similarity index 92% rename from planning/behavior_velocity_run_out_module/src/state_machine.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/state_machine.hpp index 7c0de8078f435..b88aa821548bb 100644 --- a/planning/behavior_velocity_run_out_module/src/state_machine.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/state_machine.hpp @@ -19,7 +19,7 @@ #include -namespace behavior_velocity_planner::run_out_utils +namespace autoware::behavior_velocity_planner::run_out_utils { class StateMachine @@ -53,6 +53,6 @@ class StateMachine std::optional prev_obstacle_{}; std::optional target_obstacle_{}; }; -} // namespace behavior_velocity_planner::run_out_utils +} // namespace autoware::behavior_velocity_planner::run_out_utils #endif // STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp similarity index 98% rename from planning/behavior_velocity_run_out_module/src/utils.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/utils.cpp index c6eda901a1424..ec1cd460e1bb0 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -33,8 +33,11 @@ #endif #include #include -namespace behavior_velocity_planner +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) @@ -450,4 +453,4 @@ Polygons2d createMandatoryDetectionAreaPolygon( } } // namespace run_out_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp similarity index 97% rename from planning/behavior_velocity_run_out_module/src/utils.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/utils.hpp index 9208e57dcdc7f..10f856b257a61 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace run_out_utils { @@ -38,6 +38,8 @@ 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; @@ -271,5 +273,5 @@ Polygons2d createMandatoryDetectionAreaPolygon( const PathWithLaneId & path, const PlannerData & planner_data, const PlannerParam & planner_param); } // namespace run_out_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // UTILS_HPP_ 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 90% rename from planning/behavior_velocity_walkway_module/package.xml rename to planning/autoware_behavior_velocity_walkway_module/package.xml index ea6c7803717f9..59fa84acefcbb 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 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..d7f9e948154c1 --- /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..d26a47e512bc4 100644 --- a/planning/behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp @@ -22,7 +22,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; @@ -34,6 +34,7 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { @@ -98,4 +99,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 88% rename from planning/behavior_velocity_walkway_module/src/manager.cpp rename to planning/autoware_behavior_velocity_walkway_module/src/manager.cpp index d427e57009cf6..1adea85788d5b 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -23,11 +23,15 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::SceneModuleManagerInterface; using lanelet::autoware::Crosswalk; using tier4_autoware_utils::getOrDeclareParameter; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; +using ::behavior_velocity_planner::getCrosswalkIdSetOnPath; +using ::behavior_velocity_planner::getCrosswalksOnPath; WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -100,8 +104,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, + ::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 86% rename from planning/behavior_velocity_walkway_module/src/manager.hpp rename to planning/autoware_behavior_velocity_walkway_module/src/manager.hpp index 5228445bb4e03..d19bf9c1c1d5f 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -31,9 +31,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { - +using ::behavior_velocity_planner::PluginWrapper; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface @@ -56,6 +58,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 91% 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..236654e598216 100644 --- a/planning/behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -19,14 +19,22 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using ::behavior_velocity_planner::getLinestringIntersects; +using ::behavior_velocity_planner::getPolygonIntersects; +using ::behavior_velocity_planner::getStopLineFromMap; +using ::behavior_velocity_planner::PlanningBehavior; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::StopFactor; +using ::behavior_velocity_planner::VelocityFactor; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPose; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, @@ -165,4 +173,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 88% 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..1928466400134 100644 --- a/planning/behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -31,8 +31,12 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::DebugData; +using ::behavior_velocity_planner::PathWithLaneId; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::StopReason; class WalkwayModule : public SceneModuleInterface { @@ -77,6 +81,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/external_velocity_limit_selector/CMakeLists.txt b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt similarity index 73% rename from planning/external_velocity_limit_selector/CMakeLists.txt rename to planning/autoware_external_velocity_limit_selector/CMakeLists.txt index db83dcf3cad4a..40f0ebb2a3eba 100644 --- a/planning/external_velocity_limit_selector/CMakeLists.txt +++ b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(external_velocity_limit_selector) +project(autoware_external_velocity_limit_selector) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(external_velocity_limit_selector_node SHARED ) rclcpp_components_register_node(external_velocity_limit_selector_node - PLUGIN "ExternalVelocityLimitSelectorNode" + PLUGIN "autoware::external_velocity_limit_selector::ExternalVelocityLimitSelectorNode" EXECUTABLE external_velocity_limit_selector ) diff --git a/planning/external_velocity_limit_selector/README.md b/planning/autoware_external_velocity_limit_selector/README.md similarity index 100% rename from planning/external_velocity_limit_selector/README.md rename to planning/autoware_external_velocity_limit_selector/README.md diff --git a/planning/external_velocity_limit_selector/config/default.param.yaml b/planning/autoware_external_velocity_limit_selector/config/default.param.yaml similarity index 100% rename from planning/external_velocity_limit_selector/config/default.param.yaml rename to planning/autoware_external_velocity_limit_selector/config/default.param.yaml diff --git a/planning/external_velocity_limit_selector/config/default_common.param.yaml b/planning/autoware_external_velocity_limit_selector/config/default_common.param.yaml similarity index 100% rename from planning/external_velocity_limit_selector/config/default_common.param.yaml rename to planning/autoware_external_velocity_limit_selector/config/default_common.param.yaml diff --git a/planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png b/planning/autoware_external_velocity_limit_selector/image/external_velocity_limit_selector.png similarity index 100% rename from planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png rename to planning/autoware_external_velocity_limit_selector/image/external_velocity_limit_selector.png diff --git a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp similarity index 87% rename from planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp rename to planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 93fad135ac678..e4043f1aeb986 100644 --- a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ -#define EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#ifndef AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#define AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ #include @@ -26,6 +26,9 @@ #include #include +namespace autoware::external_velocity_limit_selector +{ + using tier4_debug_msgs::msg::StringStamped; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; @@ -77,5 +80,6 @@ class ExternalVelocityLimitSelectorNode : public rclcpp::Node VelocityLimit hardest_limit_{}; VelocityLimitTable velocity_limit_table_; }; +} // namespace autoware::external_velocity_limit_selector -#endif // EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#endif // AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml similarity index 74% rename from planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml rename to planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml index 5ef089f3d3ee7..42c8709b6663a 100644 --- a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml +++ b/planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -1,7 +1,7 @@ - - + + @@ -10,7 +10,7 @@ - + diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml similarity index 88% rename from planning/external_velocity_limit_selector/package.xml rename to planning/autoware_external_velocity_limit_selector/package.xml index e83966c7b876d..803e32237e938 100644 --- a/planning/external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -1,9 +1,9 @@ - external_velocity_limit_selector + autoware_external_velocity_limit_selector 0.1.0 - The external_velocity_limit_selector ROS 2 package + The autoware_external_velocity_limit_selector ROS 2 package Satoshi Ota Shinnosuke Hirakawa Shumpei Wakabayashi diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp similarity index 96% rename from planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp rename to planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index 5e557167ac3e1..008b58df303b1 100644 --- a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" +#include "autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" #include #include @@ -21,6 +21,9 @@ #include #include +namespace autoware::external_velocity_limit_selector +{ + namespace { VelocityLimit getHardestLimit( @@ -236,6 +239,8 @@ void ExternalVelocityLimitSelectorNode::updateVelocityLimit() hardest_limit_ = getHardestLimit(velocity_limit_table_, node_param_); } +} // namespace autoware::external_velocity_limit_selector #include -RCLCPP_COMPONENTS_REGISTER_NODE(ExternalVelocityLimitSelectorNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::external_velocity_limit_selector::ExternalVelocityLimitSelectorNode) diff --git a/planning/planning_topic_converter/CMakeLists.txt b/planning/autoware_planning_topic_converter/CMakeLists.txt similarity index 83% rename from planning/planning_topic_converter/CMakeLists.txt rename to planning/autoware_planning_topic_converter/CMakeLists.txt index 75b63d26c316f..ae1b585895e5e 100644 --- a/planning/planning_topic_converter/CMakeLists.txt +++ b/planning/autoware_planning_topic_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(planning_topic_converter) +project(autoware_planning_topic_converter) ### Compile options if(NOT CMAKE_CXX_STANDARD) @@ -17,7 +17,7 @@ ament_auto_add_library(planning_topic_converter SHARED ) rclcpp_components_register_node(planning_topic_converter - PLUGIN "planning_topic_converter::PathToTrajectory" + PLUGIN "autoware::planning_topic_converter::PathToTrajectory" EXECUTABLE path_to_trajectory_converter ) diff --git a/planning/planning_topic_converter/README.md b/planning/autoware_planning_topic_converter/README.md similarity index 86% rename from planning/planning_topic_converter/README.md rename to planning/autoware_planning_topic_converter/README.md index 74137cda0197d..e5036c6c0903c 100644 --- a/planning/planning_topic_converter/README.md +++ b/planning/autoware_planning_topic_converter/README.md @@ -12,7 +12,7 @@ The tools in this package are provided as composable ROS 2 component nodes, so t ```xml - + diff --git a/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp similarity index 83% rename from planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp rename to planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp index a093d9a952029..e3b0fe5069e92 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp +++ b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ -#define PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#ifndef AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#define AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ #include "rclcpp/rclcpp.hpp" #include #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { template @@ -46,6 +46,6 @@ class ConverterBase : public rclcpp::Node private: }; -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter -#endif // PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#endif // AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ diff --git a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp similarity index 75% rename from planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp rename to planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp index 708f624e97898..7ca50d6c4e50c 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp +++ b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ -#define PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#ifndef AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#define AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ -#include "planning_topic_converter/converter_base.hpp" +#include "autoware_planning_topic_converter/converter_base.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -23,7 +23,7 @@ #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { using autoware_planning_msgs::msg::Path; @@ -40,6 +40,6 @@ class PathToTrajectory : public ConverterBase void process(const Path::ConstSharedPtr msg) override; }; -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter -#endif // PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#endif // AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ diff --git a/planning/planning_topic_converter/package.xml b/planning/autoware_planning_topic_converter/package.xml similarity index 86% rename from planning/planning_topic_converter/package.xml rename to planning/autoware_planning_topic_converter/package.xml index 3699f3e53900c..be44523fa9852 100644 --- a/planning/planning_topic_converter/package.xml +++ b/planning/autoware_planning_topic_converter/package.xml @@ -1,8 +1,8 @@ - planning_topic_converter + autoware_planning_topic_converter 0.1.0 - The planning_topic_converter package + The autoware_planning_topic_converter package Satoshi OTA Shumpei Wakabayashi diff --git a/planning/planning_topic_converter/src/path_to_trajectory.cpp b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp similarity index 88% rename from planning/planning_topic_converter/src/path_to_trajectory.cpp rename to planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp index 757dcd0d9f632..d7aceb1abd106 100644 --- a/planning/planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_topic_converter/path_to_trajectory.hpp" +#include "autoware_planning_topic_converter/path_to_trajectory.hpp" #include #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { namespace { @@ -54,7 +54,7 @@ void PathToTrajectory::process(const Path::ConstSharedPtr msg) pub_->publish(output); } -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_topic_converter::PathToTrajectory) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_topic_converter::PathToTrajectory) 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 87% rename from planning/surround_obstacle_checker/package.xml rename to planning/autoware_surround_obstacle_checker/package.xml index d9b5d4615e148..3bb961766e9f3 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/autoware_surround_obstacle_checker/package.xml @@ -1,10 +1,11 @@ - 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 Satoshi Ota 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/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 2e79589c9f1bd..08a3570b1a271 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -34,7 +34,7 @@ Behavior Path Planner has following scene modules | 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) | +| 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..a646621e5b089 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -38,13 +38,14 @@ autoware_adapi_v1_msgs autoware_behavior_path_planner_common + autoware_frenet_planner + 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 @@ -54,7 +55,6 @@ 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..8788b446e4384 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 @@ -44,14 +44,16 @@ SamplingPlannerModule::SamplingPlannerModule( // 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 +73,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 +99,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 +112,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 +146,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 +163,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 +249,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 +263,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 +303,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 +356,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 +378,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 +400,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 +412,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 +504,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 +522,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 +547,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 +597,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 +608,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 +927,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 +963,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_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_run_out_module/plugins.xml b/planning/behavior_velocity_run_out_module/plugins.xml deleted file mode 100644 index 5320d46f92929..0000000000000 --- a/planning/behavior_velocity_run_out_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - 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/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 2ccd000657948..08cee3d58152e 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] 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/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/sensing/vehicle_velocity_converter/CMakeLists.txt b/sensing/vehicle_velocity_converter/CMakeLists.txt index 4f4a14bd9f045..bb50fbff90c4b 100644 --- a/sensing/vehicle_velocity_converter/CMakeLists.txt +++ b/sensing/vehicle_velocity_converter/CMakeLists.txt @@ -4,11 +4,15 @@ project(vehicle_velocity_converter) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(vehicle_velocity_converter - src/vehicle_velocity_converter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/vehicle_velocity_converter.cpp ) -ament_target_dependencies(vehicle_velocity_converter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "VehicleVelocityConverter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index 4a1a66b842892..8ef46609ee542 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -28,7 +28,7 @@ class VehicleVelocityConverter : public rclcpp::Node { public: - VehicleVelocityConverter(); + explicit VehicleVelocityConverter(const rclcpp::NodeOptions & options); ~VehicleVelocityConverter() = default; private: diff --git a/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml b/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml index 6302c09b526f9..84e1838dc89eb 100644 --- a/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml +++ b/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/sensing/vehicle_velocity_converter/package.xml b/sensing/vehicle_velocity_converter/package.xml index 39780deaccc28..b32b51bc52e10 100644 --- a/sensing/vehicle_velocity_converter/package.xml +++ b/sensing/vehicle_velocity_converter/package.xml @@ -13,6 +13,7 @@ autoware_vehicle_msgs geometry_msgs rclcpp + rclcpp_components ament_lint_auto autoware_lint_common diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 360ec4cae58d5..2aece69f5283b 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -14,7 +14,8 @@ #include "vehicle_velocity_converter/vehicle_velocity_converter.hpp" -VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_converter") +VehicleVelocityConverter::VehicleVelocityConverter(const rclcpp::NodeOptions & options) +: rclcpp::Node("vehicle_velocity_converter", options) { // set covariance value for twist with covariance msg stddev_vx_ = declare_parameter("velocity_stddev_xx"); @@ -52,3 +53,6 @@ void VehicleVelocityConverter::callbackVelocityReport( twist_with_covariance_pub_->publish(twist_with_covariance_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(VehicleVelocityConverter) diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp deleted file mode 100644 index cee25f6e6c62a..0000000000000 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2021 TierIV -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#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/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 5ff269705456d..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_; @@ -130,12 +125,18 @@ class EmergencyHandler : public rclcpp::Node void checkHazardStatusTimeout(); // Algorithm + uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE}; void transitionTo(const int new_state); 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 b96cb0d0549f9..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; @@ -164,12 +123,17 @@ void EmergencyHandler::publishControlCommands() { GearCommand msg; msg.stamp = stamp; - if (param_.use_parking_after_stopped && isStopped()) { - msg.command = GearCommand::PARK; - } else { - msg.command = GearCommand::DRIVE; - } + const auto command = [&]() { + // If stopped and use_parking is not true, send the last gear command + if (isStopped()) + return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_; + return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE; + }(); + + msg.command = command; + last_gear_command_ = msg.command; pub_gear_cmd_->publish(msg); + return; } } @@ -288,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..."); @@ -376,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) { @@ -442,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 || @@ -450,12 +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; - if (odom_->twist.twist.linear.x < th_stopped_velocity) { - return true; - } + return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); +} - return false; +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; } #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 8c58d961953ce..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 @@ -147,13 +138,19 @@ class MrmHandler : public rclcpp::Node // Algorithm bool is_emergency_holding_ = false; + uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE}; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); void handleFailedRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); 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 afcc7ebd208ab..44407c40c6787 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; @@ -191,13 +132,17 @@ void MrmHandler::publishGearCmd() GearCommand msg; msg.stamp = this->now(); - if (param_.use_parking_after_stopped && isStopped()) { - msg.command = GearCommand::PARK; - } else { - msg.command = GearCommand::DRIVE; - } - + const auto command = [&]() { + // If stopped and use_parking is not true, send the last gear command + if (isStopped()) + return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_; + return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE; + }(); + + msg.command = command; + last_gear_command_ = msg.command; pub_gear_cmd_->publish(msg); + return; } void MrmHandler::publishMrmState() @@ -348,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..."); @@ -449,48 +389,48 @@ void MrmHandler::updateMrmState() // Check emergency const bool is_emergency = isEmergency(); + // Send recovery events if is not an emergency + if (!is_emergency) { + if (mrm_state_.state != MrmState::NORMAL) transitionTo(MrmState::NORMAL); + return; + } + // 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) { - // NORMAL - if (is_auto_mode && is_emergency) { - transitionTo(MrmState::MRM_OPERATING); - return; - } - } else { - // Emergency - // Send recovery events if "not emergency" - if (!is_emergency) { - transitionTo(MrmState::NORMAL); + switch (mrm_state_.state) { + case MrmState::NORMAL: + if (is_auto_mode) { + transitionTo(MrmState::MRM_OPERATING); + } return; - } - if (mrm_state_.state == MrmState::MRM_OPERATING) { - // TODO(TetsuKawa): Check MRC is accomplished - if (mrm_state_.behavior == MrmState::PULL_OVER) { - if (isStopped() && isArrivedAtGoal()) { - transitionTo(MrmState::MRM_SUCCEEDED); - return; - } - } else { - if (isStopped()) { - transitionTo(MrmState::MRM_SUCCEEDED); - return; - } + case MrmState::MRM_OPERATING: + if (!isStopped()) return; + if (mrm_state_.behavior != MrmState::PULL_OVER) { + transitionTo(MrmState::MRM_SUCCEEDED); + return; } - } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { - const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { + if (isArrivedAtGoal()) { + transitionTo(MrmState::MRM_SUCCEEDED); + } + return; + + case MrmState::MRM_SUCCEEDED: + if (mrm_state_.behavior != getCurrentMrmBehavior()) { transitionTo(MrmState::MRM_OPERATING); } - } else if (mrm_state_.state == MrmState::MRM_FAILED) { + return; + case MrmState::MRM_FAILED: // Do nothing(only checking common recovery events) - } else { + return; + + default: { const auto msg = "invalid state: " + std::to_string(mrm_state_.state); throw std::runtime_error(msg); } + return; } } @@ -577,12 +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; - if (odom_->twist.twist.linear.x < th_stopped_velocity) { - return true; - } + return (std::abs(odom->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity); +} - return false; +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; } bool MrmHandler::isEmergency() const @@ -591,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/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)