diff --git a/.cppcheck_suppressions b/.cppcheck_suppressions new file mode 100644 index 0000000000000..5b140152e7f08 --- /dev/null +++ b/.cppcheck_suppressions @@ -0,0 +1,59 @@ +arrayIndexThenCheck +assignBoolToFloat +checkersReport +constParameterPointer +constParameterReference +constStatement +constVariable +constVariablePointer +constVariableReference +containerOutOfBounds +cstyleCast +ctuOneDefinitionRuleViolation +current_deleted_index +duplicateAssignExpression +duplicateBranch +duplicateBreak +duplicateCondition +duplicateExpression +funcArgNamesDifferent +functionConst +functionStatic +invalidPointerCast +knownConditionTrueFalse +missingInclude +missingIncludeSystem +multiCondition +noConstructor +noExplicitConstructor +noValidConfiguration +obstacle_cruise_planner +passedByValue +preprocessorErrorDirective +redundantAssignment +redundantContinue +redundantIfRemove +redundantInitialization +returnByReference +selfAssignment +shadowArgument +shadowFunction +shadowVariable +stlFindInsert +syntaxError +uninitMemberVar +unknownMacro +unmatchedSuppression +unpreciseMathCall +unreadVariable +unsignedLessThanZero +unusedFunction +unusedScopedObject +unusedStructMember +unusedVariable +useInitializationList +useStlAlgorithm +uselessCallsSubstr +uselessOverride +variableScope +virtualCallInConstructor diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index dd73e823b6c05..2cf484c9fbe34 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -147,6 +147,7 @@ perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier 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_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_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_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@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 @@ -156,7 +157,7 @@ planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp sato 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/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_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/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 @@ -174,7 +175,6 @@ planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kur 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_virtual_traffic_light_module/** kosuke.takeuchi@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 diff --git a/.github/workflows/cppcheck-all.yaml b/.github/workflows/cppcheck-all.yaml new file mode 100644 index 0000000000000..db3bd5d259895 --- /dev/null +++ b/.github/workflows/cppcheck-all.yaml @@ -0,0 +1,60 @@ +name: cppcheck-all + +on: + pull_request: + schedule: + - cron: 0 0 * * * + workflow_dispatch: + +jobs: + cppcheck-all: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Run Cppcheck on all files + continue-on-error: true + id: cppcheck + run: | + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --xml . 2> cppcheck-report.xml + shell: bash + + - name: Count errors by error ID and severity + run: | + #!/bin/bash + temp_file=$(mktemp) + grep -oP '(?<=id=")[^"]+" severity="[^"]+' cppcheck-report.xml | sed 's/" severity="/,/g' > "$temp_file" + echo "Error counts by error ID and severity:" + sort "$temp_file" | uniq -c + rm "$temp_file" + shell: bash + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.xml + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/cppcheck-differential.yaml b/.github/workflows/cppcheck-differential.yaml new file mode 100644 index 0000000000000..914abd7df86ea --- /dev/null +++ b/.github/workflows/cppcheck-differential.yaml @@ -0,0 +1,65 @@ +name: cppcheck-differential + +on: + pull_request: + +jobs: + cppcheck-differential: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Install dependencies + run: | + sudo apt-get update + sudo apt-get install -y build-essential cmake git libpcre3-dev + + # cppcheck from apt does not yet support --check-level args, and thus install from source + - name: Install Cppcheck from source + run: | + mkdir /tmp/cppcheck + git clone https://github.com/danmar/cppcheck.git /tmp/cppcheck + cd /tmp/cppcheck + git checkout 2.14.1 + mkdir build + cd build + cmake .. + make -j $(nproc) + sudo make install + + - name: Get changed files + id: changed-files + run: | + git fetch origin ${{ github.base_ref }} --depth=1 + git diff --name-only FETCH_HEAD ${{ github.sha }} > changed_files.txt + cat changed_files.txt + + - name: Run Cppcheck on changed files + continue-on-error: true + id: cppcheck + run: | + files=$(cat changed_files.txt | grep -E '\.(cpp|hpp)$' || true) + if [ -n "$files" ]; then + echo "Running Cppcheck on changed files: $files" + cppcheck --enable=all --inconclusive --check-level=exhaustive --error-exitcode=1 --suppressions-list=.cppcheck_suppressions $files 2> cppcheck-report.txt + else + echo "No C++ files changed." + touch cppcheck-report.txt + fi + shell: bash + + - name: Show cppcheck-report result + run: | + cat cppcheck-report.txt + + - name: Upload Cppcheck report + uses: actions/upload-artifact@v2 + with: + name: cppcheck-report + path: cppcheck-report.txt + + - name: Fail the job if Cppcheck failed + if: steps.cppcheck.outcome == 'failure' + run: exit 1 diff --git a/.github/workflows/dco.yaml b/.github/workflows/dco.yaml new file mode 100644 index 0000000000000..db7ace467c658 --- /dev/null +++ b/.github/workflows/dco.yaml @@ -0,0 +1,21 @@ +name: DCO +# ref: https://github.com/anchore/syft/pull/2926/files +on: + pull_request: +jobs: + dco: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + + - name: Setup Python 3.x + uses: actions/setup-python@v5 + with: + python-version: 3.x + + - name: Check DCO + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + run: | + pip3 install -U dco-check + dco-check --verbose --exclude-pattern 'pre-commit-ci\[bot\]@users\.noreply\.github\.com' diff --git a/build_depends.repos b/build_depends.repos index 32854cc34e362..a8f431f97df25 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -20,10 +20,6 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_internal_msgs.git version: main - core/external/autoware_auto_msgs: - type: git - url: https://github.com/tier4/autoware_auto_msgs.git - version: tier4/main # universe universe/external/tier4_autoware_msgs: type: git diff --git a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp deleted file mode 100644 index 1eb0a43976daa..0000000000000 --- a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp +++ /dev/null @@ -1,48 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ -#define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ - -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_planning_msgs/msg/trajectory_point.hpp" - -#include - -namespace motion_utils -{ -/** - * @brief Convert std::vector to - * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_msgs. We should consider whether to remove this function after the porting is done. - * @attention This function just clips - * std::vector up to the capacity of Trajectory. - * Therefore, the error handling out of this function is necessary if the size of the input greater - * than the capacity. - * @todo Decide how to handle the situation that we need to use the trajectory with the size of - * points larger than the capacity. (Tier IV) - */ -autoware_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory); - -/** - * @brief Convert autoware_planning_msgs::msg::Trajectory to - * std::vector. - */ -std::vector convertToTrajectoryPointArray( - const autoware_planning_msgs::msg::Trajectory & trajectory); - -} // namespace motion_utils - -#endif // MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index a7e6737cd2d42..3dc3a8bcba3f7 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -174,7 +174,7 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) } } - // Resample which exceeds the maximum size + // Resample the path with more than 100 points { std::vector resampling_vec(101); for (size_t i = 0; i < 101; ++i) { @@ -183,10 +183,9 @@ TEST(predicted_path_utils, resamplePredictedPath_by_vector) const auto resampled_path = resamplePredictedPath(path, resampling_vec); - EXPECT_EQ(resampled_path.path.size(), resampled_path.path.max_size()); EXPECT_NEAR(path.confidence, resampled_path.confidence, epsilon); - for (size_t i = 0; i < resampled_path.path.max_size(); ++i) { + for (size_t i = 0; i < resampled_path.path.size(); ++i) { EXPECT_NEAR(resampled_path.path.at(i).position.x, resampling_vec.at(i), epsilon); EXPECT_NEAR(resampled_path.path.at(i).position.y, 0.0, epsilon); EXPECT_NEAR(resampled_path.path.at(i).position.z, 0.0, epsilon); diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 27f04603d6a31..81ab2ecff790b 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -52,9 +52,9 @@ namespace autoware::motion::control::autonomous_emergency_braking { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_system_msgs::msg::AutowareState; -using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_planning_msgs::msg::Trajectory; +using autoware_system_msgs::msg::AutowareState; +using autoware_vehicle_msgs::msg::VelocityReport; using nav_msgs::msg::Odometry; using sensor_msgs::msg::Imu; using sensor_msgs::msg::PointCloud2; diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 68c070a86dd97..bf27e6b7e1575 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -14,9 +14,10 @@ ament_cmake autoware_cmake - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs motion_utils diff --git a/control/control_performance_analysis/README.md b/control/control_performance_analysis/README.md index 1fa87c2caf3a9..3895b3cc13465 100644 --- a/control/control_performance_analysis/README.md +++ b/control/control_performance_analysis/README.md @@ -25,13 +25,13 @@ Error acceleration calculations are made based on the velocity calculations abov ### Input topics -| Name | Type | Description | -| ---------------------------------------- | -------------------------------------------------------- | ------------------------------------------- | -| `/planning/scenario_planning/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Output trajectory from planning module. | -| `/control/command/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | Output control command from control module. | -| `/vehicle/status/steering_status` | autoware_auto_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | -| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | -| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | +| Name | Type | Description | +| ---------------------------------------- | ------------------------------------------ | ------------------------------------------- | +| `/planning/scenario_planning/trajectory` | autoware_planning_msgs::msg::Trajectory | Output trajectory from planning module. | +| `/control/command/control_cmd` | autoware_control_msgs::msg::Control | Output control command from control module. | +| `/vehicle/status/steering_status` | autoware_vehicle_msgs::msg::SteeringReport | Steering information from vehicle. | +| `/localization/kinematic_state` | nav_msgs::msg::Odometry | Use twist from odometry. | +| `/tf` | tf2_msgs::msg::TFMessage | Extract ego pose from tf. | ### Output topics diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 6bf8fb1c34f5e..6ecf453bf7ab3 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -24,9 +24,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ namespace control_performance_analysis { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::Error; using control_performance_analysis::msg::ErrorStamped; @@ -73,7 +73,7 @@ class ControlPerformanceAnalysisCore // Setters void setCurrentPose(const Pose & msg); void setCurrentWaypoints(const Trajectory & trajectory); - void setCurrentControlValue(const AckermannControlCommand & msg); + void setCurrentControlValue(const Control & msg); void setInterpolatedVars( const Pose & interpolated_pose, const double & interpolated_velocity, const double & interpolated_acceleration, const double & interpolated_steering_angle); @@ -100,10 +100,10 @@ class ControlPerformanceAnalysisCore Params p_; // Variables Received Outside - std::shared_ptr current_trajectory_ptr_; + std::shared_ptr current_trajectory_ptr_; std::shared_ptr current_vec_pose_ptr_; std::shared_ptr> odom_history_ptr_; // velocities at k-2, k-1, k, k+1 - std::shared_ptr current_control_ptr_; + std::shared_ptr current_control_ptr_; std::shared_ptr current_vec_steering_msg_ptr_; // State holder diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp index 2d5ab8cce002d..73d48db6400a5 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_node.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -36,9 +36,9 @@ namespace control_performance_analysis { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; using geometry_msgs::msg::PoseStamped; @@ -52,9 +52,8 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node private: // Subscribers and Local Variable Assignment rclcpp::Subscription::SharedPtr sub_trajectory_; // subscribe to trajectory - rclcpp::Subscription::SharedPtr - sub_control_cmd_; // subscribe to steering control value - rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity + rclcpp::Subscription::SharedPtr sub_control_cmd_; // subscribe to steering control value + rclcpp::Subscription::SharedPtr sub_velocity_; // subscribe to velocity rclcpp::Subscription::SharedPtr sub_vehicle_steering_; // Publishers @@ -68,26 +67,26 @@ class ControlPerformanceAnalysisNode : public rclcpp::Node // Callback Methods void onTrajectory(const Trajectory::ConstSharedPtr msg); - void onControlRaw(const AckermannControlCommand::ConstSharedPtr control_msg); + void onControlRaw(const Control::ConstSharedPtr control_msg); void onVecSteeringMeasured(const SteeringReport::ConstSharedPtr meas_steer_msg); void onVelocity(const Odometry::ConstSharedPtr msg); // Parameters Params param_{}; // wheelbase, control period and feedback coefficients. // State holder - AckermannControlCommand::ConstSharedPtr last_control_cmd_; + Control::ConstSharedPtr last_control_cmd_; double d_control_cmd_{0}; // Subscriber Parameters Trajectory::ConstSharedPtr current_trajectory_ptr_; // ConstPtr to local traj. - AckermannControlCommand::ConstSharedPtr current_control_msg_ptr_; + Control::ConstSharedPtr current_control_msg_ptr_; SteeringReport::ConstSharedPtr current_vec_steering_msg_ptr_; Odometry::ConstSharedPtr current_odom_ptr_; PoseStamped::ConstSharedPtr current_pose_; // pose of the vehicle, x, y, heading // prev states Trajectory prev_traj; - AckermannControlCommand prev_cmd; + Control prev_cmd; SteeringReport prev_steering; // Algorithm diff --git a/control/control_performance_analysis/package.xml b/control/control_performance_analysis/package.xml index 7003b7d931fa8..a26f4164a177b 100644 --- a/control/control_performance_analysis/package.xml +++ b/control/control_performance_analysis/package.xml @@ -24,9 +24,9 @@ builtin_interfaces - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs geometry_msgs libboost-dev motion_utils diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index f4ed125956bfa..51c818205e047 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -75,9 +75,9 @@ void ControlPerformanceAnalysisCore::setCurrentPose(const Pose & msg) current_vec_pose_ptr_ = std::make_shared(msg); } -void ControlPerformanceAnalysisCore::setCurrentControlValue(const AckermannControlCommand & msg) +void ControlPerformanceAnalysisCore::setCurrentControlValue(const Control & msg) { - current_control_ptr_ = std::make_shared(msg); + current_control_ptr_ = std::make_shared(msg); } std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPointIdx_path_direction() diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index 768aed321b4f0..693194e8b70a6 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -24,7 +24,7 @@ namespace { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using control_performance_analysis::msg::DrivingMonitorStamped; using control_performance_analysis::msg::ErrorStamped; @@ -62,7 +62,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( "~/input/reference_trajectory", 1, std::bind(&ControlPerformanceAnalysisNode::onTrajectory, this, _1)); - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control_raw", 1, std::bind(&ControlPerformanceAnalysisNode::onControlRaw, this, _1)); sub_vehicle_steering_ = create_subscription( @@ -93,8 +93,7 @@ void ControlPerformanceAnalysisNode::onTrajectory(const Trajectory::ConstSharedP current_trajectory_ptr_ = msg; } -void ControlPerformanceAnalysisNode::onControlRaw( - const AckermannControlCommand::ConstSharedPtr control_msg) +void ControlPerformanceAnalysisNode::onControlRaw(const Control::ConstSharedPtr control_msg) { if (last_control_cmd_) { const rclcpp::Duration & duration = diff --git a/control/control_validator/README.md b/control/control_validator/README.md index 3d78721a0a040..9c4a9be0732a5 100644 --- a/control/control_validator/README.md +++ b/control/control_validator/README.md @@ -20,11 +20,11 @@ Other features are to be implemented. The `control_validator` takes in the following inputs: -| Name | Type | Description | -| ------------------------------ | ------------------------------------- | ------------------------------------------------------------------------------ | -| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | -| `~/input/reference_trajectory` | autoware_auto_control_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | -| `~/input/predicted_trajectory` | autoware_auto_control_msgs/Trajectory | predicted trajectory which is outputted from control module | +| Name | Type | Description | +| ------------------------------ | --------------------------------- | ------------------------------------------------------------------------------ | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/reference_trajectory` | autoware_planning_msgs/Trajectory | reference trajectory which is outputted from planning module to to be followed | +| `~/input/predicted_trajectory` | autoware_planning_msgs/Trajectory | predicted trajectory which is outputted from control module | ### Outputs diff --git a/control/control_validator/include/control_validator/control_validator.hpp b/control/control_validator/include/control_validator/control_validator.hpp index eba9bf700ba08..e048ef03bf11a 100644 --- a/control/control_validator/include/control_validator/control_validator.hpp +++ b/control/control_validator/include/control_validator/control_validator.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -31,8 +31,8 @@ namespace control_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using control_validator::msg::ControlValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; diff --git a/control/control_validator/include/control_validator/debug_marker.hpp b/control/control_validator/include/control_validator/debug_marker.hpp index 794912e085949..2d3a209cbd7da 100644 --- a/control/control_validator/include/control_validator/debug_marker.hpp +++ b/control/control_validator/include/control_validator/debug_marker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -32,8 +32,7 @@ class ControlValidatorDebugMarkerPublisher explicit ControlValidatorDebugMarkerPublisher(rclcpp::Node * node); void pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, - int id = 0); + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); void pushVirtualWall(const geometry_msgs::msg::Pose & pose); void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); diff --git a/control/control_validator/include/control_validator/utils.hpp b/control/control_validator/include/control_validator/utils.hpp index 8033ac9442960..edf97aaf5f510 100644 --- a/control/control_validator/include/control_validator/utils.hpp +++ b/control/control_validator/include/control_validator/utils.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -26,8 +26,8 @@ namespace control_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; using motion_utils::convertToTrajectory; using motion_utils::convertToTrajectoryPointArray; diff --git a/control/control_validator/package.xml b/control/control_validator/package.xml index faf254708ddbb..2f4400d6ebc57 100644 --- a/control/control_validator/package.xml +++ b/control/control_validator/package.xml @@ -20,7 +20,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_updater geometry_msgs motion_utils diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp index 1b92aee0bb416..2dbdc558b305b 100644 --- a/control/control_validator/src/debug_marker.cpp +++ b/control/control_validator/src/debug_marker.cpp @@ -40,7 +40,7 @@ void ControlValidatorDebugMarkerPublisher::clearMarkers() } void ControlValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) { pushPoseMarker(p.pose, ns, id); } diff --git a/control/external_cmd_selector/README.md b/control/external_cmd_selector/README.md index 93ded5713a25b..ca0f4f0954dbf 100644 --- a/control/external_cmd_selector/README.md +++ b/control/external_cmd_selector/README.md @@ -23,12 +23,12 @@ The current mode is set via service, `remote` is remotely operated, `local` is t ### Output topics -| Name | Type | Description | -| ------------------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------- | -| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | -| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | -| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | -| `/external/selected/gear_cmd` | autoware_auto_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | -| `/external/selected/hazard_lights_cmd` | autoware_auto_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | -| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | -| `/external/selected/turn_indicators_cmd` | autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | +| Name | Type | Description | +| ------------------------------------------------------ | ------------------------------------------------- | ----------------------------------------------- | +| `/control/external_cmd_selector/current_selector_mode` | TBD | Current selected mode, remote or local. | +| `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Check if node is active or not. | +| `/external/selected/external_control_cmd` | TBD | Pass through control command with current mode. | +| `/external/selected/gear_cmd` | autoware_vehicle_msgs::msg::GearCommand | Pass through gear command with current mode. | +| `/external/selected/hazard_lights_cmd` | autoware_vehicle_msgs::msg::HazardLightsCommand | Pass through hazard light with current mode. | +| `/external/selected/heartbeat` | TBD | Pass through heartbeat with current mode. | +| `/external/selected/turn_indicators_cmd` | autoware_vehicle_msgs::msg::TurnIndicatorsCommand | Pass through turn indicator with current mode. | diff --git a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp b/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp index f2803e897ba3e..6a4fb897b57bc 100644 --- a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp +++ b/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp @@ -19,9 +19,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ class ExternalCmdSelector : public rclcpp::Node private: using CommandSourceSelect = tier4_control_msgs::srv::ExternalCommandSelect; using CommandSourceMode = tier4_control_msgs::msg::ExternalCommandSelectorMode; - using InternalGearShift = autoware_auto_vehicle_msgs::msg::GearCommand; - using InternalTurnSignal = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; - using InternalHazardSignal = autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using InternalGearShift = autoware_vehicle_msgs::msg::GearCommand; + using InternalTurnSignal = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + using InternalHazardSignal = autoware_vehicle_msgs::msg::HazardLightsCommand; using InternalHeartbeat = tier4_external_api_msgs::msg::Heartbeat; using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped; using ExternalGearShift = tier4_external_api_msgs::msg::GearShiftStamped; diff --git a/control/external_cmd_selector/package.xml b/control/external_cmd_selector/package.xml index 4d8cc5a385494..b3520839e3b51 100644 --- a/control/external_cmd_selector/package.xml +++ b/control/external_cmd_selector/package.xml @@ -16,7 +16,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs rclcpp diff --git a/control/joy_controller/README.md b/control/joy_controller/README.md index 4674ebdadb51d..73e3644df49ac 100644 --- a/control/joy_controller/README.md +++ b/control/joy_controller/README.md @@ -30,15 +30,15 @@ ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/ ### Output topics -| Name | Type | Description | -| ----------------------------------- | -------------------------------------------------------- | ---------------------------------------- | -| `~/output/control_command` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command | -| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | -| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | -| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | -| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | -| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | -| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage | +| Name | Type | Description | +| ----------------------------------- | --------------------------------------------------- | ---------------------------------------- | +| `~/output/control_command` | autoware_control_msgs::msg::Control | lateral and longitudinal control command | +| `~/output/external_control_command` | tier4_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | +| `~/output/shift` | tier4_external_api_msgs::msg::GearShiftStamped | gear command | +| `~/output/turn_signal` | tier4_external_api_msgs::msg::TurnSignalStamped | turn signal command | +| `~/output/gate_mode` | tier4_control_msgs::msg::GateMode | gate mode (Auto or External) | +| `~/output/heartbeat` | tier4_external_api_msgs::msg::Heartbeat | heartbeat | +| `~/output/vehicle_engage` | autoware_vehicle_msgs::msg::Engage | vehicle engage | ## Parameters diff --git a/control/joy_controller/include/joy_controller/joy_controller.hpp b/control/joy_controller/include/joy_controller/joy_controller.hpp index 22064f9cefaad..e88f7bcb3904e 100644 --- a/control/joy_controller/include/joy_controller/joy_controller.hpp +++ b/control/joy_controller/include/joy_controller/joy_controller.hpp @@ -19,8 +19,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -81,15 +81,14 @@ class AutowareJoyControllerNode : public rclcpp::Node void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); // Publisher - rclcpp::Publisher::SharedPtr - pub_control_command_; + rclcpp::Publisher::SharedPtr pub_control_command_; rclcpp::Publisher::SharedPtr pub_external_control_command_; rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_heartbeat_; rclcpp::Publisher::SharedPtr pub_gate_mode_; - rclcpp::Publisher::SharedPtr pub_vehicle_engage_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; void publishControlCommand(); void publishExternalControlCommand(); @@ -106,7 +105,7 @@ class AutowareJoyControllerNode : public rclcpp::Node rclcpp::Client::SharedPtr client_autoware_engage_; // Previous State - autoware_auto_control_msgs::msg::AckermannControlCommand prev_control_command_; + autoware_control_msgs::msg::Control prev_control_command_; tier4_external_api_msgs::msg::ControlCommand prev_external_control_command_; GearShiftType prev_shift_ = tier4_external_api_msgs::msg::GearShift::NONE; TurnSignalType prev_turn_signal_ = tier4_external_api_msgs::msg::TurnSignal::NONE; diff --git a/control/joy_controller/package.xml b/control/joy_controller/package.xml index 79ee9f27868f2..f7a5ed805b8ad 100644 --- a/control/joy_controller/package.xml +++ b/control/joy_controller/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs geometry_msgs joy nav_msgs diff --git a/control/joy_controller/src/joy_controller/joy_controller_node.cpp b/control/joy_controller/src/joy_controller/joy_controller_node.cpp index 5eec438032410..39198825f9c4e 100644 --- a/control/joy_controller/src/joy_controller/joy_controller_node.cpp +++ b/control/joy_controller/src/joy_controller/joy_controller_node.cpp @@ -254,7 +254,7 @@ void AutowareJoyControllerNode::onTimer() void AutowareJoyControllerNode::publishControlCommand() { - autoware_auto_control_msgs::msg::AckermannControlCommand cmd; + autoware_control_msgs::msg::Control cmd; cmd.stamp = this->now(); { cmd.lateral.steering_tire_angle = steer_ratio_ * joy_->steer(); @@ -262,24 +262,24 @@ void AutowareJoyControllerNode::publishControlCommand() if (joy_->accel()) { cmd.longitudinal.acceleration = accel_ratio_ * joy_->accel(); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = twist_->twist.linear.x + velocity_gain_ * cmd.longitudinal.acceleration; - cmd.longitudinal.speed = - std::min(cmd.longitudinal.speed, static_cast(max_forward_velocity_)); + cmd.longitudinal.velocity = + std::min(cmd.longitudinal.velocity, static_cast(max_forward_velocity_)); } if (joy_->brake()) { - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = -brake_ratio_ * joy_->brake(); } // Backward if (joy_->accel() && joy_->brake()) { cmd.longitudinal.acceleration = backward_accel_ratio_ * joy_->accel(); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = twist_->twist.linear.x - velocity_gain_ * cmd.longitudinal.acceleration; - cmd.longitudinal.speed = - std::max(cmd.longitudinal.speed, static_cast(-max_backward_velocity_)); + cmd.longitudinal.velocity = + std::max(cmd.longitudinal.velocity, static_cast(-max_backward_velocity_)); } } @@ -426,7 +426,7 @@ void AutowareJoyControllerNode::publishAutowareEngage() void AutowareJoyControllerNode::publishVehicleEngage() { - autoware_auto_vehicle_msgs::msg::Engage engage; + autoware_vehicle_msgs::msg::Engage engage; if (joy_->vehicle_engage()) { engage.engage = true; @@ -492,8 +492,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & // Publisher pub_control_command_ = - this->create_publisher( - "output/control_command", 1); + this->create_publisher("output/control_command", 1); pub_external_control_command_ = this->create_publisher( "output/external_control_command", 1); @@ -505,7 +504,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & pub_heartbeat_ = this->create_publisher("output/heartbeat", 1); pub_vehicle_engage_ = - this->create_publisher("output/vehicle_engage", 1); + this->create_publisher("output/vehicle_engage", 1); // Service Client client_emergency_stop_ = this->create_client( diff --git a/control/lane_departure_checker/README.md b/control/lane_departure_checker/README.md index 4a6592a103f2f..6eb62dcb2c23a 100644 --- a/control/lane_departure_checker/README.md +++ b/control/lane_departure_checker/README.md @@ -49,10 +49,10 @@ This package includes the following features: ### Input - /localization/kinematic_state [`nav_msgs::msg::Odometry`] -- /map/vector_map [`autoware_auto_mapping_msgs::msg::HADMapBin`] +- /map/vector_map [`autoware_map_msgs::msg::LaneletMapBin`] - /planning/mission_planning/route [`autoware_planning_msgs::msg::LaneletRoute`] -- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] -- /control/trajectory_follower/predicted_trajectory [`autoware_auto_planning_msgs::msg::Trajectory`] +- /planning/scenario_planning/trajectory [`autoware_planning_msgs::msg::Trajectory`] +- /control/trajectory_follower/predicted_trajectory [`autoware_planning_msgs::msg::Trajectory`] ### Output diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index b95c0a4b26e5c..967cb65c8efa8 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -20,14 +20,14 @@ #include #include -#include -#include -#include #include +#include +#include #include #include #include #include +#include #include #include @@ -47,13 +47,13 @@ namespace lane_departure_checker { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::PoseDeviation; using tier4_autoware_utils::Segment2d; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; typedef boost::geometry::index::rtree> SegmentRtree; diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index dd05ab226f6b7..0f0e15d0a4f62 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -22,9 +22,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -42,7 +42,7 @@ namespace lane_departure_checker { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; struct NodeParam { @@ -67,7 +67,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_reference_trajectory_; rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; @@ -87,7 +87,7 @@ class LaneDepartureCheckerNode : public rclcpp::Node // Callback void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onLaneletMapBin(const HADMapBin::ConstSharedPtr msg); + void onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg); void onRoute(const LaneletRoute::ConstSharedPtr msg); void onReferenceTrajectory(const Trajectory::ConstSharedPtr msg); void onPredictedTrajectory(const Trajectory::ConstSharedPtr msg); diff --git a/control/lane_departure_checker/package.xml b/control/lane_departure_checker/package.xml index 1059e86cadc6d..73955613d21a9 100644 --- a/control/lane_departure_checker/package.xml +++ b/control/lane_departure_checker/package.xml @@ -11,8 +11,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_planning_msgs boost diagnostic_updater diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index cd3fd67a3ab78..eb5d674705aaa 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -39,8 +39,8 @@ using tier4_autoware_utils::Point2d; namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Point; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index 4d6c86990c399..c3fd1d314e371 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -172,7 +172,7 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o // Subscriber sub_odom_ = this->create_subscription( "~/input/odometry", 1, std::bind(&LaneDepartureCheckerNode::onOdometry, this, _1)); - sub_lanelet_map_bin_ = this->create_subscription( + sub_lanelet_map_bin_ = this->create_subscription( "~/input/lanelet_map_bin", rclcpp::QoS{1}.transient_local(), std::bind(&LaneDepartureCheckerNode::onLaneletMapBin, this, _1)); sub_route_ = this->create_subscription( @@ -206,7 +206,7 @@ void LaneDepartureCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSh current_odom_ = msg; } -void LaneDepartureCheckerNode::onLaneletMapBin(const HADMapBin::ConstSharedPtr msg) +void LaneDepartureCheckerNode::onLaneletMapBin(const LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_, &traffic_rules_, &routing_graph_); diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 6bfba5818bd06..3ad116929b50c 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -63,15 +63,15 @@ The tracking is not accurate if the first point of the reference trajectory is a Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport`: current steering +- `autoware_vehicle_msgs/SteeringReport`: current steering ### Outputs Return LateralOutput which contains the following to the controller node -- `autoware_auto_control_msgs/AckermannLateralCommand` +- `autoware_control_msgs/Lateral` - LateralSyncData - steer angle convergence diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 2e83c5ab847c4..91b803dea36fa 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -22,9 +22,9 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -38,9 +38,9 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -378,7 +378,7 @@ class MPC */ Float32MultiArrayStamped generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, - const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const; /** @@ -441,9 +441,8 @@ class MPC * @return True if the MPC calculation is successful, false otherwise. */ bool calculateMPC( - const SteeringReport & current_steer, const Odometry & current_kinematics, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, - Float32MultiArrayStamped & diagnostic); + const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, + Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic); /** * @brief Set the reference trajectory to be followed. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 92b01247105c6..eb1d75f9508b3 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -22,10 +22,9 @@ #include "rclcpp/rclcpp.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -42,9 +41,9 @@ namespace autoware::motion::control::mpc_lateral_controller { namespace trajectory_follower = ::autoware::motion::control::trajectory_follower; -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_vehicle_msgs::msg::SteeringReport; using nav_msgs::msg::Odometry; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Float32Stamped; @@ -97,10 +96,10 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase bool m_is_mpc_history_filled{false}; // store the last mpc outputs for 1 sec - std::vector> m_mpc_steering_history{}; + std::vector> m_mpc_steering_history{}; // set the mpc steering output to history - void setSteeringToHistory(const AckermannLateralCommand & steering); + void setSteeringToHistory(const Lateral & steering); // check if the mpc steering output is converged bool isMpcConverged(); @@ -118,7 +117,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase bool m_is_ctrl_cmd_prev_initialized = false; // Previous control command for path following. - AckermannLateralCommand m_ctrl_cmd_prev; + Lateral m_ctrl_cmd_prev; // Flag indicating whether the first trajectory has been received. bool m_has_received_first_trajectory = false; @@ -200,7 +199,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param ctrl_cmd Control command to be created. * @return Created control command. */ - AckermannLateralCommand createCtrlCmdMsg(const AckermannLateralCommand & ctrl_cmd); + Lateral createCtrlCmdMsg(const Lateral & ctrl_cmd); /** * @brief Publish the predicted future trajectory. @@ -218,13 +217,13 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @brief Get the stop control command. * @return Stop control command. */ - AckermannLateralCommand getStopControlCommand() const; + Lateral getStopControlCommand() const; /** * @brief Get the control command applied before initialization. * @return Initial control command. */ - AckermannLateralCommand getInitialControlCommand() const; + Lateral getInitialControlCommand() const; /** * @brief Check if the ego car is in a stopped state. @@ -250,7 +249,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @param cmd Steering control command to be checked. * @return True if the steering control is converged and stable, false otherwise. */ - bool isSteerConverged(const AckermannLateralCommand & cmd) const; + bool isSteerConverged(const Lateral & cmd) const; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res; diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp index 7e1c7ebdf1348..eb624f39ae76b 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_trajectory.hpp @@ -109,11 +109,11 @@ class MPCTrajectory return points; } - std::vector toTrajectoryPoints() const + std::vector toTrajectoryPoints() const { - std::vector points; + std::vector points; for (size_t i = 0; i < x.size(); ++i) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; + autoware_planning_msgs::msg::TrajectoryPoint point; point.pose.position.x = x.at(i); point.pose.position.y = y.at(i); point.pose.position.z = z.at(i); diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 9062ff1ea34e3..819d7fb89b8a7 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -28,8 +28,8 @@ #include "mpc_lateral_controller/mpc_trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -43,8 +43,8 @@ namespace autoware::motion::control::mpc_lateral_controller namespace MPCUtils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; /** diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp index 7ca2fa1a61528..16e9b165fb1a5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/steering_predictor.hpp @@ -17,14 +17,14 @@ #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include #include namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; +using autoware_control_msgs::msg::Lateral; class SteeringPredictor { @@ -61,7 +61,7 @@ class SteeringPredictor double m_input_delay; // Buffer of sent control commands. - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; /** * @brief Get the sum of all steering commands over the given time range. diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml index da03c4481e782..000bddc65aa1f 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/mpc_lateral_controller/package.xml @@ -17,9 +17,9 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 373de2e0bd911..177c1e0854bfb 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -36,9 +36,8 @@ MPC::MPC(rclcpp::Node & node) } bool MPC::calculateMPC( - const SteeringReport & current_steer, const Odometry & current_kinematics, - AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_trajectory, - Float32MultiArrayStamped & diagnostic) + const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd, + Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic) { // since the reference trajectory does not take into account the current velocity of the ego // vehicle, it needs to calculate the trajectory velocity considering the longitudinal dynamics. @@ -117,7 +116,7 @@ bool MPC::calculateMPC( Float32MultiArrayStamped MPC::generateDiagData( const MPCTrajectory & reference_trajectory, const MPCData & mpc_data, - const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, + const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const { Float32MultiArrayStamped diagnostic; diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 882150ffc1644..85d33a5e9f1c0 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -239,7 +239,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_current_steering.steering_tire_angle -= steering_offset_->getOffset(); } - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory predicted_traj; Float32MultiArrayStamped debug_values; @@ -309,7 +309,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( return createLateralOutput(ctrl_cmd, is_mpc_solved); } -bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) const +bool MpcLateralController::isSteerConverged(const Lateral & cmd) const { // wait for a while to propagate the trajectory shape to the output command when the trajectory // shape is changed. @@ -381,17 +381,17 @@ void MpcLateralController::setTrajectory( } } -AckermannLateralCommand MpcLateralController::getStopControlCommand() const +Lateral MpcLateralController::getStopControlCommand() const { - AckermannLateralCommand cmd; + Lateral cmd; cmd.steering_tire_angle = static_cast(m_steer_cmd_prev); cmd.steering_tire_rotation_rate = 0.0; return cmd; } -AckermannLateralCommand MpcLateralController::getInitialControlCommand() const +Lateral MpcLateralController::getInitialControlCommand() const { - AckermannLateralCommand cmd; + Lateral cmd; cmd.steering_tire_angle = m_current_steering.steering_tire_angle; cmd.steering_tire_rotation_rate = 0.0; return cmd; @@ -429,8 +429,7 @@ bool MpcLateralController::isStoppedState() const } } -AckermannLateralCommand MpcLateralController::createCtrlCmdMsg( - const AckermannLateralCommand & ctrl_cmd) +Lateral MpcLateralController::createCtrlCmdMsg(const Lateral & ctrl_cmd) { auto out = ctrl_cmd; out.stamp = clock_->now(); @@ -456,7 +455,7 @@ void MpcLateralController::publishDebugValues(Float32MultiArrayStamped & debug_v m_pub_steer_offset->publish(offset); } -void MpcLateralController::setSteeringToHistory(const AckermannLateralCommand & steering) +void MpcLateralController::setSteeringToHistory(const Lateral & steering) { const auto time = clock_->now(); if (m_mpc_steering_history.empty()) { diff --git a/control/mpc_lateral_controller/src/steering_predictor.cpp b/control/mpc_lateral_controller/src/steering_predictor.cpp index f2570047d5bd2..48d8fa8c47a97 100644 --- a/control/mpc_lateral_controller/src/steering_predictor.cpp +++ b/control/mpc_lateral_controller/src/steering_predictor.cpp @@ -47,7 +47,7 @@ double SteeringPredictor::calcSteerPrediction() void SteeringPredictor::storeSteerCmd(const double steer) { const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_input_delay); - AckermannLateralCommand cmd; + Lateral cmd; cmd.stamp = time_delayed; cmd.steering_tire_angle = static_cast(steer); diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index ba145b5dd146c..7644fb28b0788 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -21,10 +21,10 @@ #include "mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/pose.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" @@ -41,10 +41,10 @@ namespace autoware::motion::control::mpc_lateral_controller { -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_vehicle_msgs::msg::SteeringReport; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using tier4_debug_msgs::msg::Float32MultiArrayStamped; @@ -206,7 +206,7 @@ TEST_F(MPCTest, InitializeAndCalculate) initializeMPC(*mpc); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -238,7 +238,7 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) mpc->setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -265,7 +265,7 @@ TEST_F(MPCTest, OsqpCalculate) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -293,7 +293,7 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -323,7 +323,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); mpc->setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -354,7 +354,7 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -379,7 +379,7 @@ TEST_F(MPCTest, DynamicCalculate) ASSERT_TRUE(mpc->hasQPSolver()); // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -403,7 +403,7 @@ TEST_F(MPCTest, MultiSolveWithBuffer) mpc->m_input_buffer = {0.0, 0.0, 0.0}; // Calculate MPC - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_zero, default_velocity); @@ -443,7 +443,7 @@ TEST_F(MPCTest, FailureCases) Pose pose_far; pose_far.position.x = pose_zero.position.x - admissible_position_error - 1.0; pose_far.position.y = pose_zero.position.y - admissible_position_error - 1.0; - AckermannLateralCommand ctrl_cmd; + Lateral ctrl_cmd; Trajectory pred_traj; Float32MultiArrayStamped diag; const auto odom = makeOdometry(pose_far, default_velocity); diff --git a/control/mpc_lateral_controller/test/test_mpc_utils.cpp b/control/mpc_lateral_controller/test/test_mpc_utils.cpp index 75a7208074b90..51cc7d55e4560 100644 --- a/control/mpc_lateral_controller/test/test_mpc_utils.cpp +++ b/control/mpc_lateral_controller/test/test_mpc_utils.cpp @@ -16,8 +16,8 @@ #include "mpc_lateral_controller/mpc_trajectory.hpp" #include "mpc_lateral_controller/mpc_utils.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -25,8 +25,8 @@ namespace { namespace MPCUtils = autoware::motion::control::mpc_lateral_controller::MPCUtils; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPoint makePoint(const double x, const double y, const float vx) { diff --git a/control/obstacle_collision_checker/README.md b/control/obstacle_collision_checker/README.md index 5478db1f5a135..25b57fe6f4653 100644 --- a/control/obstacle_collision_checker/README.md +++ b/control/obstacle_collision_checker/README.md @@ -57,13 +57,13 @@ If any collision is found on predicted path, this module sets `ERROR` level as d ### Input -| Name | Type | Description | -| ---------------------------------------------- | ---------------------------------------------- | ------------------------------------------------------------------ | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ----------------------------------------- | ------------------------------------------------------------------ | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp index 4ab26c31f9910..a65d818056bd7 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -52,15 +52,15 @@ struct Input geometry_msgs::msg::Twist::ConstSharedPtr current_twist; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud; geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory; }; struct Output { std::map processing_time_map; bool will_collide; - autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + autoware_planning_msgs::msg::Trajectory resampled_trajectory; std::vector vehicle_footprints; std::vector vehicle_passing_areas; }; @@ -78,14 +78,14 @@ class ObstacleCollisionChecker vehicle_info_util::VehicleInfo vehicle_info_; //! This function assumes the input trajectory is sampled dense enough - static autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval); + static autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval); - static autoware_auto_planning_msgs::msg::Trajectory cutTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length); + static autoware_planning_msgs::msg::Trajectory cutTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length); static std::vector createVehicleFootprints( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, const vehicle_info_util::VehicleInfo & vehicle_info); static std::vector createVehiclePassingAreas( diff --git a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp index 12b1e51a1cf3c..9d79a0facac95 100644 --- a/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp +++ b/control/obstacle_collision_checker/include/obstacle_collision_checker/obstacle_collision_checker_node.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -51,9 +51,9 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node std::shared_ptr self_pose_listener_; std::shared_ptr transform_listener_; rclcpp::Subscription::SharedPtr sub_obstacle_pointcloud_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; @@ -62,13 +62,13 @@ class ObstacleCollisionCheckerNode : public rclcpp::Node geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_; geometry_msgs::msg::TransformStamped::ConstSharedPtr obstacle_transform_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; // Callback void onObstaclePointcloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg); - void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); // Publisher diff --git a/control/obstacle_collision_checker/package.xml b/control/obstacle_collision_checker/package.xml index 46b0b18191e81..ef7560755a122 100644 --- a/control/obstacle_collision_checker/package.xml +++ b/control/obstacle_collision_checker/package.xml @@ -18,7 +18,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs boost diagnostic_updater eigen diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp index 8b89913ef35c8..6887be4cedd77 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker.cpp @@ -54,7 +54,7 @@ pcl::PointCloud getTransformedPointCloud( pcl::PointCloud filterPointCloudByTrajectory( const pcl::PointCloud & pointcloud, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double radius) + const autoware_planning_msgs::msg::Trajectory & trajectory, const double radius) { pcl::PointCloud filtered_pointcloud; for (const auto & point : pointcloud.points) { @@ -121,10 +121,10 @@ Output ObstacleCollisionChecker::update(const Input & input) return output; } -autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double interval) +autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double interval) { - autoware_auto_planning_msgs::msg::Trajectory resampled; + autoware_planning_msgs::msg::Trajectory resampled; resampled.header = trajectory.header; resampled.points.push_back(trajectory.points.front()); @@ -143,10 +143,10 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::resampleT return resampled; } -autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const double length) +autoware_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajectory( + const autoware_planning_msgs::msg::Trajectory & trajectory, const double length) { - autoware_auto_planning_msgs::msg::Trajectory cut; + autoware_planning_msgs::msg::Trajectory cut; cut.header = trajectory.header; double total_length = 0.0; @@ -169,7 +169,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec if (remain_distance <= points_distance) { const Eigen::Vector3d p_interpolated = p1 + remain_distance * (p2 - p1).normalized(); - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = p_interpolated.x(); p.pose.position.y = p_interpolated.y(); p.pose.position.z = p_interpolated.z(); @@ -187,7 +187,7 @@ autoware_auto_planning_msgs::msg::Trajectory ObstacleCollisionChecker::cutTrajec } std::vector ObstacleCollisionChecker::createVehicleFootprints( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, const Param & param, + const autoware_planning_msgs::msg::Trajectory & trajectory, const Param & param, const vehicle_info_util::VehicleInfo & vehicle_info) { // Create vehicle footprint in base_link coordinate diff --git a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp index bf3da0fe32627..cfde4ee849728 100644 --- a/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp +++ b/control/obstacle_collision_checker/src/obstacle_collision_checker_node/obstacle_collision_checker_node.cpp @@ -76,10 +76,10 @@ ObstacleCollisionCheckerNode::ObstacleCollisionCheckerNode(const rclcpp::NodeOpt sub_obstacle_pointcloud_ = create_subscription( "input/obstacle_pointcloud", rclcpp::SensorDataQoS(), std::bind(&ObstacleCollisionCheckerNode::onObstaclePointcloud, this, _1)); - sub_reference_trajectory_ = create_subscription( + sub_reference_trajectory_ = create_subscription( "input/reference_trajectory", 1, std::bind(&ObstacleCollisionCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = create_subscription( + sub_predicted_trajectory_ = create_subscription( "input/predicted_trajectory", 1, std::bind(&ObstacleCollisionCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( @@ -109,13 +109,13 @@ void ObstacleCollisionCheckerNode::onObstaclePointcloud( } void ObstacleCollisionCheckerNode::onReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } void ObstacleCollisionCheckerNode::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } diff --git a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp index e851cca85c11f..705bff754d3d9 100644 --- a/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp +++ b/control/obstacle_collision_checker/test/test_obstacle_collision_checker.cpp @@ -18,9 +18,9 @@ TEST(test_obstacle_collision_checker, filterPointCloudByTrajectory) { pcl::PointCloud pcl; - autoware_auto_planning_msgs::msg::Trajectory trajectory; + autoware_planning_msgs::msg::Trajectory trajectory; pcl::PointXYZ pcl_point; - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + autoware_planning_msgs::msg::TrajectoryPoint traj_point; pcl_point.y = 0.0; traj_point.pose.position.y = 0.99; for (double x = 0.0; x < 10.0; x += 1.0) { diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index ea0688e88d9f1..008d092565ba4 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -58,15 +58,15 @@ For the mode transition: For the transition availability/completion check: -- /control/command/control_cmd [`autoware_auto_control_msgs/msg/AckermannControlCommand`]: vehicle control signal +- /control/command/control_cmd [`autoware_control_msgs/msg/Control`]: vehicle control signal - /localization/kinematic_state [`nav_msgs/msg/Odometry`]: ego vehicle state -- /planning/scenario_planning/trajectory [`autoware_auto_planning_msgs/msg/Trajectory`]: planning trajectory -- /vehicle/status/control_mode [`autoware_auto_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) +- /planning/scenario_planning/trajectory [`autoware_planning_msgs/msg/Trajectory`]: planning trajectory +- /vehicle/status/control_mode [`autoware_vehicle_msgs/msg/ControlModeReport`]: vehicle control mode (autonomous/manual) - /control/vehicle_cmd_gate/operation_mode [`autoware_adapi_v1_msgs/msg/OperationModeState`]: the operation mode in the `vehicle_cmd_gate`. (To be removed) For the backward compatibility (to be removed): -- /api/autoware/get/engage [`autoware_auto_vehicle_msgs/msg/Engage`] +- /api/autoware/get/engage [`autoware_vehicle_msgs/msg/Engage`] - /control/current_gate_mode [`tier4_control_msgs/msg/GateMode`] - /control/external_cmd_selector/current_selector_mode [`tier4_control_msgs/msg/ExternalCommandSelectorMode`] @@ -76,9 +76,9 @@ For the backward compatibility (to be removed): - /control/operation_mode_transition_manager/debug_info [`operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug`]: detailed information about the operation mode transition - /control/gate_mode_cmd [`tier4_control_msgs/msg/GateMode`]: to change the `vehicle_cmd_gate` state to use its features (to be removed) -- /autoware/engage [`autoware_auto_vehicle_msgs/msg/Engage`]: +- /autoware/engage [`autoware_vehicle_msgs/msg/Engage`]: -- /control/control_mode_request [`autoware_auto_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) +- /control/control_mode_request [`autoware_vehicle_msgs/srv/ControlModeCommand`]: to change the vehicle control mode (autonomous/manual) - /control/external_cmd_selector/select_external_command [`tier4_control_msgs/srv/ExternalCommandSelect`]: ## Parameters diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index dbe9a21c1186a..99439aa7e3ab7 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -11,9 +11,8 @@ autoware_cmake rosidl_default_generators - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils geometry_msgs diff --git a/control/operation_mode_transition_manager/src/compatibility.hpp b/control/operation_mode_transition_manager/src/compatibility.hpp index a69b15e69960c..d9bde7cb22872 100644 --- a/control/operation_mode_transition_manager/src/compatibility.hpp +++ b/control/operation_mode_transition_manager/src/compatibility.hpp @@ -17,7 +17,7 @@ #include "data.hpp" -#include +#include #include #include #include @@ -33,7 +33,7 @@ class Compatibility std::optional get_mode() const; private: - using AutowareEngage = autoware_auto_vehicle_msgs::msg::Engage; + using AutowareEngage = autoware_vehicle_msgs::msg::Engage; using GateMode = tier4_control_msgs::msg::GateMode; using SelectorModeMsg = tier4_control_msgs::msg::ExternalCommandSelectorMode; using SelectorModeSrv = tier4_control_msgs::srv::ExternalCommandSelect; diff --git a/control/operation_mode_transition_manager/src/data.hpp b/control/operation_mode_transition_manager/src/data.hpp index 9b822936a0252..8ea9f8d2b99ee 100644 --- a/control/operation_mode_transition_manager/src/data.hpp +++ b/control/operation_mode_transition_manager/src/data.hpp @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -34,8 +34,8 @@ using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Respon using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; using OperationModeValue = OperationModeState::_mode_type; using ChangeOperationMode = tier4_system_msgs::srv::ChangeOperationMode; -using ControlModeCommand = autoware_auto_vehicle_msgs::srv::ControlModeCommand; -using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport; +using ControlModeCommand = autoware_vehicle_msgs::srv::ControlModeCommand; +using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; enum class OperationMode { STOP, AUTONOMOUS, LOCAL, REMOTE }; diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index d57cb8c78c740..635ead2430677 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -35,13 +35,11 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) { vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo(); - sub_control_cmd_ = node->create_subscription( - "control_cmd", 1, - [this](const AckermannControlCommand::SharedPtr msg) { control_cmd_ = *msg; }); - sub_trajectory_follower_control_cmd_ = node->create_subscription( - "trajectory_follower_control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { - trajectory_follower_control_cmd_ = *msg; - }); + sub_control_cmd_ = node->create_subscription( + "control_cmd", 1, [this](const Control::SharedPtr msg) { control_cmd_ = *msg; }); + sub_trajectory_follower_control_cmd_ = node->create_subscription( + "trajectory_follower_control_cmd", 1, + [this](const Control::SharedPtr msg) { trajectory_follower_control_cmd_ = *msg; }); sub_kinematics_ = node->create_subscription( "kinematics", 1, [this](const Odometry::SharedPtr msg) { kinematics_ = *msg; }); @@ -220,7 +218,7 @@ bool AutonomousMode::isModeChangeAvailable() } const auto current_speed = kinematics_.twist.twist.linear.x; - const auto target_control_speed = control_cmd_.longitudinal.speed; + const auto target_control_speed = control_cmd_.longitudinal.velocity; const auto & param = engage_acceptable_param_; if (!enable_engage_on_driving_ && std::fabs(current_speed) > 1.0e-2) { @@ -267,7 +265,7 @@ bool AutonomousMode::isModeChangeAvailable() // No engagement if the vehicle is moving but the target speed is zero. const bool is_stop_cmd_indicated = std::abs(target_control_speed) < 0.01 || - std::abs(trajectory_follower_control_cmd_.longitudinal.speed) < 0.01; + std::abs(trajectory_follower_control_cmd_.longitudinal.velocity) < 0.01; const bool stop_ok = !(std::abs(current_speed) > 0.1 && is_stop_cmd_indicated); // No engagement if the large acceleration is commanded. diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index e10d64e367f8d..e5abd4285ad5f 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -63,11 +63,11 @@ class AutonomousMode : public ModeChangeBase bool hasDangerAcceleration(); std::pair hasDangerLateralAcceleration(); - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Control = autoware_control_msgs::msg::Control; using Odometry = nav_msgs::msg::Odometry; - using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; - rclcpp::Subscription::SharedPtr sub_control_cmd_; - rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; + using Trajectory = autoware_planning_msgs::msg::Trajectory; + rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Logger logger_; @@ -79,8 +79,8 @@ class AutonomousMode : public ModeChangeBase double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; - AckermannControlCommand control_cmd_; - AckermannControlCommand trajectory_follower_control_cmd_; + Control control_cmd_; + Control trajectory_follower_control_cmd_; Odometry kinematics_; Trajectory trajectory_; vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 57eab2d87f18e..4e20cb27fe66a 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -143,14 +143,14 @@ There are two sources of the slope information, which can be switched by a param Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry ### Output Return LongitudinalOutput which contains the following to the controller node -- `autoware_auto_control_msgs/LongitudinalCommand`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. +- `autoware_control_msgs/Longitudinal`: command to control the longitudinal motion of the vehicle. It contains the target velocity and target acceleration. - LongitudinalSyncData - velocity convergence(currently not used) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index ac4fec8dacb7d..4ea844a140f4f 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -25,7 +25,7 @@ #include #include // NOLINT -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -37,8 +37,8 @@ namespace autoware::motion::control::pid_longitudinal_controller namespace longitudinal_utils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Quaternion; @@ -150,7 +150,7 @@ double applyDiffLimitFilter( */ geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); + const autoware_planning_msgs::msg::Trajectory & trajectory); } // namespace longitudinal_utils } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 967b7c40fbd10..dee2e580e6911 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -33,9 +33,8 @@ #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -88,7 +87,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro struct ControlData { bool is_far_from_trajectory{false}; - autoware_auto_planning_msgs::msg::Trajectory interpolated_traj{}; + autoware_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx size_t target_idx{0}; StateAfterDelay state_after_delay{0.0, 0.0, 0.0}; @@ -113,7 +112,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // pointers for ros topic nav_msgs::msg::Odometry m_current_kinematic_state; geometry_msgs::msg::AccelWithCovarianceStamped m_current_accel; - autoware_auto_planning_msgs::msg::Trajectory m_trajectory; + autoware_planning_msgs::msg::Trajectory m_trajectory; OperationModeState m_current_operation_mode; // vehicle info @@ -218,7 +217,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double m_ego_nearest_yaw_threshold; // buffer of send command - std::vector m_ctrl_cmd_vec; + std::vector m_ctrl_cmd_vec; // for calculating dt std::shared_ptr m_prev_control_time{nullptr}; @@ -270,7 +269,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @brief set reference trajectory with received message * @param [in] msg trajectory message */ - void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg); + void setTrajectory(const autoware_planning_msgs::msg::Trajectory & msg); bool isReady(const trajectory_follower::InputData & input_data) override; @@ -309,7 +308,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] ctrl_cmd calculated control command to control velocity * @param [in] current_vel current velocity of the vehicle */ - autoware_auto_control_msgs::msg::LongitudinalCommand createCtrlCmdMsg( + autoware_control_msgs::msg::Longitudinal createCtrlCmdMsg( const Motion & ctrl_cmd, const double & current_vel); /** @@ -371,9 +370,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] point vehicle position * @param [in] nearest_idx index of the trajectory point nearest to the vehicle position */ - std::pair + std::pair calcInterpolatedTrajPointAndSegment( - const autoware_auto_planning_msgs::msg::Trajectory & traj, + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const; /** diff --git a/control/pid_longitudinal_controller/package.xml b/control/pid_longitudinal_controller/package.xml index 959aca689816a..82b297a69fee6 100644 --- a/control/pid_longitudinal_controller/package.xml +++ b/control/pid_longitudinal_controller/package.xml @@ -19,9 +19,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 28cd6e1824859..4e9ef52a4969e 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -161,7 +161,7 @@ double applyDiffLimitFilter( geometry_msgs::msg::Pose findTrajectoryPoseAfterDistance( const size_t src_idx, const double distance, - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) + const autoware_planning_msgs::msg::Trajectory & trajectory) { double remain_dist = distance; geometry_msgs::msg::Pose p = trajectory.points.back().pose; diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index cc1c0c6383707..9a8223c610f9a 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -232,8 +232,7 @@ void PidLongitudinalController::setCurrentOperationMode(const OperationModeState m_current_operation_mode = msg; } -void PidLongitudinalController::setTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & msg) +void PidLongitudinalController::setTrajectory(const autoware_planning_msgs::msg::Trajectory & msg) { if (!longitudinal_utils::isValidTrajectory(msg)) { RCLCPP_ERROR_THROTTLE(logger_, *clock_, 3000, "received invalid trajectory. ignore."); @@ -820,13 +819,13 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( } // Do not use nearest_idx here -autoware_auto_control_msgs::msg::LongitudinalCommand PidLongitudinalController::createCtrlCmdMsg( +autoware_control_msgs::msg::Longitudinal PidLongitudinalController::createCtrlCmdMsg( const Motion & ctrl_cmd, const double & current_vel) { // publish control command - autoware_auto_control_msgs::msg::LongitudinalCommand cmd{}; + autoware_control_msgs::msg::Longitudinal cmd{}; cmd.stamp = clock_->now(); - cmd.speed = static_cast(ctrl_cmd.vel); + cmd.velocity = static_cast(ctrl_cmd.vel); cmd.acceleration = static_cast(ctrl_cmd.acc); // store current velocity history @@ -926,7 +925,7 @@ void PidLongitudinalController::storeAccelCmd(const double accel) { if (m_control_state == ControlState::DRIVE) { // convert format - autoware_auto_control_msgs::msg::LongitudinalCommand cmd; + autoware_control_msgs::msg::Longitudinal cmd; cmd.stamp = clock_->now(); cmd.acceleration = static_cast(accel); @@ -994,10 +993,9 @@ PidLongitudinalController::Motion PidLongitudinalController::keepBrakeBeforeStop return output_motion; } -std::pair +std::pair PidLongitudinalController::calcInterpolatedTrajPointAndSegment( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & pose) const + const autoware_planning_msgs::msg::Trajectory & traj, const geometry_msgs::msg::Pose & pose) const { if (traj.points.size() == 1) { return std::make_pair(traj.points.at(0), 0); diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index 02bcce8c91c4b..bd79d7a8c3ac3 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -18,8 +18,8 @@ #include "pid_longitudinal_controller/longitudinal_controller_utils.hpp" #include "tf2/LinearMath/Quaternion.h" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -37,8 +37,8 @@ namespace longitudinal_utils = TEST(TestLongitudinalControllerUtils, isValidTrajectory) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; Trajectory traj; TrajectoryPoint point; EXPECT_FALSE(longitudinal_utils::isValidTrajectory(traj)); @@ -51,8 +51,8 @@ TEST(TestLongitudinalControllerUtils, isValidTrajectory) TEST(TestLongitudinalControllerUtils, calcStopDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; Pose current_pose; current_pose.position.x = 0.0; @@ -115,8 +115,8 @@ TEST(TestLongitudinalControllerUtils, getPitchByPose) TEST(TestLongitudinalControllerUtils, getPitchByTraj) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; const double wheel_base = 0.9; /** * Trajectory: @@ -346,10 +346,10 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation) TEST(TestLongitudinalControllerUtils, lerpTrajectoryPoint) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; const double abs_err = 1e-15; - decltype(autoware_auto_planning_msgs::msg::Trajectory::points) points; + decltype(autoware_planning_msgs::msg::Trajectory::points) points; TrajectoryPoint p; p.pose.position.x = 0.0; p.pose.position.y = 0.0; @@ -505,8 +505,8 @@ TEST(TestLongitudinalControllerUtils, applyDiffLimitFilter) TEST(TestLongitudinalControllerUtils, findTrajectoryPoseAfterDistance) { - using autoware_auto_planning_msgs::msg::Trajectory; - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::Trajectory; + using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; const double abs_err = 1e-5; Trajectory traj; diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md index 24e4b91ef441b..a968a285928f6 100644 --- a/control/predicted_path_checker/README.md +++ b/control/predicted_path_checker/README.md @@ -55,14 +55,14 @@ make the vehicle stop. ## Inputs -| Name | Type | Description | -| ------------------------------------- | ----------------------------------------------------- | --------------------------------------------------- | -| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory | -| `~/input/predicted_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Predicted trajectory | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | -| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | -| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | -| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | +| Name | Type | Description | +| ------------------------------------- | ------------------------------------------------ | --------------------------------------------------- | +| `~/input/reference_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory | +| `~/input/predicted_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Predicted trajectory | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment | +| `~/input/odometry` | `nav_msgs::msg::Odometry` | Odometry message of vehicle to get current velocity | +| `~/input/current_accel` | `geometry_msgs::msg::AccelWithCovarianceStamped` | Current acceleration | +| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused` | Current pause state of the vehicle | ## Outputs diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index 1d7791955b576..8410e7c78f723 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -42,11 +42,11 @@ namespace autoware::motion::control::predicted_path_checker { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_autoware_utils::Point2d; diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index 3ce5728521141..23696887c7051 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -27,8 +27,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -47,10 +47,10 @@ namespace autoware::motion::control::predicted_path_checker { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_autoware_utils::Point2d; @@ -90,9 +90,9 @@ class PredictedPathCheckerNode : public rclcpp::Node // Subscriber std::shared_ptr self_pose_listener_; rclcpp::Subscription::SharedPtr sub_dynamic_objects_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_reference_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_accel_; @@ -106,8 +106,8 @@ class PredictedPathCheckerNode : public rclcpp::Node geometry_msgs::msg::Twist::ConstSharedPtr current_twist_; geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_; PredictedObjects::ConstSharedPtr object_ptr_{nullptr}; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr reference_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr predicted_trajectory_; control_interface::IsStopped::Message::ConstSharedPtr is_stopped_ptr_{nullptr}; // Core @@ -122,8 +122,8 @@ class PredictedPathCheckerNode : public rclcpp::Node // Callback void onDynamicObjects(PredictedObjects::ConstSharedPtr msg); - void onReferenceTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); - void onPredictedTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg); + void onReferenceTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); + void onPredictedTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); void onIsStopped(const control_interface::IsStopped::Message::ConstSharedPtr msg); diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 75e90e624a17e..957800ad04305 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -39,9 +39,9 @@ namespace utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; @@ -85,14 +85,14 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( const double & base_to_width); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertObjToPolygon(const PredictedObject & obj); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, diff --git a/control/predicted_path_checker/package.xml b/control/predicted_path_checker/package.xml index 35f971907696a..bca65302dba55 100644 --- a/control/predicted_path_checker/package.xml +++ b/control/predicted_path_checker/package.xml @@ -12,8 +12,8 @@ ament_cmake autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs boost component_interface_specs component_interface_utils diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index bba069442bee7..28ea21f639a0e 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp @@ -72,10 +72,10 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n sub_dynamic_objects_ = create_subscription( "~/input/objects", rclcpp::SensorDataQoS(), std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); - sub_reference_trajectory_ = create_subscription( + sub_reference_trajectory_ = create_subscription( "~/input/reference_trajectory", 1, std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); - sub_predicted_trajectory_ = create_subscription( + sub_predicted_trajectory_ = create_subscription( "~/input/predicted_trajectory", 1, std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( @@ -109,13 +109,13 @@ void PredictedPathCheckerNode::onDynamicObjects(const PredictedObjects::ConstSha } void PredictedPathCheckerNode::onReferenceTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { reference_trajectory_ = msg; } void PredictedPathCheckerNode::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { predicted_trajectory_ = msg; } diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp index 9cb095e908d41..f1c76ce6eef8f 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp +++ b/control/predicted_path_checker/src/predicted_path_checker_node/utils.cpp @@ -298,7 +298,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( } Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; @@ -320,7 +320,7 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; tf2::Transform tf_map2obj; @@ -350,15 +350,15 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & Polygon2d convertObjToPolygon(const PredictedObject & obj) { Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = utils::convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = utils::convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = utils::convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); } else { @@ -377,13 +377,13 @@ bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & ob return base_pose_vec.dot(obstacle_vec) >= 0; } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); diff --git a/control/pure_pursuit/README.md b/control/pure_pursuit/README.md index 48e2a13ef664d..55c847fe88d22 100644 --- a/control/pure_pursuit/README.md +++ b/control/pure_pursuit/README.md @@ -6,14 +6,14 @@ The Pure Pursuit Controller module calculates the steering angle for tracking a Set the following from the [controller_node](../trajectory_follower_node/README.md) -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current ego pose and velocity information ## Outputs Return LateralOutput which contains the following to the controller node -- `autoware_auto_control_msgs/AckermannLateralCommand`: target steering angle +- `autoware_control_msgs/Lateral`: target steering angle - LateralSyncData - steer angle convergence -- `autoware_auto_planning_msgs/Trajectory`: predicted path for ego vehicle +- `autoware_planning_msgs/Trajectory`: predicted path for ego vehicle diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index ca0d77140b195..5b7b466dcb4dd 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp @@ -41,8 +41,8 @@ #include #include -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -56,9 +56,9 @@ using autoware::motion::control::trajectory_follower::InputData; using autoware::motion::control::trajectory_follower::LateralControllerBase; using autoware::motion::control::trajectory_follower::LateralOutput; -using autoware_auto_control_msgs::msg::AckermannLateralCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_control_msgs::msg::Lateral; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; namespace pure_pursuit { @@ -107,20 +107,19 @@ class PurePursuitLateralController : public LateralControllerBase rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_; std::vector output_tp_array_; - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; - autoware_auto_planning_msgs::msg::Trajectory trajectory_; + autoware_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_; + autoware_planning_msgs::msg::Trajectory trajectory_; nav_msgs::msg::Odometry current_odometry_; - autoware_auto_vehicle_msgs::msg::SteeringReport current_steering_; - boost::optional prev_cmd_; + autoware_vehicle_msgs::msg::SteeringReport current_steering_; + boost::optional prev_cmd_; // Debug Publisher rclcpp::Publisher::SharedPtr pub_debug_marker_; rclcpp::Publisher::SharedPtr pub_debug_values_; // Predicted Trajectory publish - rclcpp::Publisher::SharedPtr - pub_predicted_trajectory_; + rclcpp::Publisher::SharedPtr pub_predicted_trajectory_; - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); @@ -139,7 +138,7 @@ class PurePursuitLateralController : public LateralControllerBase bool isReady([[maybe_unused]] const InputData & input_data) override; LateralOutput run(const InputData & input_data) override; - AckermannLateralCommand generateCtrlCmdMsg(const double target_curvature); + Lateral generateCtrlCmdMsg(const double target_curvature); // Parameter Param param_{}; @@ -155,14 +154,13 @@ class PurePursuitLateralController : public LateralControllerBase * of vehicle. */ - TrajectoryPoint calcNextPose( - const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const; + TrajectoryPoint calcNextPose(const double ds, TrajectoryPoint & point, Lateral cmd) const; boost::optional generatePredictedTrajectory(); - AckermannLateralCommand generateOutputControlCmd(); + Lateral generateOutputControlCmd(); - bool calcIsSteerConverged(const AckermannLateralCommand & cmd); + bool calcIsSteerConverged(const Lateral & cmd); double calcLookaheadDistance( const double lateral_error, const double curvature, const double velocity, const double min_ld, @@ -170,7 +168,7 @@ class PurePursuitLateralController : public LateralControllerBase double calcCurvature(const size_t closest_idx); - void averageFilterTrajectory(autoware_auto_planning_msgs::msg::Trajectory & u); + void averageFilterTrajectory(autoware_planning_msgs::msg::Trajectory & u); // Debug mutable DebugData debug_data_; diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp index 6d9fca4b852cf..a5ae31133be8e 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp +++ b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_node.hpp @@ -36,8 +36,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -79,15 +79,15 @@ class PurePursuitNode : public rclcpp::Node private: // Subscriber tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; - rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Subscription::SharedPtr sub_current_odometry_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_; nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_; bool isDataReady(); - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); // TF @@ -96,8 +96,7 @@ class PurePursuitNode : public rclcpp::Node geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; // Publisher - rclcpp::Publisher::SharedPtr - pub_ctrl_cmd_; + rclcpp::Publisher::SharedPtr pub_ctrl_cmd_; void publishCommand(const double target_curvature); @@ -117,7 +116,7 @@ class PurePursuitNode : public rclcpp::Node std::unique_ptr pure_pursuit_; boost::optional calcTargetCurvature(); - boost::optional calcTargetPoint() const; + boost::optional calcTargetPoint() const; // Debug mutable DebugData debug_data_; diff --git a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp index a5b1e17ed983f..b2d6e13c585f4 100644 --- a/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp +++ b/control/pure_pursuit/include/pure_pursuit/util/planning_utils.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -51,7 +51,7 @@ namespace planning_utils { constexpr double ERROR = 1e-6; double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx); double calcCurvature( const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & curr_pose); @@ -66,7 +66,7 @@ double calcRadius( double convertCurvatureToSteeringAngle(double wheel_base, double kappa); std::vector extractPoses( - const autoware_auto_planning_msgs::msg::Trajectory & motions); + const autoware_planning_msgs::msg::Trajectory & motions); std::pair findClosestIdxWithDistAngThr( const std::vector & poses, diff --git a/control/pure_pursuit/package.xml b/control/pure_pursuit/package.xml index 30356b856fbac..b4820aee5cec4 100644 --- a/control/pure_pursuit/package.xml +++ b/control/pure_pursuit/package.xml @@ -13,8 +13,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_planning_msgs + autoware_control_msgs + autoware_planning_msgs boost geometry_msgs libboost-dev diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index f0c49b07e675a..a8232cce5d08d 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -95,7 +95,7 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) "~/debug/ld_outputs", rclcpp::QoS{1}); // Publish predicted trajectory - pub_predicted_trajectory_ = node.create_publisher( + pub_predicted_trajectory_ = node.create_publisher( "~/output/predicted_trajectory", 1); } @@ -138,7 +138,7 @@ double PurePursuitLateralController::calcLookaheadDistance( } TrajectoryPoint PurePursuitLateralController::calcNextPose( - const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const + const double ds, TrajectoryPoint & point, Lateral cmd) const { geometry_msgs::msg::Transform transform; transform.translation = tier4_autoware_utils::createTranslation(ds, 0.0, 0.0); @@ -164,7 +164,7 @@ void PurePursuitLateralController::setResampledTrajectory() out_arclength.push_back(s); } trajectory_resampled_ = - std::make_shared(motion_utils::resampleTrajectory( + std::make_shared(motion_utils::resampleTrajectory( motion_utils::convertToTrajectory(input_tp_array), out_arclength)); trajectory_resampled_->points.back() = trajectory_.points.back(); trajectory_resampled_->header = trajectory_.header; @@ -215,14 +215,14 @@ double PurePursuitLateralController::calcCurvature(const size_t closest_idx) } void PurePursuitLateralController::averageFilterTrajectory( - autoware_auto_planning_msgs::msg::Trajectory & u) + autoware_planning_msgs::msg::Trajectory & u) { if (static_cast(u.points.size()) <= 2 * param_.path_filter_moving_ave_num) { RCLCPP_ERROR(logger_, "Cannot smooth path! Trajectory size is too low!"); return; } - autoware_auto_planning_msgs::msg::Trajectory filtered_trajectory(u); + autoware_planning_msgs::msg::Trajectory filtered_trajectory(u); for (int64_t i = 0; i < static_cast(u.points.size()); ++i) { TrajectoryPoint tmp{}; @@ -295,7 +295,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje predicted_trajectory.points.push_back(p); const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose); - AckermannLateralCommand tmp_msg; + Lateral tmp_msg; if (pp_output) { tmp_msg = generateCtrlCmdMsg(pp_output->curvature); @@ -310,7 +310,7 @@ boost::optional PurePursuitLateralController::generatePredictedTraje } else { const auto pp_output = calcTargetCurvature(false, predicted_trajectory.points.at(i).pose); - AckermannLateralCommand tmp_msg; + Lateral tmp_msg; if (pp_output) { tmp_msg = generateCtrlCmdMsg(pp_output->curvature); @@ -365,21 +365,21 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data) return output; } -bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd) +bool PurePursuitLateralController::calcIsSteerConverged(const Lateral & cmd) { return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) < static_cast(param_.converged_steer_rad_); } -AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() +Lateral PurePursuitLateralController::generateOutputControlCmd() { // Generate the control command const auto pp_output = calcTargetCurvature(true, current_odometry_.pose.pose); - AckermannLateralCommand output_cmd; + Lateral output_cmd; if (pp_output) { output_cmd = generateCtrlCmdMsg(pp_output->curvature); - prev_cmd_ = boost::optional(output_cmd); + prev_cmd_ = boost::optional(output_cmd); publishDebugMarker(); } else { RCLCPP_WARN_THROTTLE( @@ -393,12 +393,11 @@ AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd() return output_cmd; } -AckermannLateralCommand PurePursuitLateralController::generateCtrlCmdMsg( - const double target_curvature) +Lateral PurePursuitLateralController::generateCtrlCmdMsg(const double target_curvature) { const double tmp_steering = planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); - AckermannLateralCommand cmd; + Lateral cmd; cmd.stamp = clock_->now(); cmd.steering_tire_angle = static_cast( std::min(std::max(tmp_steering, -param_.max_steering_angle), param_.max_steering_angle)); diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp index 254b83bccbc34..a4b491930df1e 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_node.cpp @@ -73,14 +73,14 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options) // Subscribers using std::placeholders::_1; - sub_trajectory_ = this->create_subscription( + sub_trajectory_ = this->create_subscription( "input/reference_trajectory", 1, std::bind(&PurePursuitNode::onTrajectory, this, _1)); sub_current_odometry_ = this->create_subscription( "input/current_odometry", 1, std::bind(&PurePursuitNode::onCurrentOdometry, this, _1)); // Publishers - pub_ctrl_cmd_ = this->create_publisher( - "output/control_raw", 1); + pub_ctrl_cmd_ = + this->create_publisher("output/control_raw", 1); // Debug Publishers pub_debug_marker_ = @@ -124,7 +124,7 @@ void PurePursuitNode::onCurrentOdometry(const nav_msgs::msg::Odometry::ConstShar } void PurePursuitNode::onTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { trajectory_ = msg; } @@ -150,7 +150,7 @@ void PurePursuitNode::onTimer() void PurePursuitNode::publishCommand(const double target_curvature) { - autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; + autoware_control_msgs::msg::Lateral cmd; cmd.stamp = get_clock()->now(); cmd.steering_tire_angle = planning_utils::convertCurvatureToSteeringAngle(param_.wheel_base, target_curvature); @@ -211,8 +211,8 @@ boost::optional PurePursuitNode::calcTargetCurvature() return kappa; } -boost::optional -PurePursuitNode::calcTargetPoint() const +boost::optional PurePursuitNode::calcTargetPoint() + const { const auto closest_idx_result = planning_utils::findClosestIdxWithDistAngThr( planning_utils::extractPoses(*trajectory_), current_pose_->pose, 3.0, M_PI_4); diff --git a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp index 2597d7087ad61..b3a846157638a 100644 --- a/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp +++ b/control/pure_pursuit/src/pure_pursuit_core/planning_utils.cpp @@ -23,7 +23,7 @@ namespace pure_pursuit namespace planning_utils { double calcArcLengthFromWayPoint( - const autoware_auto_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, + const autoware_planning_msgs::msg::Trajectory & input_path, const size_t src_idx, const size_t dst_idx) { double length = 0; @@ -101,7 +101,7 @@ double convertCurvatureToSteeringAngle(double wheel_base, double kappa) } std::vector extractPoses( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) + const autoware_planning_msgs::msg::Trajectory & trajectory) { std::vector poses; diff --git a/control/shift_decider/README.md b/control/shift_decider/README.md index 820432e4c9e1a..c9fdc20696f0e 100644 --- a/control/shift_decider/README.md +++ b/control/shift_decider/README.md @@ -37,15 +37,15 @@ stop ### Input -| Name | Type | Description | -| --------------------- | ---------------------------------------------------------- | ---------------------------- | -| `~/input/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command for vehicle. | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ---------------------------- | +| `~/input/control_cmd` | `autoware_control_msgs::msg::Control` | Control command for vehicle. | ### Output -| Name | Type | Description | -| ------------------ | ---------------------------------------------- | ---------------------------------- | -| `~output/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | +| Name | Type | Description | +| ------------------ | ----------------------------------------- | ---------------------------------- | +| `~output/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Gear for drive forward / backward. | ## Parameters diff --git a/control/shift_decider/include/shift_decider/shift_decider.hpp b/control/shift_decider/include/shift_decider/shift_decider.hpp index a6b9938e404f6..b11a0f40625af 100644 --- a/control/shift_decider/include/shift_decider/shift_decider.hpp +++ b/control/shift_decider/include/shift_decider/shift_decider.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -31,26 +31,24 @@ class ShiftDecider : public rclcpp::Node private: void onTimer(); - void onControlCmd(autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg); - void onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg); - void onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg); + void onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg); + void onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg); + void onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg); void updateCurrentShiftCmd(); void initTimer(double period_s); - rclcpp::Publisher::SharedPtr pub_shift_cmd_; - rclcpp::Subscription::SharedPtr - sub_control_cmd_; - rclcpp::Subscription::SharedPtr - sub_autoware_state_; - rclcpp::Subscription::SharedPtr sub_current_gear_; + rclcpp::Publisher::SharedPtr pub_shift_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_autoware_state_; + rclcpp::Subscription::SharedPtr sub_current_gear_; rclcpp::TimerBase::SharedPtr timer_; - autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr control_cmd_; - autoware_auto_system_msgs::msg::AutowareState::SharedPtr autoware_state_; - autoware_auto_vehicle_msgs::msg::GearCommand shift_cmd_; - autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; - uint8_t prev_shift_command = autoware_auto_vehicle_msgs::msg::GearCommand::PARK; + autoware_control_msgs::msg::Control::SharedPtr control_cmd_; + autoware_system_msgs::msg::AutowareState::SharedPtr autoware_state_; + autoware_vehicle_msgs::msg::GearCommand shift_cmd_; + autoware_vehicle_msgs::msg::GearReport::SharedPtr current_gear_ptr_; + uint8_t prev_shift_command = autoware_vehicle_msgs::msg::GearCommand::PARK; bool park_on_goal_; }; diff --git a/control/shift_decider/package.xml b/control/shift_decider/package.xml index b24dbab1786e1..885e780c90bcc 100644 --- a/control/shift_decider/package.xml +++ b/control/shift_decider/package.xml @@ -12,9 +12,9 @@ ament_cmake autoware_cmake - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs rclcpp rclcpp_components diff --git a/control/shift_decider/src/shift_decider.cpp b/control/shift_decider/src/shift_decider.cpp index 0e47cc7378f27..f003513060a34 100644 --- a/control/shift_decider/src/shift_decider.cpp +++ b/control/shift_decider/src/shift_decider.cpp @@ -33,29 +33,28 @@ ShiftDecider::ShiftDecider(const rclcpp::NodeOptions & node_options) park_on_goal_ = declare_parameter("park_on_goal"); pub_shift_cmd_ = - create_publisher("output/gear_cmd", durable_qos); - sub_control_cmd_ = create_subscription( + create_publisher("output/gear_cmd", durable_qos); + sub_control_cmd_ = create_subscription( "input/control_cmd", queue_size, std::bind(&ShiftDecider::onControlCmd, this, _1)); - sub_autoware_state_ = create_subscription( + sub_autoware_state_ = create_subscription( "input/state", queue_size, std::bind(&ShiftDecider::onAutowareState, this, _1)); - sub_current_gear_ = create_subscription( + sub_current_gear_ = create_subscription( "input/current_gear", queue_size, std::bind(&ShiftDecider::onCurrentGear, this, _1)); initTimer(0.1); } -void ShiftDecider::onControlCmd( - autoware_auto_control_msgs::msg::AckermannControlCommand::SharedPtr msg) +void ShiftDecider::onControlCmd(autoware_control_msgs::msg::Control::SharedPtr msg) { control_cmd_ = msg; } -void ShiftDecider::onAutowareState(autoware_auto_system_msgs::msg::AutowareState::SharedPtr msg) +void ShiftDecider::onAutowareState(autoware_system_msgs::msg::AutowareState::SharedPtr msg) { autoware_state_ = msg; } -void ShiftDecider::onCurrentGear(autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) +void ShiftDecider::onCurrentGear(autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) { current_gear_ptr_ = msg; } @@ -72,15 +71,15 @@ void ShiftDecider::onTimer() void ShiftDecider::updateCurrentShiftCmd() { - using autoware_auto_system_msgs::msg::AutowareState; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_system_msgs::msg::AutowareState; + using autoware_vehicle_msgs::msg::GearCommand; shift_cmd_.stamp = now(); static constexpr double vel_threshold = 0.01; // to prevent chattering if (autoware_state_->state == AutowareState::DRIVING) { - if (control_cmd_->longitudinal.speed > vel_threshold) { + if (control_cmd_->longitudinal.velocity > vel_threshold) { shift_cmd_.command = GearCommand::DRIVE; - } else if (control_cmd_->longitudinal.speed < -vel_threshold) { + } else if (control_cmd_->longitudinal.velocity < -vel_threshold) { shift_cmd_.command = GearCommand::REVERSE; } else { shift_cmd_.command = prev_shift_command; diff --git a/control/smart_mpc_trajectory_follower/package.xml b/control/smart_mpc_trajectory_follower/package.xml index 70bd15f97d609..6cdd689c8bd5a 100644 --- a/control/smart_mpc_trajectory_follower/package.xml +++ b/control/smart_mpc_trajectory_follower/package.xml @@ -17,10 +17,10 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs motion_utils pybind11_vendor python3-scipy diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py index 409b6bf594c9e..992a5fd53a10c 100755 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py +++ b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py @@ -19,10 +19,10 @@ import time from autoware_adapi_v1_msgs.msg import OperationModeState -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_planning_msgs.msg import Trajectory -from autoware_auto_planning_msgs.msg import TrajectoryPoint -from autoware_auto_vehicle_msgs.msg import SteeringReport +from autoware_control_msgs.msg import Control +from autoware_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import TrajectoryPoint +from autoware_vehicle_msgs.msg import SteeringReport from builtin_interfaces.msg import Duration from geometry_msgs.msg import AccelWithCovarianceStamped from geometry_msgs.msg import PoseStamped @@ -128,7 +128,7 @@ def __init__(self): self.sub_reload_mpc_param_trigger_ self.sub_control_command_control_cmd_ = self.create_subscription( - AckermannControlCommand, + Control, "/control/command/control_cmd", self.onControlCommandControlCmd, 3, @@ -136,7 +136,7 @@ def __init__(self): self.sub_control_command_control_cmd_ self.control_cmd_pub_ = self.create_publisher( - AckermannControlCommand, + Control, "/control/trajectory_follower/control_cmd", 1, ) @@ -699,7 +699,7 @@ def control(self): else: steer_cmd = 0.0 - cmd_msg = AckermannControlCommand() + cmd_msg = Control() cmd_msg.stamp = cmd_msg.lateral.stamp = cmd_msg.longitudinal.stamp = ( self.get_clock().now().to_msg() ) diff --git a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash index 524dfe5a169df..d2687b352a17d 100644 --- a/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash +++ b/control/smart_mpc_trajectory_follower/smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash @@ -2,16 +2,16 @@ gnome-terminal -- bash -c 'ros2 topic echo /localization/kinematic_state nav_msgs/msg/Odometry --csv --qos-history keep_all --qos-reliability reliable > kinematic_state.csv' gnome-terminal -- bash -c 'ros2 topic echo /localization/acceleration geometry_msgs/msg/AccelWithCovarianceStamped --csv --qos-history keep_all --qos-reliability reliable > acceleration.csv' -gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autoware_auto_vehicle_msgs/msg/SteeringReport --csv --qos-history keep_all --qos-reliability reliable > steering_status.csv' +gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/steering_status autoware_vehicle_msgs/msg/SteeringReport --csv --qos-history keep_all --qos-reliability reliable > steering_status.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_auto_control_msgs/msg/AckermannControlCommand --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' -gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_auto_control_msgs/msg/AckermannControlCommand --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/command/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd.csv' +gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/control_cmd autoware_control_msgs/msg/Control --csv --qos-history keep_all --qos-reliability reliable > control_cmd_orig.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/lateral tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > lateral_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /control/trajectory_follower/lane_departure_checker_node/debug/deviation/yaw tier4_debug_msgs/msg/Float64Stamped --csv --qos-history keep_all --qos-reliability reliable > yaw_error.csv' gnome-terminal -- bash -c 'ros2 topic echo /system/operation_mode/state autoware_adapi_v1_msgs/msg/OperationModeState --csv --qos-history keep_all --qos-reliability reliable > system_operation_mode_state.csv' -gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_auto_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' +gnome-terminal -- bash -c 'ros2 topic echo /vehicle/status/control_mode autoware_vehicle_msgs/msg/ControlModeReport --csv --qos-history keep_all --qos-reliability reliable > vehicle_status_control_mode.csv' gnome-terminal -- bash -c 'ros2 topic echo /sensing/imu/imu_data sensor_msgs/msg/Imu --csv --qos-history keep_all --qos-reliability reliable > imu.csv' gnome-terminal -- bash -c 'ros2 topic echo /debug_mpc_x_des tier4_debug_msgs/msg/Float32MultiArrayStamped --csv --qos-history keep_all --qos-reliability reliable > debug_mpc_x_des.csv' diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp index 52a82526a9548..665a8604214dd 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/input_data.hpp @@ -16,8 +16,8 @@ #define TRAJECTORY_FOLLOWER_BASE__INPUT_DATA_HPP_ #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -25,9 +25,9 @@ namespace autoware::motion::control::trajectory_follower { struct InputData { - autoware_auto_planning_msgs::msg::Trajectory current_trajectory; + autoware_planning_msgs::msg::Trajectory current_trajectory; nav_msgs::msg::Odometry current_odometry; - autoware_auto_vehicle_msgs::msg::SteeringReport current_steering; + autoware_vehicle_msgs::msg::SteeringReport current_steering; geometry_msgs::msg::AccelWithCovarianceStamped current_accel; autoware_adapi_v1_msgs::msg::OperationModeState current_operation_mode; }; diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp index 657c981414c32..a70c4c18fedb3 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/lateral_controller_base.hpp @@ -19,7 +19,7 @@ #include "trajectory_follower_base/input_data.hpp" #include "trajectory_follower_base/sync_data.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" +#include "autoware_control_msgs/msg/lateral.hpp" #include @@ -27,7 +27,7 @@ namespace autoware::motion::control::trajectory_follower { struct LateralOutput { - autoware_auto_control_msgs::msg::AckermannLateralCommand control_cmd; + autoware_control_msgs::msg::Lateral control_cmd; LateralSyncData sync_data; }; diff --git a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp index 0f9c0d57bb5cd..da5381091113f 100644 --- a/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp +++ b/control/trajectory_follower_base/include/trajectory_follower_base/longitudinal_controller_base.hpp @@ -19,7 +19,7 @@ #include "trajectory_follower_base/input_data.hpp" #include "trajectory_follower_base/sync_data.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" #include @@ -27,7 +27,7 @@ namespace autoware::motion::control::trajectory_follower { struct LongitudinalOutput { - autoware_auto_control_msgs::msg::LongitudinalCommand control_cmd; + autoware_control_msgs::msg::Longitudinal control_cmd; LongitudinalSyncData sync_data; }; class LongitudinalControllerBase diff --git a/control/trajectory_follower_base/package.xml b/control/trajectory_follower_base/package.xml index d896f874a3a20..6f2e9c3e8ebc2 100644 --- a/control/trajectory_follower_base/package.xml +++ b/control/trajectory_follower_base/package.xml @@ -20,9 +20,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs diagnostic_msgs diagnostic_updater eigen diff --git a/control/trajectory_follower_node/README.md b/control/trajectory_follower_node/README.md index c3ea3ddf6b7e8..3496284efd670 100644 --- a/control/trajectory_follower_node/README.md +++ b/control/trajectory_follower_node/README.md @@ -129,19 +129,19 @@ Giving the longitudinal controller information about steer convergence allows it #### Inputs -- `autoware_auto_planning_msgs/Trajectory` : reference trajectory to follow. +- `autoware_planning_msgs/Trajectory` : reference trajectory to follow. - `nav_msgs/Odometry`: current odometry -- `autoware_auto_vehicle_msgs/SteeringReport` current steering +- `autoware_vehicle_msgs/SteeringReport` current steering #### Outputs -- `autoware_auto_control_msgs/AckermannControlCommand`: message containing both lateral and longitudinal commands. +- `autoware_control_msgs/Control`: message containing both lateral and longitudinal commands. #### Parameter - `ctrl_period`: control commands publishing period - `timeout_thr_sec`: duration in second after which input messages are discarded. - - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `AckermannControlCommand` if the following two conditions are met. + - Each time the node receives lateral and longitudinal commands from each controller, it publishes an `Control` if the following two conditions are met. 1. Both commands have been received. 2. The last received commands are not older than defined by `timeout_thr_sec`. - `lateral_controller_mode`: `mpc` or `pure_pursuit` diff --git a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md index 24ffe3166bbe4..4c1ee1b422484 100644 --- a/control/trajectory_follower_node/design/simple_trajectory_follower-design.md +++ b/control/trajectory_follower_node/design/simple_trajectory_follower-design.md @@ -10,10 +10,10 @@ Provide a base trajectory follower code that is simple and flexible to use. This Inputs -- `input/reference_trajectory` [autoware_auto_planning_msgs::msg::Trajectory] : reference trajectory to follow. +- `input/reference_trajectory` [autoware_planning_msgs::msg::Trajectory] : reference trajectory to follow. - `input/current_kinematic_state` [nav_msgs::msg::Odometry] : current state of the vehicle (position, velocity, etc). - Output -- `output/control_cmd` [autoware_auto_control_msgs::msg::AckermannControlCommand] : generated control command. +- `output/control_cmd` [autoware_control_msgs::msg::Control] : generated control command. ### Parameters diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index be4fe885ae8c1..8e9752ba19f66 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -30,10 +30,9 @@ #include #include -#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_control_msgs/msg/longitudinal.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -76,20 +75,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; - rclcpp::Subscription::SharedPtr sub_ref_path_; + rclcpp::Subscription::SharedPtr sub_ref_path_; rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_steering_; + rclcpp::Subscription::SharedPtr sub_steering_; rclcpp::Subscription::SharedPtr sub_accel_; rclcpp::Subscription::SharedPtr sub_operation_mode_; - rclcpp::Publisher::SharedPtr - control_cmd_pub_; + rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; rclcpp::Publisher::SharedPtr debug_marker_pub_; - autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; + autoware_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; - autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_; OperationModeState::SharedPtr current_operation_mode_ptr_; @@ -108,9 +106,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node */ boost::optional createInputData(rclcpp::Clock & clock) const; void callbackTimerControl(); - void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); + void onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr); void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); - void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); + void onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp index e748bdf25d419..e744243969cab 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp @@ -17,9 +17,9 @@ #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -28,9 +28,9 @@ namespace simple_trajectory_follower { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_control_msgs::msg::Control; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; @@ -44,7 +44,7 @@ class SimpleTrajectoryFollower : public rclcpp::Node private: rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::TimerBase::SharedPtr timer_; Trajectory::SharedPtr trajectory_; diff --git a/control/trajectory_follower_node/package.xml b/control/trajectory_follower_node/package.xml index 36b4d0108de78..65446dfb3dd01 100644 --- a/control/trajectory_follower_node/package.xml +++ b/control/trajectory_follower_node/package.xml @@ -20,10 +20,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_planning_msgs + autoware_vehicle_msgs motion_utils mpc_lateral_controller pid_longitudinal_controller diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 801f42ad9a470..6fe63ca07de6f 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -62,9 +62,9 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control throw std::domain_error("[LongitudinalController] invalid algorithm"); } - sub_ref_path_ = create_subscription( + sub_ref_path_ = create_subscription( "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1)); - sub_steering_ = create_subscription( + sub_steering_ = create_subscription( "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); sub_odometry_ = create_subscription( "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); @@ -73,7 +73,7 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control sub_operation_mode_ = create_subscription( "~/input/current_operation_mode", rclcpp::QoS{1}, [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); - control_cmd_pub_ = create_publisher( + control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); pub_processing_time_lat_ms_ = create_publisher("~/lateral/debug/processing_time_ms", 1); @@ -112,7 +112,7 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode return LongitudinalControllerMode::INVALID; } -void Controller::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +void Controller::onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) { current_trajectory_ptr_ = msg; } @@ -122,7 +122,7 @@ void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) current_odometry_ptr_ = msg; } -void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) +void Controller::onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { current_steering_ptr_ = msg; } @@ -227,7 +227,7 @@ void Controller::callbackTimerControl() if (isTimeOut(lon_out, lat_out)) return; // 5. publish control command - autoware_auto_control_msgs::msg::AckermannControlCommand out; + autoware_control_msgs::msg::Control out; out.stamp = this->now(); out.lateral = lat_out.control_cmd; out.longitudinal = lon_out.control_cmd; diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index 5e9488bf2ca93..7c27eaed41380 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -29,7 +29,7 @@ using tier4_autoware_utils::calcYawDeviation; SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & options) : Node("simple_trajectory_follower", options) { - pub_cmd_ = create_publisher("output/control_cmd", 1); + pub_cmd_ = create_publisher("output/control_cmd", 1); sub_kinematics_ = create_subscription( "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); @@ -54,11 +54,12 @@ void SimpleTrajectoryFollower::onTimer() updateClosest(); - AckermannControlCommand cmd; + Control cmd; cmd.stamp = cmd.lateral.stamp = cmd.longitudinal.stamp = get_clock()->now(); cmd.lateral.steering_tire_angle = static_cast(calcSteerCmd()); - cmd.longitudinal.speed = use_external_target_vel_ ? static_cast(external_target_vel_) - : closest_traj_point_.longitudinal_velocity_mps; + cmd.longitudinal.velocity = use_external_target_vel_ + ? static_cast(external_target_vel_) + : closest_traj_point_.longitudinal_velocity_mps; cmd.longitudinal.acceleration = static_cast(calcAccCmd()); pub_cmd_->publish(cmd); } diff --git a/control/trajectory_follower_node/test/test_controller_node.cpp b/control/trajectory_follower_node/test/test_controller_node.cpp index 1aa4035e98d51..9bdf625226134 100644 --- a/control/trajectory_follower_node/test/test_controller_node.cpp +++ b/control/trajectory_follower_node/test/test_controller_node.cpp @@ -21,10 +21,9 @@ #include "trajectory_follower_test_utils.hpp" #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_control_msgs/msg/ackermann_lateral_command.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" +#include "autoware_control_msgs/msg/control.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -35,11 +34,11 @@ #include using Controller = autoware::motion::control::trajectory_follower_node::Controller; -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; +using Control = autoware_control_msgs::msg::Control; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; using VehicleOdometry = nav_msgs::msg::Odometry; -using SteeringReport = autoware_auto_vehicle_msgs::msg::SteeringReport; +using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; using autoware_adapi_v1_msgs::msg::OperationModeState; using geometry_msgs::msg::AccelWithCovarianceStamped; @@ -96,7 +95,7 @@ class ControllerTester FakeNodeFixture * fnf; std::shared_ptr node; - AckermannControlCommand::SharedPtr cmd_msg; + Control::SharedPtr cmd_msg; bool received_control_command = false; void publish_default_odom() @@ -178,13 +177,11 @@ class ControllerTester rclcpp::Publisher::SharedPtr operation_mode_pub = fnf->create_publisher("controller/input/current_operation_mode"); - rclcpp::Subscription::SharedPtr cmd_sub = - fnf->create_subscription( - "controller/output/control_cmd", *fnf->get_fake_node(), - [this](const AckermannControlCommand::SharedPtr msg) { - cmd_msg = msg; - received_control_command = true; - }); + rclcpp::Subscription::SharedPtr cmd_sub = fnf->create_subscription( + "controller/output/control_cmd", *fnf->get_fake_node(), [this](const Control::SharedPtr msg) { + cmd_msg = msg; + received_control_command = true; + }); std::shared_ptr br = std::make_shared(fnf->get_fake_node()); @@ -255,7 +252,7 @@ TEST_F(FakeNodeFixture, straight_trajectory) // following conditions will pass even if the MPC solution does not converge EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_angle, 0.0f); EXPECT_EQ(tester.cmd_msg->lateral.steering_tire_rotation_rate, 0.0f); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_GT(rclcpp::Time(tester.cmd_msg->stamp), rclcpp::Time(traj_msg.header.stamp)); } @@ -369,14 +366,14 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0); EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); // Generate another control message tester.traj_pub->publish(traj_msg); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0); EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.acceleration, 0.0); } @@ -406,14 +403,14 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message tester.traj_pub->publish(traj); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -443,14 +440,14 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); // Generate another control message tester.traj_pub->publish(traj); test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_GT(tester.cmd_msg->longitudinal.speed, static_cast(odom_vx)); + EXPECT_GT(tester.cmd_msg->longitudinal.velocity, static_cast(odom_vx)); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -477,7 +474,7 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_LT( tester.cmd_msg->longitudinal.acceleration, 0.0f); // when stopped negative acceleration to brake @@ -507,7 +504,7 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) test_utils::waitForMessage(tester.node, this, tester.received_control_command); ASSERT_TRUE(tester.received_control_command); - EXPECT_LT(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_LT(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -535,7 +532,7 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) ASSERT_TRUE(tester.received_control_command); // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } @@ -566,7 +563,7 @@ TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) ASSERT_TRUE(tester.received_control_command); // Not keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 1.0f); } TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) @@ -597,5 +594,5 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) ASSERT_TRUE(tester.received_control_command); // Keep stopped state when the lateral control is not converged. - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); } diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 6ec2da84a7b6c..2d6f5c5f949af 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -8,38 +8,38 @@ ### Input -| Name | Type | Description | -| ------------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------------------- | -| `~/input/steering` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering status | -| `~/input/auto/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from planning module | -| `~/input/auto/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | -| `~/input/auto/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | -| `~/input/auto/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from planning module | -| `~/input/external/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from external | -| `~/input/external/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | -| `~/input/external/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | -| `~/input/external/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from external | -| `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | -| `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/input/emergency/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity from emergency handler | -| `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | -| `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | -| `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | +| Name | Type | Description | +| ------------------------------------------- | --------------------------------------------------- | -------------------------------------------------------------------- | +| `~/input/steering` | `autoware_vehicle_msgs::msg::SteeringReport` | steering status | +| `~/input/auto/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from planning module | +| `~/input/auto/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from planning module | +| `~/input/auto/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from planning module | +| `~/input/auto/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from planning module | +| `~/input/external/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from external | +| `~/input/external/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command from external | +| `~/input/external/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from external | +| `~/input/external/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from external | +| `~/input/external_emergency_stop_heartbeat` | `tier4_external_api_msgs::msg::Heartbeat` | heartbeat | +| `~/input/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/input/emergency/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity from emergency handler | +| `~/input/emergency/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | +| `~/input/emergency/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | +| `~/input/engage` | `autoware_vehicle_msgs::msg::Engage` | engage signal | +| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | ### Output -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------------- | -------------------------------------------------------- | -| `~/output/vehicle_cmd_emergency` | `autoware_auto_system_msgs::msg::EmergencyState` | emergency state which was originally in vehicle command | -| `~/output/command/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | command for lateral and longitudinal velocity to vehicle | -| `~/output/command/turn_indicators_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | -| `~/output/command/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | -| `~/output/command/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command to vehicle | -| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | -| `~/output/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | -| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------------- | -------------------------------------------------------- | +| `~/output/vehicle_cmd_emergency` | `tier4_vehicle_msgs::msg::VehicleEmergencyStamped` | emergency state which was originally in vehicle command | +| `~/output/command/control_cmd` | `autoware_control_msgs::msg::Control` | command for lateral and longitudinal velocity to vehicle | +| `~/output/command/turn_indicators_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command to vehicle | +| `~/output/command/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command to vehicle | +| `~/output/command/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | gear command to vehicle | +| `~/output/gate_mode` | `tier4_control_msgs::msg::GateMode` | gate mode (AUTO or EXTERNAL) | +| `~/output/engage` | `autoware_vehicle_msgs::msg::Engage` | engage signal | +| `~/output/external_emergency` | `tier4_external_api_msgs::msg::Emergency` | external emergency signal | +| `~/output/operation_mode` | `tier4_system_msgs::msg::OperationMode` | current operation mode of the vehicle_cmd_gate | ## Parameters diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index 22ae9da6d222e..e01d566c37df9 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -16,9 +16,8 @@ rosidl_default_generators autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils diagnostic_updater diff --git a/control/vehicle_cmd_gate/script/delays_checker.py b/control/vehicle_cmd_gate/script/delays_checker.py index 564b7c7ac94e5..a67c794877a92 100644 --- a/control/vehicle_cmd_gate/script/delays_checker.py +++ b/control/vehicle_cmd_gate/script/delays_checker.py @@ -14,8 +14,8 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_control_msgs.msg import AckermannControlCommand -from autoware_auto_vehicle_msgs.msg import Engage +from autoware_control_msgs.msg import Control +from autoware_vehicle_msgs.msg import Engage from geometry_msgs.msg import AccelWithCovarianceStamped from nav_msgs.msg import Odometry import rclpy @@ -68,13 +68,13 @@ def __init__(self): ) self.sub_engage = self.create_subscription(Engage, engage_topic, self.CallBackEngage, 1) self.sub_in_gate_cmd = self.create_subscription( - AckermannControlCommand, + Control, in_gate_cmd_topic, self.CallBackInCmd, 1, ) self.sub_out_gate_cmd = self.create_subscription( - AckermannControlCommand, + Control, out_gate_cmd_topic, self.CallBackOutCmd, 1, diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp b/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp index afc86f0b4fb20..f4cf28d337a09 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp +++ b/control/vehicle_cmd_gate/src/adapi_pause_interface.cpp @@ -53,9 +53,9 @@ void AdapiPauseInterface::publish() } } -void AdapiPauseInterface::update(const AckermannControlCommand & control) +void AdapiPauseInterface::update(const Control & control) { - is_start_requested_ = eps < std::abs(control.longitudinal.speed); + is_start_requested_ = eps < std::abs(control.longitudinal.velocity); } void AdapiPauseInterface::on_pause( diff --git a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp b/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp index c4294ee5f643d..1d5f05e98975e 100644 --- a/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp +++ b/control/vehicle_cmd_gate/src/adapi_pause_interface.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include namespace vehicle_cmd_gate { @@ -28,7 +28,7 @@ class AdapiPauseInterface { private: static constexpr double eps = 1e-3; - using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Control = autoware_control_msgs::msg::Control; using SetPause = control_interface::SetPause; using IsPaused = control_interface::IsPaused; using IsStartRequested = control_interface::IsStartRequested; @@ -37,7 +37,7 @@ class AdapiPauseInterface explicit AdapiPauseInterface(rclcpp::Node * node); bool is_paused(); void publish(); - void update(const AckermannControlCommand & control); + void update(const Control & control); private: bool is_paused_; diff --git a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp index 7ef30b4926b7e..b643afc212f61 100644 --- a/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp +++ b/control/vehicle_cmd_gate/src/moderate_stop_interface.hpp @@ -19,8 +19,6 @@ #include #include -#include - #include #include diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index b289365b46b3b..bd9955e773020 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -95,24 +95,23 @@ VehicleCmdFilterParam VehicleCmdFilter::getParam() const return param_; } -void VehicleCmdFilter::limitLongitudinalWithVel(AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithVel(Control & input) const { - input.longitudinal.speed = std::max( - std::min(static_cast(input.longitudinal.speed), param_.vel_lim), -param_.vel_lim); + input.longitudinal.velocity = std::max( + std::min(static_cast(input.longitudinal.velocity), param_.vel_lim), -param_.vel_lim); } -void VehicleCmdFilter::limitLongitudinalWithAcc( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLongitudinalWithAcc(const double dt, Control & input) const { const auto lon_acc_lim = getLonAccLim(); input.longitudinal.acceleration = std::max( std::min(static_cast(input.longitudinal.acceleration), lon_acc_lim), -lon_acc_lim); - input.longitudinal.speed = - limitDiff(input.longitudinal.speed, prev_cmd_.longitudinal.speed, lon_acc_lim * dt); + input.longitudinal.velocity = + limitDiff(input.longitudinal.velocity, prev_cmd_.longitudinal.velocity, lon_acc_lim * dt); } void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( - const double dt, AckermannControlCommand & input) const + const double dt, Control & input) const { const auto lon_jerk_lim = getLonJerkLim(); input.longitudinal.acceleration = limitDiff( @@ -124,7 +123,7 @@ void VehicleCmdFilter::VehicleCmdFilter::limitLongitudinalWithJerk( // Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the // filtered steering angle oscillates if the input velocity oscillates. void VehicleCmdFilter::limitLateralWithLatAcc( - [[maybe_unused]] const double dt, AckermannControlCommand & input) const + [[maybe_unused]] const double dt, Control & input) const { const auto lat_acc_lim = getLatAccLim(); @@ -138,8 +137,7 @@ void VehicleCmdFilter::limitLateralWithLatAcc( // Use ego vehicle speed (not speed command) for the lateral acceleration calculation, otherwise the // filtered steering angle oscillates if the input velocity oscillates. -void VehicleCmdFilter::limitLateralWithLatJerk( - const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralWithLatJerk(const double dt, Control & input) const { double curr_latacc = calcLatAcc(input, current_speed_); double prev_latacc = calcLatAcc(prev_cmd_, current_speed_); @@ -156,8 +154,7 @@ void VehicleCmdFilter::limitLateralWithLatJerk( } } -void VehicleCmdFilter::limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const +void VehicleCmdFilter::limitActualSteerDiff(const double current_steer_angle, Control & input) const { const auto actual_steer_diff_lim = getSteerDiffLim(); @@ -166,7 +163,7 @@ void VehicleCmdFilter::limitActualSteerDiff( input.lateral.steering_tire_angle = current_steer_angle + ds; } -void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralSteer(Control & input) const { const float steer_limit = getSteerLim(); @@ -185,7 +182,7 @@ void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const } } -void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCommand & input) const +void VehicleCmdFilter::limitLateralSteerRate(const double dt, Control & input) const { const float steer_rate_limit = getSteerRateLim(); @@ -201,7 +198,7 @@ void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCo } void VehicleCmdFilter::filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & cmd, + const double dt, const double current_steer_angle, Control & cmd, IsFilterActivated & is_activated) const { const auto cmd_orig = cmd; @@ -219,14 +216,14 @@ void VehicleCmdFilter::filterAll( } IsFilterActivated VehicleCmdFilter::checkIsActivated( - const AckermannControlCommand & c1, const AckermannControlCommand & c2, const double tol) + const Control & c1, const Control & c2, const double tol) { IsFilterActivated msg; msg.is_activated_on_steering = std::abs(c1.lateral.steering_tire_angle - c2.lateral.steering_tire_angle) > tol; msg.is_activated_on_steering_rate = std::abs(c1.lateral.steering_tire_rotation_rate - c2.lateral.steering_tire_rotation_rate) > tol; - msg.is_activated_on_speed = std::abs(c1.longitudinal.speed - c2.longitudinal.speed) > tol; + msg.is_activated_on_speed = std::abs(c1.longitudinal.velocity - c2.longitudinal.velocity) > tol; msg.is_activated_on_acceleration = std::abs(c1.longitudinal.acceleration - c2.longitudinal.acceleration) > tol; msg.is_activated_on_jerk = std::abs(c1.longitudinal.jerk - c2.longitudinal.jerk) > tol; @@ -244,13 +241,13 @@ double VehicleCmdFilter::calcSteerFromLatacc(const double v, const double latacc return std::atan(latacc * param_.wheel_base / v_sq); } -double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd) const +double VehicleCmdFilter::calcLatAcc(const Control & cmd) const { - double v = cmd.longitudinal.speed; + double v = cmd.longitudinal.velocity; return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } -double VehicleCmdFilter::calcLatAcc(const AckermannControlCommand & cmd, const double v) const +double VehicleCmdFilter::calcLatAcc(const Control & cmd, const double v) const { return v * v * std::tan(cmd.lateral.steering_tire_angle) / param_.wheel_base; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index e71fb405beda1..d9b8383a1de51 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -18,13 +18,13 @@ #include #include -#include +#include #include namespace vehicle_cmd_gate { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using vehicle_cmd_gate::msg::IsFilterActivated; using LimitArray = std::vector; @@ -59,35 +59,33 @@ class VehicleCmdFilter void setCurrentSpeed(double v) { current_speed_ = v; } void setParam(const VehicleCmdFilterParam & p); VehicleCmdFilterParam getParam() const; - void setPrevCmd(const AckermannControlCommand & v) { prev_cmd_ = v; } + void setPrevCmd(const Control & v) { prev_cmd_ = v; } - void limitLongitudinalWithVel(AckermannControlCommand & input) const; - void limitLongitudinalWithAcc(const double dt, AckermannControlCommand & input) const; - void limitLongitudinalWithJerk(const double dt, AckermannControlCommand & input) const; - void limitLateralWithLatAcc(const double dt, AckermannControlCommand & input) const; - void limitLateralWithLatJerk(const double dt, AckermannControlCommand & input) const; - void limitActualSteerDiff( - const double current_steer_angle, AckermannControlCommand & input) const; - void limitLateralSteer(AckermannControlCommand & input) const; - void limitLateralSteerRate(const double dt, AckermannControlCommand & input) const; + void limitLongitudinalWithVel(Control & input) const; + void limitLongitudinalWithAcc(const double dt, Control & input) const; + void limitLongitudinalWithJerk(const double dt, Control & input) const; + void limitLateralWithLatAcc(const double dt, Control & input) const; + void limitLateralWithLatJerk(const double dt, Control & input) const; + void limitActualSteerDiff(const double current_steer_angle, Control & input) const; + void limitLateralSteer(Control & input) const; + void limitLateralSteerRate(const double dt, Control & input) const; void filterAll( - const double dt, const double current_steer_angle, AckermannControlCommand & input, + const double dt, const double current_steer_angle, Control & input, IsFilterActivated & is_activated) const; static IsFilterActivated checkIsActivated( - const AckermannControlCommand & c1, const AckermannControlCommand & c2, - const double tol = 1.0e-3); + const Control & c1, const Control & c2, const double tol = 1.0e-3); - AckermannControlCommand getPrevCmd() { return prev_cmd_; } + Control getPrevCmd() { return prev_cmd_; } private: VehicleCmdFilterParam param_; - AckermannControlCommand prev_cmd_; + Control prev_cmd_; double current_speed_ = 0.0; bool setParameterWithValidation(const VehicleCmdFilterParam & p); - double calcLatAcc(const AckermannControlCommand & cmd) const; - double calcLatAcc(const AckermannControlCommand & cmd, const double v) const; + double calcLatAcc(const Control & cmd) const; + double calcLatAcc(const Control & cmd, const double v) const; double calcSteerFromLatacc(const double v, const double latacc) const; double limitDiff(const double curr, const double prev, const double diff_lim) const; diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index ffd452db0337c..cc9d7813083f4 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -62,7 +62,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Publisher vehicle_cmd_emergency_pub_ = create_publisher("output/vehicle_cmd_emergency", durable_qos); - control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); + control_cmd_pub_ = create_publisher("output/control_cmd", durable_qos); gear_cmd_pub_ = create_publisher("output/gear_cmd", durable_qos); turn_indicator_cmd_pub_ = create_publisher("output/turn_indicators_cmd", durable_qos); @@ -108,7 +108,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) "input/mrm_state", 1, std::bind(&VehicleCmdGate::onMrmState, this, _1)); // Subscriber for auto - auto_control_cmd_sub_ = create_subscription( + auto_control_cmd_sub_ = create_subscription( "input/auto/control_cmd", 1, std::bind(&VehicleCmdGate::onAutoCtrlCmd, this, _1)); auto_turn_indicator_cmd_sub_ = create_subscription( @@ -124,7 +124,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) [this](GearCommand::ConstSharedPtr msg) { auto_commands_.gear = *msg; }); // Subscriber for external - remote_control_cmd_sub_ = create_subscription( + remote_control_cmd_sub_ = create_subscription( "input/external/control_cmd", 1, std::bind(&VehicleCmdGate::onRemoteCtrlCmd, this, _1)); remote_turn_indicator_cmd_sub_ = create_subscription( @@ -140,7 +140,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) [this](GearCommand::ConstSharedPtr msg) { remote_commands_.gear = *msg; }); // Subscriber for emergency - emergency_control_cmd_sub_ = create_subscription( + emergency_control_cmd_sub_ = create_subscription( "input/emergency/control_cmd", 1, std::bind(&VehicleCmdGate::onEmergencyCtrlCmd, this, _1)); emergency_hazard_light_cmd_sub_ = create_subscription( @@ -354,7 +354,7 @@ bool VehicleCmdGate::isDataReady() } // for auto -void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onAutoCtrlCmd(Control::ConstSharedPtr msg) { auto_commands_.control = *msg; @@ -364,7 +364,7 @@ void VehicleCmdGate::onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) } // for remote -void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onRemoteCtrlCmd(Control::ConstSharedPtr msg) { remote_commands_.control = *msg; @@ -374,7 +374,7 @@ void VehicleCmdGate::onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg } // for emergency -void VehicleCmdGate::onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg) +void VehicleCmdGate::onEmergencyCtrlCmd(Control::ConstSharedPtr msg) { emergency_commands_.control = *msg; @@ -489,7 +489,7 @@ void VehicleCmdGate::publishControlCommands(const Commands & commands) } if (moderate_stop_interface_->is_stop_requested()) { // if stop requested, stop the vehicle - filtered_commands.control.longitudinal.speed = 0.0; + filtered_commands.control.longitudinal.velocity = 0.0; filtered_commands.control.longitudinal.acceleration = moderate_stop_service_acceleration_; } @@ -543,7 +543,7 @@ void VehicleCmdGate::publishEmergencyStopControlCommands() const auto stamp = this->now(); // ControlCommand - AckermannControlCommand control_cmd; + Control control_cmd; control_cmd.stamp = stamp; control_cmd = createEmergencyStopControlCmd(); @@ -600,9 +600,9 @@ void VehicleCmdGate::publishStatus() moderate_stop_interface_->publish(); } -AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannControlCommand & in) +Control VehicleCmdGate::filterControlCommand(const Control & in) { - AckermannControlCommand out = in; + Control out = in; const double dt = getDt(); const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); @@ -629,10 +629,10 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont prev_cmd.longitudinal.acceleration = std::max(prev_cmd.longitudinal.acceleration, current_status_cmd.longitudinal.acceleration); // consider reverse driving - prev_cmd.longitudinal.speed = - std::fabs(prev_cmd.longitudinal.speed) > std::fabs(current_status_cmd.longitudinal.speed) - ? prev_cmd.longitudinal.speed - : current_status_cmd.longitudinal.speed; + prev_cmd.longitudinal.velocity = std::fabs(prev_cmd.longitudinal.velocity) > + std::fabs(current_status_cmd.longitudinal.velocity) + ? prev_cmd.longitudinal.velocity + : current_status_cmd.longitudinal.velocity; filter_.setPrevCmd(prev_cmd); } filter_.filterAll(dt, current_steer_, out, is_filter_activated); @@ -663,42 +663,42 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont return out; } -AckermannControlCommand VehicleCmdGate::createStopControlCmd() const +Control VehicleCmdGate::createStopControlCmd() const { - AckermannControlCommand cmd; + Control cmd; const auto t = this->now(); cmd.stamp = t; cmd.lateral.stamp = t; cmd.longitudinal.stamp = t; cmd.lateral.steering_tire_angle = current_steer_; cmd.lateral.steering_tire_rotation_rate = 0.0; - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = stop_hold_acceleration_; return cmd; } -LongitudinalCommand VehicleCmdGate::createLongitudinalStopControlCmd() const +Longitudinal VehicleCmdGate::createLongitudinalStopControlCmd() const { - LongitudinalCommand cmd; + Longitudinal cmd; const auto t = this->now(); cmd.stamp = t; - cmd.speed = 0.0; + cmd.velocity = 0.0; cmd.acceleration = stop_hold_acceleration_; return cmd; } -AckermannControlCommand VehicleCmdGate::createEmergencyStopControlCmd() const +Control VehicleCmdGate::createEmergencyStopControlCmd() const { - AckermannControlCommand cmd; + Control cmd; const auto t = this->now(); cmd.stamp = t; cmd.lateral.stamp = t; cmd.longitudinal.stamp = t; cmd.lateral.steering_tire_angle = prev_control_cmd_.lateral.steering_tire_angle; cmd.lateral.steering_tire_rotation_rate = prev_control_cmd_.lateral.steering_tire_rotation_rate; - cmd.longitudinal.speed = 0.0; + cmd.longitudinal.velocity = 0.0; cmd.longitudinal.acceleration = emergency_acceleration_; return cmd; @@ -757,13 +757,13 @@ double VehicleCmdGate::getDt() return dt; } -AckermannControlCommand VehicleCmdGate::getActualStatusAsCommand() +Control VehicleCmdGate::getActualStatusAsCommand() { - AckermannControlCommand status; + Control status; status.stamp = status.lateral.stamp = status.longitudinal.stamp = this->now(); status.lateral.steering_tire_angle = current_steer_; status.lateral.steering_tire_rotation_rate = 0.0; - status.longitudinal.speed = current_kinematics_.twist.twist.linear.x; + status.longitudinal.velocity = current_kinematics_.twist.twist.linear.x; status.longitudinal.acceleration = current_acceleration_; return status; } diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 85bc183361b94..f45280d9d2e48 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -29,12 +29,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -56,12 +56,12 @@ namespace vehicle_cmd_gate using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_control_msgs::msg::LongitudinalCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_control_msgs::msg::Control; +using autoware_control_msgs::msg::Longitudinal; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using std_srvs::srv::Trigger; using tier4_control_msgs::msg::GateMode; @@ -77,13 +77,13 @@ using visualization_msgs::msg::MarkerArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; -using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageMsg = autoware_vehicle_msgs::msg::Engage; using EngageSrv = tier4_external_api_msgs::srv::Engage; using motion_utils::VehicleStopChecker; struct Commands { - AckermannControlCommand control; + Control control; TurnIndicatorsCommand turn_indicator; HazardLightsCommand hazard_light; GearCommand gear; @@ -101,7 +101,7 @@ class VehicleCmdGate : public rclcpp::Node private: // Publisher rclcpp::Publisher::SharedPtr vehicle_cmd_emergency_pub_; - rclcpp::Publisher::SharedPtr control_cmd_pub_; + rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Publisher::SharedPtr gear_cmd_pub_; rclcpp::Publisher::SharedPtr turn_indicator_cmd_pub_; rclcpp::Publisher::SharedPtr hazard_light_cmd_pub_; @@ -153,26 +153,26 @@ class VehicleCmdGate : public rclcpp::Node // Subscriber for auto Commands auto_commands_; - rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; + rclcpp::Subscription::SharedPtr auto_control_cmd_sub_; rclcpp::Subscription::SharedPtr auto_turn_indicator_cmd_sub_; rclcpp::Subscription::SharedPtr auto_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr auto_gear_cmd_sub_; - void onAutoCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onAutoCtrlCmd(Control::ConstSharedPtr msg); // Subscription for external Commands remote_commands_; - rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; + rclcpp::Subscription::SharedPtr remote_control_cmd_sub_; rclcpp::Subscription::SharedPtr remote_turn_indicator_cmd_sub_; rclcpp::Subscription::SharedPtr remote_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr remote_gear_cmd_sub_; - void onRemoteCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onRemoteCtrlCmd(Control::ConstSharedPtr msg); // Subscription for emergency Commands emergency_commands_; - rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; + rclcpp::Subscription::SharedPtr emergency_control_cmd_sub_; rclcpp::Subscription::SharedPtr emergency_hazard_light_cmd_sub_; rclcpp::Subscription::SharedPtr emergency_gear_cmd_sub_; - void onEmergencyCtrlCmd(AckermannControlCommand::ConstSharedPtr msg); + void onEmergencyCtrlCmd(Control::ConstSharedPtr msg); // Parameter bool use_emergency_handling_; @@ -224,17 +224,17 @@ class VehicleCmdGate : public rclcpp::Node void checkExternalEmergencyStop(diagnostic_updater::DiagnosticStatusWrapper & stat); // Algorithm - AckermannControlCommand prev_control_cmd_; - AckermannControlCommand createStopControlCmd() const; - LongitudinalCommand createLongitudinalStopControlCmd() const; - AckermannControlCommand createEmergencyStopControlCmd() const; + Control prev_control_cmd_; + Control createStopControlCmd() const; + Longitudinal createLongitudinalStopControlCmd() const; + Control createEmergencyStopControlCmd() const; std::shared_ptr prev_time_; double getDt(); - AckermannControlCommand getActualStatusAsCommand(); + Control getActualStatusAsCommand(); VehicleCmdFilter filter_; - AckermannControlCommand filterControlCommand(const AckermannControlCommand & msg); + Control filterControlCommand(const Control & msg); // filtering on transition OperationModeState current_operation_mode_; diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 949ca46d27dea..d51db90c8a260 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -58,26 +58,25 @@ using vehicle_cmd_gate::VehicleCmdGate; using autoware_adapi_v1_msgs::msg::MrmState; using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::GearCommand; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_control_msgs::msg::Control; +using autoware_vehicle_msgs::msg::GearCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using nav_msgs::msg::Odometry; using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; -using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; +using EngageMsg = autoware_vehicle_msgs::msg::Engage; class PubSubNode : public rclcpp::Node { public: PubSubNode() : Node{"test_vehicle_cmd_gate_filter_pubsub"} { - sub_cmd_ = create_subscription( - "output/control_cmd", rclcpp::QoS{1}, - [this](const AckermannControlCommand::ConstSharedPtr msg) { + sub_cmd_ = create_subscription( + "output/control_cmd", rclcpp::QoS{1}, [this](const Control::ConstSharedPtr msg) { cmd_history_.push_back(msg); cmd_received_times_.push_back(now()); // check the effectiveness of the filter for last x elements in the queue @@ -97,8 +96,7 @@ class PubSubNode : public rclcpp::Node pub_operation_mode_ = create_publisher("input/operation_mode", qos); pub_mrm_state_ = create_publisher("input/mrm_state", qos); - pub_auto_control_cmd_ = - create_publisher("input/auto/control_cmd", qos); + pub_auto_control_cmd_ = create_publisher("input/auto/control_cmd", qos); pub_auto_turn_indicator_cmd_ = create_publisher("input/auto/turn_indicators_cmd", qos); pub_auto_hazard_light_cmd_ = @@ -106,7 +104,7 @@ class PubSubNode : public rclcpp::Node pub_auto_gear_cmd_ = create_publisher("input/auto/gear_cmd", qos); } - rclcpp::Subscription::SharedPtr sub_cmd_; + rclcpp::Subscription::SharedPtr sub_cmd_; rclcpp::Publisher::SharedPtr pub_external_emergency_stop_heartbeat_; rclcpp::Publisher::SharedPtr pub_engage_; @@ -116,13 +114,13 @@ class PubSubNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_steer_; rclcpp::Publisher::SharedPtr pub_operation_mode_; rclcpp::Publisher::SharedPtr pub_mrm_state_; - rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; + rclcpp::Publisher::SharedPtr pub_auto_control_cmd_; rclcpp::Publisher::SharedPtr pub_auto_turn_indicator_cmd_; rclcpp::Publisher::SharedPtr pub_auto_hazard_light_cmd_; rclcpp::Publisher::SharedPtr pub_auto_gear_cmd_; - std::vector cmd_history_; - std::vector raw_cmd_history_; + std::vector cmd_history_; + std::vector raw_cmd_history_; std::vector cmd_received_times_; // publish except for the control_cmd @@ -152,7 +150,7 @@ class PubSubNode : public rclcpp::Node msg.twist.twist.linear.x = 0.0; if (!cmd_history_.empty()) { // ego moves as commanded. msg.twist.twist.linear.x = - cmd_history_.back()->longitudinal.speed; // ego moves as commanded. + cmd_history_.back()->longitudinal.velocity; // ego moves as commanded. } pub_odom_->publish(msg); } @@ -209,11 +207,11 @@ class PubSubNode : public rclcpp::Node } } - void publishControlCommand(AckermannControlCommand msg) + void publishControlCommand(Control msg) { msg.stamp = now(); pub_auto_control_cmd_->publish(msg); - raw_cmd_history_.push_back(std::make_shared(msg)); + raw_cmd_history_.push_back(std::make_shared(msg)); } void checkFilter(size_t last_x) @@ -249,7 +247,7 @@ class PubSubNode : public rclcpp::Node // TODO(Horibe): prev_lat_acc should use the previous velocity, but use the current velocity // since the current filtering logic uses the current velocity. // when it's fixed, should be like this: - // prev_lat_acc = calculate_lateral_acceleration(cmd_start->longitudinal.speed, + // prev_lat_acc = calculate_lateral_acceleration(cmd_start->longitudinal.velocity, // cmd_start->lateral.steering_tire_angle, wheelbase); prev_tire_angle = cmd_start->lateral.steering_tire_angle; } @@ -261,7 +259,7 @@ class PubSubNode : public rclcpp::Node ASSERT_GT(dt, 0.0) << "Invalid dt. Time must be strictly positive."; - lon_vel = cmd->longitudinal.speed; + lon_vel = cmd->longitudinal.velocity; const auto lon_acc = cmd->longitudinal.acceleration; const auto lon_jerk = (lon_acc - prev_lon_acc) / dt; @@ -341,7 +339,7 @@ class ControlCmdGenerator // generate ControlCommand with sin wave format. // TODO(Horibe): implement steering_rate and jerk command. - AckermannControlCommand calcSinWaveCommand(bool reset_clock = false) + Control calcSinWaveCommand(bool reset_clock = false) { if (reset_clock) { start_time_ = Clock::now(); @@ -355,9 +353,9 @@ class ControlCmdGenerator return amp * std::sin(2.0 * M_PI * freq * dt_s + bias); }; - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = sinWave(p_.steering.max, p_.steering.freq, p_.steering.bias); - cmd.longitudinal.speed = + cmd.longitudinal.velocity = sinWave(p_.velocity.max, p_.velocity.freq, p_.velocity.bias) + p_.velocity.max; cmd.longitudinal.acceleration = sinWave(p_.acceleration.max, p_.acceleration.freq, p_.acceleration.bias); diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index b8200490dd1d5..5fbab1c5cfb4f 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -24,7 +24,7 @@ #define ASSERT_LT_NEAR(x, y) ASSERT_LT(x, y + THRESHOLD) #define ASSERT_GT_NEAR(x, y) ASSERT_GT(x, y - THRESHOLD) -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using vehicle_cmd_gate::LimitArray; constexpr double NOMINAL_INTERVAL = 1.0; @@ -49,38 +49,37 @@ void setFilterParams( f.setParam(p); } -AckermannControlCommand genCmd(double s, double sr, double v, double a) +Control genCmd(double s, double sr, double v, double a) { - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = s; cmd.lateral.steering_tire_rotation_rate = sr; - cmd.longitudinal.speed = v; + cmd.longitudinal.velocity = v; cmd.longitudinal.acceleration = a; return cmd; } // calc from ego velocity -double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase, const double ego_v) +double calcLatAcc(const Control & cmd, const double wheelbase, const double ego_v) { return ego_v * ego_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } // calc from command velocity -double calcLatAcc(const AckermannControlCommand & cmd, const double wheelbase) +double calcLatAcc(const Control & cmd, const double wheelbase) { - double v = cmd.longitudinal.speed; + double v = cmd.longitudinal.velocity; return v * v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; } // calc from command velocity double calcLatJerk( - const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, - const double wheelbase, const double dt) + const Control & cmd, const Control & prev_cmd, const double wheelbase, const double dt) { - const auto prev_v = prev_cmd.longitudinal.speed; + const auto prev_v = prev_cmd.longitudinal.velocity; const auto prev = prev_v * prev_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; - const auto curr_v = cmd.longitudinal.speed; + const auto curr_v = cmd.longitudinal.velocity; const auto curr = curr_v * curr_v * std::tan(cmd.lateral.steering_tire_angle) / wheelbase; return (curr - prev) / dt; @@ -88,8 +87,8 @@ double calcLatJerk( // calc from ego velocity double calcLatJerk( - const AckermannControlCommand & cmd, const AckermannControlCommand & prev_cmd, - const double wheelbase, const double dt, const double ego_v) + const Control & cmd, const Control & prev_cmd, const double wheelbase, const double dt, + const double ego_v) { const auto prev = ego_v * ego_v * std::tan(prev_cmd.lateral.steering_tire_angle) / wheelbase; @@ -100,8 +99,8 @@ double calcLatJerk( void test_1d_limit( double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, - double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, - const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) + double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, const Control & prev_cmd, + const Control & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] @@ -119,11 +118,11 @@ void test_1d_limit( filter.limitLongitudinalWithVel(filtered_cmd); // check if the filtered value does not exceed the limit. - ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, V_LIM); + ASSERT_LT_NEAR(filtered_cmd.longitudinal.velocity, V_LIM); // check if the undesired filter is not applied. - if (std::abs(raw_cmd.longitudinal.speed) < V_LIM) { - ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + if (std::abs(raw_cmd.longitudinal.velocity) < V_LIM) { + ASSERT_NEAR(filtered_cmd.longitudinal.velocity, raw_cmd.longitudinal.velocity, THRESHOLD); } } @@ -145,14 +144,14 @@ void test_1d_limit( } // check if the filtered value does not exceed the limit. - const double v_max = prev_cmd.longitudinal.speed + A_LIM * DT; - const double v_min = prev_cmd.longitudinal.speed - A_LIM * DT; - ASSERT_LT_NEAR(filtered_cmd.longitudinal.speed, v_max); - ASSERT_GT_NEAR(filtered_cmd.longitudinal.speed, v_min); + const double v_max = prev_cmd.longitudinal.velocity + A_LIM * DT; + const double v_min = prev_cmd.longitudinal.velocity - A_LIM * DT; + ASSERT_LT_NEAR(filtered_cmd.longitudinal.velocity, v_max); + ASSERT_GT_NEAR(filtered_cmd.longitudinal.velocity, v_min); // check if the undesired filter is not applied. - if (v_min < raw_cmd.longitudinal.speed && raw_cmd.longitudinal.speed < v_max) { - ASSERT_NEAR(filtered_cmd.longitudinal.speed, raw_cmd.longitudinal.speed, THRESHOLD); + if (v_min < raw_cmd.longitudinal.velocity && raw_cmd.longitudinal.velocity < v_max) { + ASSERT_NEAR(filtered_cmd.longitudinal.velocity, raw_cmd.longitudinal.velocity, THRESHOLD); } } @@ -241,10 +240,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector steer_rate_lim_arr = {0.01, 1.0, 100.0}; const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0}; - const std::vector prev_cmd_arr = { + const std::vector prev_cmd_arr = { genCmd(0.0, 0.0, 0.0, 0.0), genCmd(1.0, 1.0, 1.0, 1.0)}; - const std::vector raw_cmd_arr = { + const std::vector raw_cmd_arr = { genCmd(1.0, 1.0, 1.0, 1.0), genCmd(10.0, -1.0, -1.0, -1.0)}; for (const auto & v : v_arr) { @@ -294,10 +293,10 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto DT = 0.033; const auto orig_cmd = []() { - AckermannControlCommand cmd; + Control cmd; cmd.lateral.steering_tire_angle = 0.5; cmd.lateral.steering_tire_rotation_rate = 0.5; - cmd.longitudinal.speed = 30.0; + cmd.longitudinal.velocity = 30.0; cmd.longitudinal.acceleration = 10.0; cmd.longitudinal.jerk = 10.0; return cmd; @@ -353,7 +352,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // vel lim { set_speed_and_reset_prev(0.0); - EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); + EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.velocity, 20.0, ep); } // steer angle lim @@ -389,7 +388,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto calcSteerRateFromAngle = [&](const auto & cmd) { return (cmd.steering_tire_angle - 0.0) / DT; }; - autoware_auto_control_msgs::msg::AckermannLateralCommand filtered; + autoware_control_msgs::msg::Lateral filtered; set_speed_and_reset_prev(0.0); filtered = _limitSteerRate(orig_cmd).lateral; @@ -521,7 +520,7 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; // p.lat_jerk_lim = std::vector{0.9, 0.7, 0.1}; const auto _calcLatJerk = [&](const auto & cmd, const double ego_v) { - return calcLatJerk(cmd, AckermannControlCommand{}, WHEELBASE, DT, ego_v); + return calcLatJerk(cmd, Control{}, WHEELBASE, DT, ego_v); }; { // since the lateral acceleration and jerk is 0 when the velocity is 0, the target value is 0 diff --git a/evaluator/kinematic_evaluator/package.xml b/evaluator/kinematic_evaluator/package.xml index 7129221f6c27b..1b62b06e57c9f 100644 --- a/evaluator/kinematic_evaluator/package.xml +++ b/evaluator/kinematic_evaluator/package.xml @@ -17,8 +17,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_msgs eigen geometry_msgs diff --git a/evaluator/localization_evaluator/package.xml b/evaluator/localization_evaluator/package.xml index 075bc83a1f985..ac52f9fe94d5e 100644 --- a/evaluator/localization_evaluator/package.xml +++ b/evaluator/localization_evaluator/package.xml @@ -12,8 +12,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_msgs eigen geometry_msgs diff --git a/evaluator/perception_online_evaluator/README.md b/evaluator/perception_online_evaluator/README.md index 7ee93d2ffddf2..7df375ac236d0 100644 --- a/evaluator/perception_online_evaluator/README.md +++ b/evaluator/perception_online_evaluator/README.md @@ -155,11 +155,11 @@ where: ## Inputs / Outputs -| Name | Type | Description | -| ----------------- | ------------------------------------------------------ | ------------------------------------------------- | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | -| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | -| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | +| Name | Type | Description | +| ----------------- | ------------------------------------------------- | ------------------------------------------------- | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | The predicted objects to evaluate. | +| `~/metrics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information about perception accuracy. | +| `~/markers` | `visualization_msgs::msg::MarkerArray` | Visual markers for debugging and visualization. | ## Parameters diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp index 5715f22669fdc..010f1497da3da 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/detection_count.hpp @@ -21,8 +21,8 @@ #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include #include @@ -37,8 +37,8 @@ namespace perception_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; struct DetectionRange { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp index da7a23b6980b6..b480eaa3e67d6 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/deviation_metrics.hpp @@ -17,7 +17,7 @@ #include "perception_online_evaluator/stat.hpp" -#include +#include #include #include @@ -26,7 +26,7 @@ namespace perception_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; /** diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index 96b1c01ee2d16..ec435b0a5e17f 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -25,8 +25,8 @@ #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "geometry_msgs/msg/pose.hpp" #include @@ -41,8 +41,8 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using metrics::DetectionCounter; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index e5c41ff28c4da..daaea56178873 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -22,7 +22,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker_array.hpp" @@ -35,8 +35,8 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp index 0831d580248d3..be335a2d78fa3 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/marker_utils.hpp @@ -17,7 +17,7 @@ #include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include #include @@ -30,7 +30,7 @@ namespace marker_utils { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp index ee348b108d139..7adab46f42d2f 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/utils/objects_filtering.hpp @@ -17,9 +17,9 @@ #include "perception_online_evaluator/parameters.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -34,9 +34,9 @@ namespace perception_diagnostics { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using ClassObjectsMap = std::unordered_map; diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 95d6f07cca5d9..a408821466fb9 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -19,8 +19,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -35,11 +35,11 @@ #include using EvalNode = perception_diagnostics::PerceptionOnlineEvaluatorNode; -using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; -using PredictedObject = autoware_auto_perception_msgs::msg::PredictedObject; +using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; +using PredictedObject = autoware_perception_msgs::msg::PredictedObject; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; using MarkerArray = visualization_msgs::msg::MarkerArray; -using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification; +using ObjectClassification = autoware_perception_msgs::msg::ObjectClassification; using nav_msgs::msg::Odometry; using TFMessage = tf2_msgs::msg::TFMessage; @@ -172,7 +172,7 @@ class EvalTest : public ::testing::Test object.kinematics.initial_twist_with_covariance.twist.linear.y = 0.0; object.kinematics.initial_twist_with_covariance.twist.linear.z = 0.0; - autoware_auto_perception_msgs::msg::PredictedPath path; + autoware_perception_msgs::msg::PredictedPath path; for (size_t i = 0; i < predicted_path.size(); ++i) { geometry_msgs::msg::Pose pose; pose.position.x = predicted_path[i].first; diff --git a/evaluator/planning_evaluator/README.md b/evaluator/planning_evaluator/README.md index 23c8101429c35..eeeba99da9b8b 100644 --- a/evaluator/planning_evaluator/README.md +++ b/evaluator/planning_evaluator/README.md @@ -48,11 +48,11 @@ Adding a new metric `M` requires the following steps: ### Inputs -| Name | Type | Description | -| ------------------------------ | ------------------------------------------------------ | ------------------------------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | -| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Obstacles | +| Name | Type | Description | +| ------------------------------ | ------------------------------------------------- | ------------------------------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | +| `~/input/reference_trajectory` | `autoware_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Obstacles | ### Outputs diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp index 04a5b758d62e1..02464207801cb 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp @@ -17,15 +17,15 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp index 4806446d4270f..e3280187ccef7 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metrics_utils.hpp @@ -17,8 +17,8 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" namespace planning_diagnostics { @@ -26,7 +26,7 @@ namespace metrics { namespace utils { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; /** * @brief find the index in the trajectory at the given distance of the given index diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp index 848d92c91f13e..062ace8747253 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp @@ -17,15 +17,15 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; /** * @brief calculate the distance to the closest obstacle at each point of the trajectory diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp index c4bf1fe901604..e6ae9222d4bb9 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/stability_metrics.hpp @@ -17,13 +17,13 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; /** * @brief calculate the discrete Frechet distance between two trajectories diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp index b55ad245d8425..e88864016ca84 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/trajectory_metrics.hpp @@ -17,15 +17,15 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; /** * @brief calculate relative angle metric (angle between successive points) diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp index 851678e55d755..99ec5b787706b 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp @@ -18,20 +18,20 @@ #include "planning_evaluator/parameters.hpp" #include "planning_evaluator/stat.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include namespace planning_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp index 28a4b2cba8365..8e2561e3eaf0f 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/motion_evaluator_node.hpp @@ -21,8 +21,8 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include @@ -33,8 +33,8 @@ namespace planning_diagnostics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; /** * @brief Node for planning evaluation diff --git a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp index 888eea855295c..e02f833a67291 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp @@ -21,10 +21,10 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -36,14 +36,13 @@ namespace planning_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; - /** * @brief Node for planning evaluation */ diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml index fb3270fb0d89b..cf91d7077d609 100644 --- a/evaluator/planning_evaluator/package.xml +++ b/evaluator/planning_evaluator/package.xml @@ -13,8 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs autoware_planning_msgs diagnostic_msgs eigen diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp index e1ad9c1a2eeec..e403b8415938d 100644 --- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp @@ -22,8 +22,8 @@ namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; Stat calcLateralDeviation(const Trajectory & ref, const Trajectory & traj) { diff --git a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp index 6134a100f31a4..974f30223bff8 100644 --- a/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp +++ b/evaluator/planning_evaluator/src/metrics/metrics_utils.cpp @@ -20,8 +20,8 @@ namespace metrics { namespace utils { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::calcDistance2d; size_t getIndexAfterDistance(const Trajectory & traj, const size_t curr_id, const double distance) diff --git a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp index 82d6dcc51097e..af4508a370ab6 100644 --- a/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/obstacle_metrics.cpp @@ -18,7 +18,7 @@ #include -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -27,7 +27,7 @@ namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::calcDistance2d; Stat calcDistanceToObstacle(const PredictedObjects & obstacles, const Trajectory & traj) diff --git a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp index 7a3418da881c6..c963c5e46aa43 100644 --- a/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/stability_metrics.cpp @@ -19,7 +19,7 @@ #include -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include @@ -27,7 +27,7 @@ namespace planning_diagnostics { namespace metrics { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; Stat calcFrechetDistance(const Trajectory & traj1, const Trajectory & traj2) { diff --git a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp index 4f2ab014a79d6..e87b5c8ad35b7 100644 --- a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp @@ -20,8 +20,8 @@ #include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -33,9 +33,9 @@ #include using EvalNode = planning_diagnostics::PlanningEvaluatorNode; -using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using Objects = autoware_auto_perception_msgs::msg::PredictedObjects; +using Trajectory = autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoint = autoware_planning_msgs::msg::TrajectoryPoint; +using Objects = autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; using nav_msgs::msg::Odometry; @@ -409,7 +409,7 @@ TEST_F(EvalTest, TestObstacleDistance) { setTargetMetric(planning_diagnostics::Metric::obstacle_distance); Objects objs; - autoware_auto_perception_msgs::msg::PredictedObject obj; + autoware_perception_msgs::msg::PredictedObject obj; obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; objs.objects.push_back(obj); @@ -425,7 +425,7 @@ TEST_F(EvalTest, TestObstacleTTC) { setTargetMetric(planning_diagnostics::Metric::obstacle_ttc); Objects objs; - autoware_auto_perception_msgs::msg::PredictedObject obj; + autoware_perception_msgs::msg::PredictedObject obj; obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; objs.objects.push_back(obj); diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 132d1ec9be3ea..c58c0848eb5b3 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -22,7 +22,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml index 3be8678113728..21143aa0cb28a 100644 --- a/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml +++ b/launch/tier4_perception_launch/launch/traffic_light_recognition/traffic_light.launch.xml @@ -171,7 +171,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 29fe7f7071126..154a3019f1c17 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 @@ -136,7 +136,7 @@ /> - + @@ -221,7 +221,7 @@ - + diff --git a/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml b/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml index 0395496ae0b78..e2b90b2160a62 100755 --- a/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml +++ b/localization/autoware_pose_covariance_modifier/launch/pose_covariance_modifier.launch.xml @@ -5,7 +5,7 @@ - + diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index a832383ff4761..1b142b254d2e1 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp + src/diagnostics_module.cpp ) target_link_libraries(${PROJECT_NAME} fmt) diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md index e9e390010f084..aa6f2a96f4838 100644 --- a/localization/gyro_odometer/README.md +++ b/localization/gyro_odometer/README.md @@ -34,3 +34,18 @@ - [Limitation] The frequency of the output messages depends on the frequency of the input IMU message. - [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix. + +## Diagnostics + +drawing + +| Name | Description | Transition condition to Warning | Transition condition to Error | +| -------------------------------- | ----------------------------------------------------------------------------------------- | ------------------------------- | ------------------------------------------------- | +| `topic_time_stamp` | the time stamp of service calling. [nano second] | none | none | +| `is_arrived_first_vehicle_twist` | whether the vehicle twist topic has been received even once. | not arrive yet | none | +| `is_arrived_first_imu` | whether the imu topic has been received even once. | not arrive yet | none | +| `vehicle_twist_time_stamp_dt` | the time difference between the current time and the latest vehicle twist topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `imu_time_stamp_dt` | the time difference between the current time and the latest imu topic. [second] | none | the time is **longer** than `message_timeout_sec` | +| `vehicle_twist_queue_size` | the size of vehicle_twist_queue. | none | none | +| `imu_queue_size` | the size of gyro_queue. | none | none | +| `is_succeed_transform_imu` | whether transform imu is succeed or not. | none | failed | diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp new file mode 100644 index 0000000000000..49b881900b997 --- /dev/null +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ +#define GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +class DiagnosticsModule +{ +public: + DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); + void clear(); + void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + template + void addKeyValue(const std::string & key, const T & value); + void updateLevelAndMessage(const int8_t level, const std::string & message); + void publish(const rclcpp::Time & publish_time_stamp); + +private: + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const; + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Publisher::SharedPtr diagnostics_pub_; + + diagnostic_msgs::msg::DiagnosticStatus diagnostics_status_msg_; +}; + +template +void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = std::to_string(value); + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); + +} // namespace autoware::gyro_odometer + +#endif // GYRO_ODOMETER__DIAGNOSTICS_MODULE_HPP_ diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 2926589bd2da9..3b2da504f938a 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -15,6 +15,7 @@ #ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ #define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ +#include "gyro_odometer/diagnostics_module.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" @@ -52,6 +53,7 @@ class GyroOdometerNode : public rclcpp::Node void callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concatGyroAndOdometer(); void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr @@ -74,8 +76,12 @@ class GyroOdometerNode : public rclcpp::Node bool vehicle_twist_arrived_; bool imu_arrived_; + rclcpp::Time latest_vehicle_twist_ros_time_; + rclcpp::Time latest_imu_ros_time_; std::deque vehicle_twist_queue_; std::deque gyro_queue_; + + std::unique_ptr diagnostics_; }; } // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/media/diagnostic.png b/localization/gyro_odometer/media/diagnostic.png new file mode 100644 index 0000000000000..f2324f4079d0d Binary files /dev/null and b/localization/gyro_odometer/media/diagnostic.png differ diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index 3c979eb69ac8a..575f6a2d74837 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -17,6 +17,7 @@ ament_cmake_auto autoware_cmake + diagnostic_msgs fmt geometry_msgs rclcpp diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp new file mode 100644 index 0000000000000..9d88d8e6833ed --- /dev/null +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -0,0 +1,110 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "gyro_odometer/diagnostics_module.hpp" + +#include + +#include + +#include +#include + +namespace autoware::gyro_odometer +{ + +DiagnosticsModule::DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name) +: clock_(node->get_clock()) +{ + diagnostics_pub_ = + node->create_publisher("/diagnostics", 10); + + diagnostics_status_msg_.name = + std::string(node->get_name()) + std::string(": ") + diagnostic_name; + diagnostics_status_msg_.hardware_id = node->get_name(); +} + +void DiagnosticsModule::clear() +{ + diagnostics_status_msg_.values.clear(); + diagnostics_status_msg_.values.shrink_to_fit(); + + diagnostics_status_msg_.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diagnostics_status_msg_.message = ""; +} + +void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +{ + auto it = std::find_if( + std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), + [key_value_msg](const auto & arg) { return arg.key == key_value_msg.key; }); + + if (it != std::cend(diagnostics_status_msg_.values)) { + it->value = key_value_msg.value; + } else { + diagnostics_status_msg_.values.push_back(key_value_msg); + } +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value; + addKeyValue(key_value); +} + +template <> +void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +{ + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = key; + key_value.value = value ? "True" : "False"; + addKeyValue(key_value); +} + +void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +{ + if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!diagnostics_status_msg_.message.empty()) { + diagnostics_status_msg_.message += "; "; + } + diagnostics_status_msg_.message += message; + } + if (level > diagnostics_status_msg_.level) { + diagnostics_status_msg_.level = level; + } +} + +void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) +{ + diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); +} + +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( + const rclcpp::Time & publish_time_stamp) const +{ + diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; + diagnostics_msg.header.stamp = publish_time_stamp; + diagnostics_msg.status.push_back(diagnostics_status_msg_); + + if (diagnostics_msg.status.at(0).level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + diagnostics_msg.status.at(0).message = "OK"; + } + + return diagnostics_msg; +} + +} // namespace autoware::gyro_odometer diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index bda7af74b8489..1852d70bced71 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -25,11 +25,26 @@ #include #include +#include #include namespace autoware::gyro_odometer { +std::array transformCovariance(const std::array & cov) +{ + using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + + double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + + std::array cov_transformed; + cov_transformed.fill(0.); + cov_transformed[COV_IDX::X_X] = max_cov; + cov_transformed[COV_IDX::Y_Y] = max_cov; + cov_transformed[COV_IDX::Z_Z] = max_cov; + return cov_transformed; +} + GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) : Node("gyro_odometer", node_options), output_frame_(declare_parameter("output_frame")), @@ -56,6 +71,8 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) twist_with_covariance_pub_ = create_publisher( "twist_with_covariance", rclcpp::QoS{10}); + diagnostics_ = std::make_unique(this, "gyro_odometer_status"); + // TODO(YamatoAndo) createTimer } @@ -63,186 +80,195 @@ GyroOdometerNode::~GyroOdometerNode() { } -std::array transformCovariance(const std::array & cov) +void GyroOdometerNode::callbackVehicleTwist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { - using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + diagnostics_->clear(); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); - double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); + vehicle_twist_arrived_ = true; + latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + concatGyroAndOdometer(); - std::array cov_transformed; - cov_transformed.fill(0.); - cov_transformed[COV_IDX::X_X] = max_cov; - cov_transformed[COV_IDX::Y_Y] = max_cov; - cov_transformed[COV_IDX::Z_Z] = max_cov; - return cov_transformed; + diagnostics_->publish(vehicle_twist_ptr->header.stamp); } -geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( - const std::deque & vehicle_twist_queue, - const std::deque & gyro_queue) +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { - using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; - using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + diagnostics_->clear(); + diagnostics_->addKeyValue( + "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); - double vx_mean = 0; - geometry_msgs::msg::Vector3 gyro_mean{}; - double vx_covariance_original = 0; - geometry_msgs::msg::Vector3 gyro_covariance_original{}; - for (const auto & vehicle_twist : vehicle_twist_queue) { - vx_mean += vehicle_twist.twist.twist.linear.x; - vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; - } - vx_mean /= vehicle_twist_queue.size(); - vx_covariance_original /= vehicle_twist_queue.size(); - - for (const auto & gyro : gyro_queue) { - gyro_mean.x += gyro.angular_velocity.x; - gyro_mean.y += gyro.angular_velocity.y; - gyro_mean.z += gyro.angular_velocity.z; - gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; - gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; - gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; - } - gyro_mean.x /= gyro_queue.size(); - gyro_mean.y /= gyro_queue.size(); - gyro_mean.z /= gyro_queue.size(); - gyro_covariance_original.x /= gyro_queue.size(); - gyro_covariance_original.y /= gyro_queue.size(); - gyro_covariance_original.z /= gyro_queue.size(); - - geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; - const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue.back().header.stamp); - const auto latest_imu_stamp = rclcpp::Time(gyro_queue.back().header.stamp); - if (latest_vehicle_twist_stamp < latest_imu_stamp) { - twist_with_cov.header.stamp = latest_imu_stamp; - } else { - twist_with_cov.header.stamp = latest_vehicle_twist_stamp; - } - twist_with_cov.header.frame_id = gyro_queue.front().header.frame_id; - twist_with_cov.twist.twist.linear.x = vx_mean; - twist_with_cov.twist.twist.angular = gyro_mean; - - // From a statistical point of view, here we reduce the covariances according to the number of - // observed data - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue.size(); - twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue.size(); + imu_arrived_ = true; + latest_imu_ros_time_ = imu_msg_ptr->header.stamp; + gyro_queue_.push_back(*imu_msg_ptr); + concatGyroAndOdometer(); - return twist_with_cov; + diagnostics_->publish(imu_msg_ptr->header.stamp); } -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) +void GyroOdometerNode::concatGyroAndOdometer() { - vehicle_twist_arrived_ = true; - if (!imu_arrived_) { - RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Imu msg is not subscribed"); + // check arrive first topic + diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->addKeyValue("is_arrived_first_imu", imu_arrived_); + if (!vehicle_twist_arrived_) { + std::stringstream message; + message << "Twist msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (!imu_arrived_) { + std::stringstream message; + message << "Imu msg is not subscribed"; + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); - const double twist_dt = std::abs((this->now() - vehicle_twist_ptr->header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); + // check timeout + const double vehicle_twist_dt = + std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); + const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); + diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->addKeyValue("imu_time_stamp_dt", imu_dt); + if (vehicle_twist_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", + vehicle_twist_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - if (gyro_queue_.empty()) return; - const double imu_dt = std::abs((this->now() - gyro_queue_.back().header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } + if (imu_dt > message_timeout_sec_) { + const std::string message = fmt::format( + "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); + diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); -} - -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) -{ - imu_arrived_ = true; - if (!vehicle_twist_arrived_) { - RCLCPP_WARN_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Twist msg is not subscribed"); vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - const double imu_dt = std::abs((this->now() - imu_msg_ptr->header.stamp).seconds()); - if (imu_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); + // check queue size + diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + if (vehicle_twist_queue_.empty()) { + // not output error and clear queue + return; + } + if (gyro_queue_.empty()) { + // not output error and clear queue return; } + // get transformation geometry_msgs::msg::TransformStamped::ConstSharedPtr tf_imu2base_ptr = - transform_listener_->getLatestTransform(imu_msg_ptr->header.frame_id, output_frame_); - if (!tf_imu2base_ptr) { - RCLCPP_ERROR( - this->get_logger(), "Please publish TF %s to %s", output_frame_.c_str(), - (imu_msg_ptr->header.frame_id).c_str()); + transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); + + const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); + diagnostics_->addKeyValue("is_succeed_transform_imu", is_succeed_transform_imu); + if (!is_succeed_transform_imu) { + std::stringstream message; + message << "Please publish TF " << output_frame_ << " to " + << gyro_queue_.front().header.frame_id; + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); + diagnostics_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); + vehicle_twist_queue_.clear(); gyro_queue_.clear(); return; } - geometry_msgs::msg::Vector3Stamped angular_velocity; - angular_velocity.header = imu_msg_ptr->header; - angular_velocity.vector = imu_msg_ptr->angular_velocity; - - geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; - transformed_angular_velocity.header = tf_imu2base_ptr->header; - tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); - - sensor_msgs::msg::Imu gyro_base_link; - gyro_base_link.header = imu_msg_ptr->header; - gyro_base_link.header.frame_id = output_frame_; - gyro_base_link.angular_velocity = transformed_angular_velocity.vector; - gyro_base_link.angular_velocity_covariance = - transformCovariance(imu_msg_ptr->angular_velocity_covariance); - - gyro_queue_.push_back(gyro_base_link); - - if (vehicle_twist_queue_.empty()) return; - const double twist_dt = - std::abs((this->now() - vehicle_twist_queue_.back().header.stamp).seconds()); - if (twist_dt > message_timeout_sec_) { - const std::string error_msg = fmt::format( - "Twist msg is timeout. twist_dt: {}[sec], tolerance {}[sec]", twist_dt, message_timeout_sec_); - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, error_msg.c_str()); - vehicle_twist_queue_.clear(); - gyro_queue_.clear(); - return; + // transform gyro frame + for (auto & gyro : gyro_queue_) { + geometry_msgs::msg::Vector3Stamped angular_velocity; + angular_velocity.header = gyro.header; + angular_velocity.vector = gyro.angular_velocity; + + geometry_msgs::msg::Vector3Stamped transformed_angular_velocity; + transformed_angular_velocity.header = tf_imu2base_ptr->header; + tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_imu2base_ptr); + + gyro.header.frame_id = output_frame_; + gyro.angular_velocity = transformed_angular_velocity.vector; + gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance); + } + + using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + using COV_IDX_XYZRPY = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + + // calc mean, covariance + double vx_mean = 0; + geometry_msgs::msg::Vector3 gyro_mean{}; + double vx_covariance_original = 0; + geometry_msgs::msg::Vector3 gyro_covariance_original{}; + for (const auto & vehicle_twist : vehicle_twist_queue_) { + vx_mean += vehicle_twist.twist.twist.linear.x; + vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } + vx_mean /= vehicle_twist_queue_.size(); + vx_covariance_original /= vehicle_twist_queue_.size(); + + for (const auto & gyro : gyro_queue_) { + gyro_mean.x += gyro.angular_velocity.x; + gyro_mean.y += gyro.angular_velocity.y; + gyro_mean.z += gyro.angular_velocity.z; + gyro_covariance_original.x += gyro.angular_velocity_covariance[COV_IDX_XYZ::X_X]; + gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; + gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; + } + gyro_mean.x /= gyro_queue_.size(); + gyro_mean.y /= gyro_queue_.size(); + gyro_mean.z /= gyro_queue_.size(); + gyro_covariance_original.x /= gyro_queue_.size(); + gyro_covariance_original.y /= gyro_queue_.size(); + gyro_covariance_original.z /= gyro_queue_.size(); + + // concat + geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; + const auto latest_vehicle_twist_stamp = rclcpp::Time(vehicle_twist_queue_.back().header.stamp); + const auto latest_imu_stamp = rclcpp::Time(gyro_queue_.back().header.stamp); + if (latest_vehicle_twist_stamp < latest_imu_stamp) { + twist_with_cov.header.stamp = latest_imu_stamp; + } else { + twist_with_cov.header.stamp = latest_vehicle_twist_stamp; + } + twist_with_cov.header.frame_id = gyro_queue_.front().header.frame_id; + twist_with_cov.twist.twist.linear.x = vx_mean; + twist_with_cov.twist.twist.angular = gyro_mean; + + // From a statistical point of view, here we reduce the covariances according to the number of + // observed data + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = + vx_covariance_original / vehicle_twist_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = + gyro_covariance_original.x / gyro_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = + gyro_covariance_original.y / gyro_queue_.size(); + twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = + gyro_covariance_original.z / gyro_queue_.size(); + + publishData(twist_with_cov); - const geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov_raw = - concatGyroAndOdometer(vehicle_twist_queue_, gyro_queue_); - publishData(twist_with_cov_raw); vehicle_twist_queue_.clear(); gyro_queue_.clear(); } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md index 1a776bd810fff..37596ee9820b9 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -15,7 +15,7 @@ The positions and orientations of the AR-Tags are assumed to be written in the L | Name | Type | Description | | :--------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | -| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | +| `~/input/lanelet2_map` | `autoware_map_msgs::msg::LaneletMapBin` | Data of lanelet2 | | `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | | `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | | `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index 9a6823e330acd..0ea40690d9f4d 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -111,7 +111,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) Subscribers */ using std::placeholders::_1; - map_bin_sub_ = this->create_subscription( + map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), std::bind(&ArTagBasedLocalizer::map_bin_callback, this, _1)); @@ -140,7 +140,7 @@ ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); } -void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) +void ArTagBasedLocalizer::map_bin_callback(const LaneletMapBin::ConstSharedPtr & msg) { landmark_manager_.parse_landmarks(msg, "apriltag_16h5"); const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index f70821a39ffe8..f00621b3c520a 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -70,7 +70,7 @@ class ArTagBasedLocalizer : public rclcpp::Node { - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Image = sensor_msgs::msg::Image; using CameraInfo = sensor_msgs::msg::CameraInfo; using Pose = geometry_msgs::msg::Pose; @@ -86,7 +86,7 @@ class ArTagBasedLocalizer : public rclcpp::Node explicit ArTagBasedLocalizer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: - void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); + void map_bin_callback(const LaneletMapBin::ConstSharedPtr & msg); void image_callback(const Image::ConstSharedPtr & msg); void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); @@ -106,7 +106,7 @@ class ArTagBasedLocalizer : public rclcpp::Node std::unique_ptr tf_listener_; // Subscribers - rclcpp::Subscription::SharedPtr map_bin_sub_; + rclcpp::Subscription::SharedPtr map_bin_sub_; rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; rclcpp::Subscription::SharedPtr ekf_pose_sub_; diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index c2b0751d91f9a..7c1a3f656dd66 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -19,7 +19,7 @@ #include -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" #include #include #include @@ -41,7 +41,7 @@ class LandmarkManager { public: void parse_landmarks( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg, const std::string & target_subtype); [[nodiscard]] std::vector get_landmarks() const; diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml index be19de9334e84..4e080c0362c07 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -18,7 +18,7 @@ eigen - autoware_auto_mapping_msgs + autoware_map_msgs geometry_msgs lanelet2_extension localization_util diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index e977dbf6cf74a..a808a6428f682 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -28,7 +28,7 @@ namespace landmark_manager { void LandmarkManager::parse_landmarks( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr & msg, const std::string & target_subtype) { std::vector landmarks = diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index a44db7bbaa4bf..8049104e8f2f5 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -262,23 +262,27 @@ initial_pose_offset_model_x & initial_pose_offset_model_y must have the same num drawing -| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | -| ----------------------------------------- | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | -| `topic_time_stamp` | the time stamp of input topic | none | none | no | -| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | -| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | -| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | -| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | -| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | -| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | -| `is_set_map_points` | whether the map points is set or not | not set | none | yes | -| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | -| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | -| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | -| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | -| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | -| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | -| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | +| Name | Description | Transition condition to Warning | Transition condition to Error | Whether to reject the estimation result (affects `skipping_publish_num`) | +| ------------------------------------------------ | -------------------------------------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ----------------------------- | --------------------------------------------------------------------------------------------------- | +| `topic_time_stamp` | the time stamp of input topic | none | none | no | +| `sensor_points_size` | the size of sensor points | the size is 0 | none | yes | +| `sensor_points_delay_time_sec` | the delay time of sensor points | the time is **longer** than `validation.lidar_topic_timeout_sec` | none | yes | +| `is_succeed_transform_sensor_points` | whether transform sensor points is succeed or not | none | failed | yes | +| `sensor_points_max_distance` | the max distance of sensor points | the max distance is **shorter** than `sensor_points.required_distance` | none | yes | +| `is_activated` | whether the node is in the "activate" state or not | not "activate" state | none | if `is_activated` is false, then estimation is not executed and `skipping_publish_num` is set to 0. | +| `is_succeed_interpolate_initial_pose` | whether the interpolate of initial pose is succeed or not | failed.
(1) the size of `initial_pose_buffer_` is **smaller** than 2.
(2) the timestamp difference between initial_pose and sensor pointcloud is **longer** than `validation.initial_pose_timeout_sec`.
(3) distance difference between two initial poses used for linear interpolation is **longer** than `validation.initial_pose_distance_tolerance_m` | none | yes | +| `is_set_map_points` | whether the map points is set or not | not set | none | yes | +| `iteration_num` | the number of times calculate alignment | the number of times is **larger** than `ndt.max_iterations` | none | yes | +| `local_optimal_solution_oscillation_num` | the number of times the solution is judged to be oscillating | the number of times is **larger** than 10 | none | yes | +| `transform_probability` | the score of how well the map aligns with the sensor points | the score is **smaller** than`score_estimation.converged_param_transform_probability` (only in the case of `score_estimation.converged_param_type` is 0=TRANSFORM_PROBABILITY) | none | yes | +| `transform_probability_diff` | the tp score difference for the current ndt optimization | none | none | no | +| `transform_probability_before` | the tp score before the current ndt optimization | none | none | no | +| `nearest_voxel_transformation_likelihood` | the score of how well the map aligns with the sensor points | the score is **smaller** than `score_estimation.converged_param_nearest_voxel_transformation_likelihood` (only in the case of `score_estimation.converged_param_type` is 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) | none | yes | +| `nearest_voxel_transformation_likelihood_diff` | the nvtl score difference for the current ndt optimization | none | none | no | +| `nearest_voxel_transformation_likelihood_before` | the nvtl score before the current ndt optimization | none | none | no | +| `distance_initial_to_result` | the distance between the position before convergence processing and the position after | the distance is **longer** than 3 | none | no | +| `execution_time` | the time for convergence processing | the time is **longer** than `validation.critical_upper_bound_exe_time_ms` | none | no | +| `skipping_publish_num` | the number of times rejected estimation results consecutively | the number of times is 5 or more | none | - | ※The `sensor_points_callback` shares the same callback group as the `trigger_node_service` and `ndt_align_service`. Consequently, if the initial pose estimation takes too long, this diagnostic may become stale. diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 8acfe3bd5c1ca..349f924019c28 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -489,6 +489,38 @@ bool NDTScanMatcher::callback_sensor_points_main( return false; } + // check score diff + const std::vector & tp_array = ndt_result.transform_probability_array; + if (static_cast(tp_array.size()) != ndt_result.iteration_num + 1) { + // only publish warning to /diagnostics, not skip publishing pose + std::stringstream message; + message << "transform_probability_array size is not equal to iteration_num + 1." + << " transform_probability_array size: " << tp_array.size() + << ", iteration_num: " << ndt_result.iteration_num; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } else { + const float diff = tp_array.back() - tp_array.front(); + diagnostics_scan_points_->addKeyValue("transform_probability_diff", diff); + diagnostics_scan_points_->addKeyValue("transform_probability_before", tp_array.front()); + } + const std::vector & nvtl_array = ndt_result.nearest_voxel_transformation_likelihood_array; + if (static_cast(nvtl_array.size()) != ndt_result.iteration_num + 1) { + // only publish warning to /diagnostics, not skip publishing pose + std::stringstream message; + message + << "nearest_voxel_transformation_likelihood_array size is not equal to iteration_num + 1." + << " nearest_voxel_transformation_likelihood_array size: " << nvtl_array.size() + << ", iteration_num: " << ndt_result.iteration_num; + diagnostics_scan_points_->updateLevelAndMessage( + diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); + } else { + const float diff = nvtl_array.back() - nvtl_array.front(); + diagnostics_scan_points_->addKeyValue("nearest_voxel_transformation_likelihood_diff", diff); + diagnostics_scan_points_->addKeyValue( + "nearest_voxel_transformation_likelihood_before", nvtl_array.front()); + } + bool is_ok_score = (score > score_threshold); if (!is_ok_score) { std::stringstream message; diff --git a/localization/pose_estimator_arbiter/README.md b/localization/pose_estimator_arbiter/README.md index 7e1ad2b9ec6eb..a28da699ad926 100644 --- a/localization/pose_estimator_arbiter/README.md +++ b/localization/pose_estimator_arbiter/README.md @@ -93,7 +93,7 @@ For switching rule: | Name | Type | Description | | ----------------------------- | ------------------------------------------------------------ | --------------------------------- | -| `/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | | `/input/pose_with_covariance` | geometry_msgs::msg::PoseWithCovarianceStamped | localization final output | | `/input/initialization_state` | autoware_adapi_v1_msgs::msg::LocalizationInitializationState | localization initialization state | diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp index d6901f9be2dbf..392219b2281e1 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -29,7 +29,7 @@ namespace pose_estimator_arbiter::rule_helper { class PoseEstimatorArea { - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp index 094434c62ac9c..675842662acf8 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp @@ -27,7 +27,7 @@ VectorMapBasedRule::VectorMapBasedRule( // Register callback shared_data_->vector_map.register_callback( - [this](autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) -> void { + [this](autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) -> void { pose_estimator_area_->init(msg); }); diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp index 35ed8b04bfcad..beda4d720b7f6 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -18,7 +18,7 @@ #include -#include +#include #include @@ -59,7 +59,7 @@ TEST_F(MockNode, poseEstimatorArea) lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet_map->add(create_polygon3d()); - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Point = geometry_msgs::msg::Point; HADMapBin msg; lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp index a0a983e2ad3b7..66d867883272b 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -16,7 +16,7 @@ #include -#include +#include #include #include @@ -71,7 +71,7 @@ TEST_F(MockNode, vectorMapBasedRule) lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet_map->add(create_polygon3d()); - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; HADMapBin msg; lanelet::utils::conversion::toBinMsg(lanelet_map, &msg); diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index d164086ada87e..70896d93d9cb8 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/pose_estimator_arbiter/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_mapping_msgs + autoware_map_msgs diagnostic_msgs geometry_msgs lanelet2_extension diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp index 54dac6ac254c1..fda77ec877482 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_arbiter.hpp @@ -42,7 +42,7 @@ class PoseEstimatorArbiter : public rclcpp::Node using Image = sensor_msgs::msg::Image; using PointCloud2 = sensor_msgs::msg::PointCloud2; using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp index b9c3bd45c9e24..a72dc5585877b 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp @@ -16,7 +16,7 @@ #define POSE_ESTIMATOR_ARBITER__SHARED_DATA_HPP_ #include -#include +#include #include #include #include @@ -77,7 +77,7 @@ struct SharedData using Image = sensor_msgs::msg::Image; using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using PointCloud2 = sensor_msgs::msg::PointCloud2; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; SharedData() {} diff --git a/localization/yabloc/yabloc_common/README.md b/localization/yabloc/yabloc_common/README.md index 6368305bdbfad..cb1799ce21e61 100644 --- a/localization/yabloc/yabloc_common/README.md +++ b/localization/yabloc/yabloc_common/README.md @@ -15,10 +15,10 @@ It estimates the height and tilt of the ground from lanelet2. #### Input -| Name | Type | Description | -| ------------------ | -------------------------------------------- | ------------------- | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | -| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose | +| Name | Type | Description | +| ------------------ | --------------------------------------- | ------------------- | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | +| `input/pose` | `geometry_msgs::msg::PoseStamped` | estimated self pose | #### Output @@ -44,9 +44,9 @@ This node extracts the elements related to the road surface markings and yabloc #### Input -| Name | Type | Description | -| ------------------ | -------------------------------------------- | ----------- | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| ------------------ | --------------------------------------- | ----------- | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | #### Output diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp index 4cc4128ad5a70..92e5d939d0b7e 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ground_server/ground_server.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ class GroundServer : public rclcpp::Node { public: using GroundPlane = common::GroundPlane; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; @@ -65,7 +65,7 @@ class GroundServer : public rclcpp::Node const int K; // Subscriber - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_pose_stamped_; rclcpp::Subscription::SharedPtr sub_initial_pose_; // Publisher @@ -86,7 +86,7 @@ class GroundServer : public rclcpp::Node std::vector last_indices_; // Callback - void on_map(const HADMapBin & msg); + void on_map(const LaneletMapBin & msg); void on_initial_pose(const PoseCovStamped & msg); void on_pose_stamped(const PoseStamped & msg); diff --git a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp index 784b3997faffd..b044ae985c6c0 100644 --- a/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp +++ b/localization/yabloc/yabloc_common/include/yabloc_common/ll2_decomposer/ll2_decomposer.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -35,7 +35,7 @@ class Ll2Decomposer : public rclcpp::Node { public: using Cloud2 = sensor_msgs::msg::PointCloud2; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using Marker = visualization_msgs::msg::Marker; using MarkerArray = visualization_msgs::msg::MarkerArray; @@ -47,12 +47,12 @@ class Ll2Decomposer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_bounding_box_; rclcpp::Publisher::SharedPtr pub_marker_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; std::set road_marking_labels_; std::set sign_board_labels_; std::set bounding_box_labels_; - void on_map(const HADMapBin & msg); + void on_map(const LaneletMapBin & msg); pcl::PointNormal to_point_normal( const lanelet::ConstPoint3d & from, const lanelet::ConstPoint3d & to) const; diff --git a/localization/yabloc/yabloc_common/package.xml b/localization/yabloc/yabloc_common/package.xml index caf044660257e..de675da5cc2d4 100644 --- a/localization/yabloc/yabloc_common/package.xml +++ b/localization/yabloc/yabloc_common/package.xml @@ -16,7 +16,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_mapping_msgs + autoware_map_msgs cv_bridge geometry_msgs lanelet2_core diff --git a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp index 75d5f8e1f9cbc..d2d1929991442 100644 --- a/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp +++ b/localization/yabloc/yabloc_common/src/ground_server/ground_server_core.cpp @@ -43,7 +43,7 @@ GroundServer::GroundServer(const rclcpp::NodeOptions & options) auto on_pose = std::bind(&GroundServer::on_pose_stamped, this, _1); auto on_map = std::bind(&GroundServer::on_map, this, _1); - sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); sub_pose_stamped_ = create_subscription("~/input/pose", 10, on_pose); pub_ground_height_ = create_publisher("~/output/height", 10); @@ -100,7 +100,7 @@ void GroundServer::on_pose_stamped(const PoseStamped & msg) } } -void GroundServer::on_map(const HADMapBin & msg) +void GroundServer::on_map(const LaneletMapBin & msg) { lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); 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 df56f43b9287c..d0dfc87067f7f 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 @@ -38,7 +38,7 @@ Ll2Decomposer::Ll2Decomposer(const rclcpp::NodeOptions & options) : Node("ll2_to // Subscriber auto cb_map = std::bind(&Ll2Decomposer::on_map, this, _1); - sub_map_ = create_subscription("~/input/vector_map", map_qos, cb_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, cb_map); auto load_lanelet2_labels = [this](const std::string & param_name, std::set & labels) -> void { @@ -102,7 +102,7 @@ pcl::PointCloud Ll2Decomposer::load_bounding_boxes( return cloud; } -void Ll2Decomposer::on_map(const HADMapBin & msg) +void Ll2Decomposer::on_map(const LaneletMapBin & msg) { RCLCPP_INFO_STREAM(get_logger(), "subscribed binary vector map"); lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); diff --git a/localization/yabloc/yabloc_image_processing/CMakeLists.txt b/localization/yabloc/yabloc_image_processing/CMakeLists.txt index 58437c085c4b2..2af75ab237585 100644 --- a/localization/yabloc/yabloc_image_processing/CMakeLists.txt +++ b/localization/yabloc/yabloc_image_processing/CMakeLists.txt @@ -14,56 +14,55 @@ find_package(OpenCV REQUIRED) # PCL find_package(PCL REQUIRED COMPONENTS common) +ament_auto_add_library(${PROJECT_NAME} SHARED + src/line_segment_detector/line_segment_detector_core.cpp + src/graph_segment/graph_segment_core.cpp + src/graph_segment/similar_area_searcher.cpp + src/segment_filter/segment_filter_core.cpp + src/undistort/undistort_node.cpp + src/line_segments_overlay/line_segments_overlay_core.cpp + src/lanelet2_overlay/lanelet2_overlay_core.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ${OpenCV_LIBS}) + # =================================================== # Executable -# line segment detector -set(TARGET line_segment_detector_node) -ament_auto_add_executable(${TARGET} - src/line_segment_detector/line_segment_detector_node.cpp - src/line_segment_detector/line_segment_detector_core.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::graph_segment::GraphSegment" + EXECUTABLE yabloc_graph_segment_node + EXECUTOR SingleThreadedExecutor +) -# graph based segmentation -set(TARGET graph_segment_node) -ament_auto_add_executable(${TARGET} - src/graph_segment/graph_segment_node.cpp - src/graph_segment/graph_segment_core.cpp - src/graph_segment/similar_area_searcher.cpp) -target_include_directories(${TARGET} PUBLIC include) -target_include_directories(${TARGET} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::lanelet2_overlay::Lanelet2Overlay" + EXECUTABLE yabloc_lanelet2_overlay_node + EXECUTOR SingleThreadedExecutor +) -# segment filter -set(TARGET segment_filter_node) -ament_auto_add_executable(${TARGET} - src/segment_filter/segment_filter_node.cpp - src/segment_filter/segment_filter_core.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segment_detector::LineSegmentDetector" + EXECUTABLE yabloc_line_segment_detector_node + EXECUTOR SingleThreadedExecutor +) -# undistort -set(TARGET undistort_node) -ament_auto_add_executable(${TARGET} - src/undistort/undistort_node.cpp) -target_link_libraries(${TARGET} ${OpenCV_LIBS}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::line_segments_overlay::LineSegmentsOverlay" + EXECUTABLE yabloc_line_segments_overlay_node + EXECUTOR SingleThreadedExecutor +) -# line_segments_overlay -set(TARGET line_segments_overlay_node) -ament_auto_add_executable(${TARGET} - src/line_segments_overlay/line_segments_overlay_core.cpp - src/line_segments_overlay/line_segments_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::segment_filter::SegmentFilter" + EXECUTABLE yabloc_segment_filter_node + EXECUTOR SingleThreadedExecutor +) -# lanelet2_overlay -set(TARGET lanelet2_overlay_node) -ament_auto_add_executable(${TARGET} - src/lanelet2_overlay/lanelet2_overlay_core.cpp - src/lanelet2_overlay/lanelet2_overlay_node.cpp) -target_include_directories(${TARGET} PUBLIC include ${EIGEN_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) -target_link_libraries(${TARGET} ${PCL_LIBRARIES}) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "yabloc::undistort::UndistortNode" + EXECUTABLE yabloc_undistort_node + EXECUTOR SingleThreadedExecutor +) # =================================================== ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp index a1e9c90eaa362..6c2e670a7a72e 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/graph_segment/graph_segment.hpp @@ -32,7 +32,7 @@ class GraphSegment : public rclcpp::Node public: using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - GraphSegment(); + explicit GraphSegment(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: const float target_height_ratio_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp index 468e765b74175..7d644376ba591 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp @@ -47,7 +47,7 @@ class Lanelet2Overlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using Float32Array = std_msgs::msg::Float32MultiArray; - Lanelet2Overlay(); + explicit Lanelet2Overlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: common::StaticTfSubscriber tf_subscriber_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp index 74ef82dd94f59..761c581200369 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segment_detector/line_segment_detector.hpp @@ -42,7 +42,7 @@ class LineSegmentDetector : public rclcpp::Node using Image = sensor_msgs::msg::Image; using PointCloud2 = sensor_msgs::msg::PointCloud2; - LineSegmentDetector(); + explicit LineSegmentDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: rclcpp::Subscription::SharedPtr sub_image_; diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp index abbc2f75725ed..00f01be5984e5 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp @@ -34,7 +34,7 @@ class LineSegmentsOverlay : public rclcpp::Node using Image = sensor_msgs::msg::Image; using LineSegment = pcl::PointXYZLNormal; using LineSegments = pcl::PointCloud; - LineSegmentsOverlay(); + explicit LineSegmentsOverlay(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: void on_image(const Image::ConstSharedPtr & img_msg); diff --git a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp index 766bb77d4da85..c8f036f2b12de 100644 --- a/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp +++ b/localization/yabloc/yabloc_image_processing/include/yabloc_image_processing/segment_filter/segment_filter.hpp @@ -39,7 +39,7 @@ class SegmentFilter : public rclcpp::Node using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - SegmentFilter(); + explicit SegmentFilter(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); private: using ProjectFunc = std::function(const Eigen::Vector3f &)>; diff --git a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml index a0cad16302c2b..214772751f931 100644 --- a/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/image_processing.launch.xml @@ -5,7 +5,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -27,7 +27,7 @@ - + @@ -40,7 +40,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml index 150ffed58138d..134f3ee765476 100644 --- a/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml +++ b/localization/yabloc/yabloc_image_processing/launch/overlay.launch.xml @@ -10,7 +10,7 @@ - + @@ -23,7 +23,7 @@ - + diff --git a/localization/yabloc/yabloc_image_processing/package.xml b/localization/yabloc/yabloc_image_processing/package.xml index 209f09fdaa7ac..416acfdc76a16 100644 --- a/localization/yabloc/yabloc_image_processing/package.xml +++ b/localization/yabloc/yabloc_image_processing/package.xml @@ -19,6 +19,7 @@ cv_bridge pcl_conversions rclcpp + rclcpp_components sensor_msgs std_msgs tier4_autoware_utils diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp index 75e14b5a4cd4b..f8d4c74dd5bf8 100644 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::graph_segment { -GraphSegment::GraphSegment() -: Node("graph_segment"), +GraphSegment::GraphSegment(const rclcpp::NodeOptions & options) +: Node("graph_segment", options), target_height_ratio_(declare_parameter("target_height_ratio")), target_candidate_box_width_(declare_parameter("target_candidate_box_width")) { @@ -159,3 +159,6 @@ void GraphSegment::draw_and_publish_image( } } // namespace yabloc::graph_segment + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::graph_segment::GraphSegment) diff --git a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp b/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp deleted file mode 100644 index d11701e115eff..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/graph_segment/graph_segment_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/graph_segment/graph_segment.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp index 367ff51567147..107d861364038 100644 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_core.cpp @@ -28,8 +28,8 @@ namespace yabloc::lanelet2_overlay { -Lanelet2Overlay::Lanelet2Overlay() -: Node("lanelet2_overlay"), tf_subscriber_(get_clock()), pose_buffer_{40} +Lanelet2Overlay::Lanelet2Overlay(const rclcpp::NodeOptions & options) +: Node("lanelet2_overlay", options), tf_subscriber_(get_clock()), pose_buffer_{40} { using std::placeholders::_1; @@ -211,3 +211,6 @@ void Lanelet2Overlay::make_vis_marker( } } // namespace yabloc::lanelet2_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::lanelet2_overlay::Lanelet2Overlay) diff --git a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp deleted file mode 100644 index 941ac9d84ab16..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/lanelet2_overlay/lanelet2_overlay_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/lanelet2_overlay/lanelet2_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp index 57f2302fd5c5a..c613642628499 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_core.cpp @@ -23,7 +23,8 @@ namespace yabloc::line_segment_detector { -LineSegmentDetector::LineSegmentDetector() : Node("line_detector") +LineSegmentDetector::LineSegmentDetector(const rclcpp::NodeOptions & options) +: Node("line_detector", options) { using std::placeholders::_1; @@ -106,3 +107,6 @@ std::vector LineSegmentDetector::remove_too_outer_elements( } } // namespace yabloc::line_segment_detector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segment_detector::LineSegmentDetector) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp deleted file mode 100644 index 42965ae853ea7..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segment_detector/line_segment_detector_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/line_segment_detector/line_segment_detector.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp index 0ee4115a39760..7e06de81fbd18 100644 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::line_segments_overlay { -LineSegmentsOverlay::LineSegmentsOverlay() -: Node("overlay_lanelet2"), +LineSegmentsOverlay::LineSegmentsOverlay(const rclcpp::NodeOptions & options) +: Node("line_segments_overlay", options), max_buffer_size_(static_cast(declare_parameter("max_buffer_size", 5))) { using std::placeholders::_1; @@ -90,3 +90,6 @@ void LineSegmentsOverlay::on_line_segments(const PointCloud2::ConstSharedPtr & l } } // namespace yabloc::line_segments_overlay + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::line_segments_overlay::LineSegmentsOverlay) diff --git a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp b/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp deleted file mode 100644 index cac6a6a0c5a66..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/line_segments_overlay/line_segments_overlay_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/line_segments_overlay/line_segments_overlay.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp index cb1505903b210..df0aa7d65c617 100644 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp +++ b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_core.cpp @@ -23,8 +23,8 @@ namespace yabloc::segment_filter { -SegmentFilter::SegmentFilter() -: Node("segment_filter"), +SegmentFilter::SegmentFilter(const rclcpp::NodeOptions & options) +: Node("segment_filter", options), image_size_(declare_parameter("image_size")), max_range_(declare_parameter("max_range")), min_segment_length_(declare_parameter("min_segment_length")), @@ -282,3 +282,6 @@ std::set SegmentFilter::filter_by_mask( } } // namespace yabloc::segment_filter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::segment_filter::SegmentFilter) diff --git a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp b/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp deleted file mode 100644 index ea51babceee60..0000000000000 --- a/localization/yabloc/yabloc_image_processing/src/segment_filter/segment_filter_node.cpp +++ /dev/null @@ -1,23 +0,0 @@ -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "yabloc_image_processing/segment_filter/segment_filter.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp index 7fc9ad785dbe2..0714b6c8091c8 100644 --- a/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp +++ b/localization/yabloc/yabloc_image_processing/src/undistort/undistort_node.cpp @@ -37,8 +37,8 @@ class UndistortNode : public rclcpp::Node using CameraInfo = sensor_msgs::msg::CameraInfo; using Image = sensor_msgs::msg::Image; - UndistortNode() - : Node("undistort"), + explicit UndistortNode(const rclcpp::NodeOptions & options) + : Node("undistort", options), OUTPUT_WIDTH(declare_parameter("width")), OVERRIDE_FRAME_ID(declare_parameter("override_frame_id")) { @@ -166,10 +166,5 @@ class UndistortNode : public rclcpp::Node }; } // namespace yabloc::undistort -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(yabloc::undistort::UndistortNode) diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 33b9788cca3d3..9ab5dd570510d 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -45,11 +45,11 @@ Converted model URL #### Input -| Name | Type | Description | -| ------------------- | -------------------------------------------- | ------------------------ | -| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | -| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | -| `input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| ------------------- | --------------------------------------- | ------------------------ | +| `input/camera_info` | `sensor_msgs::msg::CameraInfo` | undistorted camera info | +| `input/image_raw` | `sensor_msgs::msg::Image` | undistorted camera image | +| `input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | #### Output diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp index 917a6ecaaf291..979d1b370699e 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include @@ -40,7 +40,7 @@ class CameraPoseInitializer : public rclcpp::Node using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using PointCloud2 = sensor_msgs::msg::PointCloud2; using Image = sensor_msgs::msg::Image; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; CameraPoseInitializer(); @@ -52,7 +52,7 @@ class CameraPoseInitializer : public rclcpp::Node std::unique_ptr projector_module_{nullptr}; rclcpp::Subscription::SharedPtr sub_initialpose_; - rclcpp::Subscription::SharedPtr sub_map_; + rclcpp::Subscription::SharedPtr sub_map_; rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Service::SharedPtr align_server_; @@ -63,7 +63,7 @@ class CameraPoseInitializer : public rclcpp::Node std::unique_ptr semantic_segmentation_{nullptr}; - void on_map(const HADMapBin & msg); + void on_map(const LaneletMapBin & msg); void on_service( const RequestPoseAlignment::Request::SharedPtr, RequestPoseAlignment::Response::SharedPtr request); diff --git a/localization/yabloc/yabloc_pose_initializer/package.xml b/localization/yabloc/yabloc_pose_initializer/package.xml index afd0d4aa86bcf..7ed16c9a8b82d 100644 --- a/localization/yabloc/yabloc_pose_initializer/package.xml +++ b/localization/yabloc/yabloc_pose_initializer/package.xml @@ -16,7 +16,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_mapping_msgs + autoware_map_msgs cv_bridge geometry_msgs lanelet2_extension diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index 19d3c8d88f260..083e7dd5bcd43 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -40,7 +40,7 @@ CameraPoseInitializer::CameraPoseInitializer() // Subscriber auto on_map = std::bind(&CameraPoseInitializer::on_map, this, _1); auto on_image = [this](Image::ConstSharedPtr msg) -> void { latest_image_msg_ = msg; }; - sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); + sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); sub_image_ = create_subscription("~/input/image_raw", 10, on_image); // Server @@ -164,7 +164,7 @@ std::optional CameraPoseInitializer::estimate_pose( return angles_rad.at(max_index); } -void CameraPoseInitializer::on_map(const HADMapBin & msg) +void CameraPoseInitializer::on_map(const LaneletMapBin & msg) { lanelet::LaneletMapPtr lanelet_map(new lanelet::LaneletMap); lanelet::utils::conversion::fromBinMsg(msg, lanelet_map); diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml index ea0157620ce43..5baee911d6572 100644 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -4,16 +4,16 @@ - + - + - + diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 3e447456bb623..3633a8fa39a6b 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp index 446417f060b9b..26bcfd5ed802a 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/segmentation_pointcloud_fusion/node.hpp @@ -50,6 +50,13 @@ class SegmentPointCloudFusionNode : public FusionNode iter_x(transformed_cloud, "x"), - iter_y(transformed_cloud, "y"), iter_z(transformed_cloud, "z"), - iter_orig_x(input_pointcloud_msg, "x"), iter_orig_y(input_pointcloud_msg, "y"), - iter_orig_z(input_pointcloud_msg, "z"); - iter_x != iter_x.end(); - ++iter_x, ++iter_y, ++iter_z, ++iter_orig_x, ++iter_orig_y, ++iter_orig_z) { + int point_step = input_pointcloud_msg.point_step; + int x_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "x")].offset; + int y_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "y")].offset; + int z_offset = input_pointcloud_msg.fields[pcl::getFieldIndex(input_pointcloud_msg, "z")].offset; + size_t output_pointcloud_size = 0; + output_cloud.data.clear(); + output_cloud.data.resize(input_pointcloud_msg.data.size()); + output_cloud.fields = input_pointcloud_msg.fields; + output_cloud.header = input_pointcloud_msg.header; + output_cloud.height = input_pointcloud_msg.height; + output_cloud.point_step = input_pointcloud_msg.point_step; + output_cloud.is_bigendian = input_pointcloud_msg.is_bigendian; + output_cloud.is_dense = input_pointcloud_msg.is_dense; + for (size_t global_offset = 0; global_offset < transformed_cloud.data.size(); + global_offset += point_step) { + float transformed_x = + *reinterpret_cast(&transformed_cloud.data[global_offset + x_offset]); + float transformed_y = + *reinterpret_cast(&transformed_cloud.data[global_offset + y_offset]); + float transformed_z = + *reinterpret_cast(&transformed_cloud.data[global_offset + z_offset]); // skip filtering pointcloud behind the camera or too far from camera - if (*iter_z <= 0.0 || *iter_z > filter_distance_threshold_) { - output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + if (transformed_z <= 0.0 || transformed_z > filter_distance_threshold_) { + copyPointCloud( + input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } @@ -103,7 +117,8 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( bool is_inside_image = projected_point.x() > 0 && projected_point.x() < camera_info.width && projected_point.y() > 0 && projected_point.y() < camera_info.height; if (!is_inside_image) { - output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + copyPointCloud( + input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } @@ -111,16 +126,19 @@ void SegmentPointCloudFusionNode::fuseOnSingleImage( uint8_t semantic_id = mask.at( static_cast(projected_point.y()), static_cast(projected_point.x())); if (static_cast(semantic_id) >= filter_semantic_label_target_.size()) { - output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + copyPointCloud( + input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); continue; } if (!filter_semantic_label_target_.at(semantic_id)) { - output_cloud.push_back(pcl::PointXYZ(*iter_orig_x, *iter_orig_y, *iter_orig_z)); + copyPointCloud( + input_pointcloud_msg, point_step, global_offset, output_cloud, output_pointcloud_size); } } - pcl::toROSMsg(output_cloud, output_pointcloud_msg); - output_pointcloud_msg.header = input_pointcloud_msg.header; + output_cloud.data.resize(output_pointcloud_size); + output_cloud.row_step = output_pointcloud_size / output_cloud.height; + output_cloud.width = output_pointcloud_size / output_cloud.point_step / output_cloud.height; } bool SegmentPointCloudFusionNode::out_of_scope(__attribute__((unused)) diff --git a/planning/.pages b/planning/.pages index a9a9e3dd7e55b..849d117b73b71 100644 --- a/planning/.pages +++ b/planning/.pages @@ -5,18 +5,18 @@ nav: - 'Concept': - 'Planner Manager': planning/behavior_path_planner/docs/behavior_path_planner_manager_design - 'Scene Module Interface': planning/behavior_path_planner/docs/behavior_path_planner_interface_design - - 'Path Generation': planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design - - 'Collision Assessment/Safety Check': planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check - - 'Dynamic Drivable Area': planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design - - 'Turn Signal': planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design + - 'Path Generation': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design + - 'Collision Assessment/Safety Check': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check + - 'Dynamic Drivable Area': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design + - 'Turn Signal': planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design - 'Scene Module': - - 'Avoidance': planning/behavior_path_avoidance_module - 'Avoidance by Lane Change': planning/behavior_path_avoidance_by_lane_change_module - - 'Dynamic Avoidance': planning/behavior_path_dynamic_avoidance_module + - 'Dynamic Obstacle Avoidance': planning/autoware_behavior_path_dynamic_obstacle_avoidance_module - 'Goal Planner': planning/behavior_path_goal_planner_module - 'Lane Change': planning/behavior_path_lane_change_module - 'Side Shift': planning/behavior_path_side_shift_module - 'Start Planner': planning/behavior_path_start_planner_module + - 'Static Obstacle Avoidance': planning/autoware_behavior_path_static_obstacle_avoidance_module - 'Behavior Velocity Planner': - 'About Behavior Velocity': planning/autoware_behavior_velocity_planner - 'Template for Custom Module': planning/autoware_behavior_velocity_template_module @@ -34,7 +34,7 @@ nav: - '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/behavior_velocity_virtual_traffic_light_module + - 'Virtual Traffic Light': planning/autoware_behavior_velocity_virtual_traffic_light_module - 'Walkway': planning/behavior_velocity_walkway_module - 'Parking': - 'Freespace Planner': @@ -67,9 +67,9 @@ nav: - 'About Motion Velocity Planner': planning/autoware_motion_velocity_planner_node/ - 'Available Modules': - 'Out of Lane': planning/autoware_motion_velocity_planner_out_of_lane_module/ - - 'Motion Velocity Smoother': - - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja + - 'Velocity Smoother': + - 'About Velocity Smoother': planning/autoware_velocity_smoother + - 'About Velocity Smoother (Japanese)': planning/autoware_velocity_smoother/README.ja - 'Scenario Selector': planning/scenario_selector - 'Static Centerline Generator': planning/autoware_static_centerline_generator - 'API and Library': @@ -79,6 +79,7 @@ nav: - 'Route Handler': planning/route_handler - 'RTC Interface': planning/rtc_interface - 'Additional Tools': + - 'Remaining Distance Time Calculator': planning/autoware_remaining_distance_time_calculator - 'RTC Replayer': planning/rtc_replayer - 'Planning Debug Tools': - 'About Planning Debug Tools': https://github.com/autowarefoundation/autoware_tools/tree/main/planning/planning_debug_tools diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md index 0b29d463bb105..94784fe6771c0 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/README.md @@ -9,8 +9,8 @@ Each module performs the following roles. Dynamic Avoidance module cuts off the drivable area according to the position and velocity of the target to be avoided. Obstacle Avoidance module modifies the path to be followed so that it fits within the received drivable area. -Avoidance functions are also provided by the [Avoidance module](https://autowarefoundation.github.io/autoware.universe/main/planning/behavior_path_avoidance_module/), but these modules have different roles. -The Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects. +Static obstacle's avoidance functions are also provided by the [Static Avoidance module](https://autowarefoundation.github.io/autoware.universe/main/planning/autoware_behavior_path_static_obstacle_avoidance_module/), but these modules have different roles. +The Static Obstacle Avoidance module performs avoidance through the outside of own lanes but cannot avoid the moving objects. On the other hand, this module can avoid moving objects. For this reason, the word "dynamic" is used in the module's name. The table below lists the avoidance modules that can handle each situation. diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp index caf0544b0852e..336bab7a2f961 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/manager.hpp @@ -16,7 +16,7 @@ #define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ #include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp index 22152911b46b2..120801cb908bd 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/include/autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp @@ -15,15 +15,18 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ #define AUTOWARE_BEHAVIOR_PATH_DYNAMIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include #include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include @@ -54,9 +57,9 @@ std::vector getAllKeys(const std::unordered_map & map) namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedPath; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; struct MinMaxValue { @@ -190,12 +193,12 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface std::string uuid{}; uint8_t label{}; geometry_msgs::msg::Pose pose{}; - autoware_auto_perception_msgs::msg::Shape shape; + autoware_perception_msgs::msg::Shape shape; double vel{0.0}; double lat_vel{0.0}; bool is_object_on_ego_path{false}; std::optional latest_time_inside_ego_path{std::nullopt}; - std::vector predicted_paths{}; + std::vector predicted_paths{}; // NOTE: Previous values of the following are used for low-pass filtering. // Therefore, they has to be initialized as nullopt. @@ -397,7 +400,7 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const std::optional & prev_object) const; bool willObjectBeOutsideEgoChangingPath( const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const; + const autoware_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const; bool isObjectFarFromPath( const PredictedObject & predicted_object, const double obj_dist_to_path) const; TimeWhileCollision calcTimeWhileCollision( @@ -407,15 +410,15 @@ class DynamicObstacleAvoidanceModule : public SceneModuleInterface const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; + const autoware_perception_msgs::msg::Shape & obj_shape) const; double calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const autoware_perception_msgs::msg::Shape & obj_shape, const bool is_object_same_direction) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, - const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape, const TimeWhileCollision & time_while_collision) const; std::optional calcMinMaxLateralOffsetToAvoidRegulatedObject( const std::vector & ref_path_points_for_obj_poly, diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml index 9b64069eddafd..afc7799094b92 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/package.xml @@ -16,11 +16,11 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_behavior_path_planner_common + autoware_perception_msgs + autoware_planning_msgs + autoware_vehicle_msgs behavior_path_planner - behavior_path_planner_common geometry_msgs lanelet2_core lanelet2_extension diff --git a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp index efc56c4d61f5f..f038585172502 100644 --- a/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_dynamic_obstacle_avoidance_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "autoware_behavior_path_dynamic_obstacle_avoidance_module/scene.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -128,13 +128,13 @@ std::pair projectObstacleVelocityToTrajectory( return std::make_pair(projected_velocity[0], projected_velocity[1]); } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); @@ -148,13 +148,13 @@ double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & s throw std::logic_error("The shape type is not supported in dynamic_avoidance."); } -double calcObstacleWidth(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleWidth(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return shape.dimensions.y; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); @@ -458,7 +458,7 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::planWaitingApproval() ObjectType DynamicObstacleAvoidanceModule::getObjectType(const uint8_t label) const { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; if (label == ObjectClassification::CAR && parameters_->avoid_car) { return ObjectType::REGULATED; @@ -1121,8 +1121,8 @@ DynamicObstacleAvoidanceModule::DecisionWithReason DynamicObstacleAvoidanceModul } [[maybe_unused]] bool DynamicObstacleAvoidanceModule::willObjectBeOutsideEgoChangingPath( - const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double obj_vel) const + const geometry_msgs::msg::Pose & obj_pose, const autoware_perception_msgs::msg::Shape & obj_shape, + const double obj_vel) const { if (!ref_path_before_lane_change_ || obj_vel < 0.0) { return false; @@ -1183,7 +1183,7 @@ DynamicObstacleAvoidanceModule::getAdjacentLanes( DynamicObstacleAvoidanceModule::LatLonOffset DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const + const autoware_perception_msgs::msg::Shape & obj_shape) const { const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); @@ -1218,7 +1218,7 @@ DynamicObstacleAvoidanceModule::getLateralLongitudinalOffset( MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( const std::vector & ref_path_points_for_obj_poly, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, - const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, + const PredictedPath & obj_path, const autoware_perception_msgs::msg::Shape & obj_shape, const TimeWhileCollision & time_while_collision) const { const size_t obj_seg_idx = @@ -1290,8 +1290,7 @@ MinMaxValue DynamicObstacleAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( double DynamicObstacleAvoidanceModule::calcValidLengthToAvoid( const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, - const bool is_object_same_direction) const + const autoware_perception_msgs::msg::Shape & obj_shape, const bool is_object_same_direction) const { const auto & input_path_points = getPreviousModuleOutput().path.points; const size_t obj_seg_idx = diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/package.xml b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml index c08eb20a2b589..26b78f5cf8cf1 100644 --- a/planning/autoware_behavior_path_external_request_lane_change_module/package.xml +++ b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -18,9 +18,9 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_path_planner_common behavior_path_lane_change_module behavior_path_planner - behavior_path_planner_common motion_utils pluginlib rclcpp diff --git a/planning/behavior_path_planner_common/CMakeLists.txt b/planning/autoware_behavior_path_planner_common/CMakeLists.txt similarity index 97% rename from planning/behavior_path_planner_common/CMakeLists.txt rename to planning/autoware_behavior_path_planner_common/CMakeLists.txt index dbbd7c7cedc67..d3774bb004826 100644 --- a/planning/behavior_path_planner_common/CMakeLists.txt +++ b/planning/autoware_behavior_path_planner_common/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_planner_common) +project(autoware_behavior_path_planner_common) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md similarity index 100% rename from planning/behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md rename to planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md similarity index 97% rename from planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md rename to planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md index 2a22db5597241..dd2e4df00bddd 100644 --- a/planning/behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md +++ b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md @@ -1,6 +1,6 @@ # Path Generation design -This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp). +This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp). ## Overview diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md similarity index 100% rename from planning/behavior_path_planner_common/docs/behavior_path_planner_safety_check.md rename to planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md diff --git a/planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md b/planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md similarity index 100% rename from planning/behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md rename to planning/autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-ExpDistances.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Expansion_Input.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MaxWidth.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-MinWidth.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/DynamicDrivableArea-Result.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png b/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png rename to planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png b/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png rename to planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_area_boundary_marker_example2.png diff --git a/planning/behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/drivable_lines.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png b/planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png rename to planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_off.png diff --git a/planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png b/planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png rename to planning/autoware_behavior_path_planner_common/images/drivable_area/dynamic_expansion_on.png diff --git a/planning/behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/expanded_lanes.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/hatched_road_markings_drivable_area_expansion.drawio.svg diff --git a/planning/behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg b/planning/autoware_behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/drivable_area/sorted_lanes.drawio.svg diff --git a/planning/behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg b/planning/autoware_behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/path_safety_checker/extended_polygons.drawio.svg diff --git a/planning/behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg b/planning/autoware_behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/path_safety_checker/front_object.drawio.svg diff --git a/planning/behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg b/planning/autoware_behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/path_safety_checker/safety_check_flow.drawio.svg diff --git a/planning/behavior_path_planner_common/images/path_shifter/path_shifter.png b/planning/autoware_behavior_path_planner_common/images/path_shifter/path_shifter.png similarity index 100% rename from planning/behavior_path_planner_common/images/path_shifter/path_shifter.png rename to planning/autoware_behavior_path_planner_common/images/path_shifter/path_shifter.png diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/activation_distance.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_and_turn2.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_first_section.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/avoidance_second_section.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/continuous_turns.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/lane_change_and_turn.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/left_right_turn.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern1.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern2.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern3.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern4.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pattern5.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_out.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/pull_over.drawio.svg diff --git a/planning/behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg b/planning/autoware_behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg similarity index 100% rename from planning/behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg rename to planning/autoware_behavior_path_planner_common/images/turn_signal_decider/sections_definition.drawio.svg diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp similarity index 89% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp index 09f3c2c66bcd4..3b4d8e3ef2ef3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/data_manager.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ -#include "behavior_path_planner_common/parameters.hpp" -#include "behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/parameters.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -27,12 +27,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include #include +#include +#include #include #include #include @@ -40,6 +39,7 @@ #include #include #include +#include #include #include @@ -51,19 +51,19 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_perception_msgs::msg::TrafficSignal; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroup; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using PlanResult = PathWithLaneId::SharedPtr; using lanelet::TrafficLight; using tier4_planning_msgs::msg::VelocityLimit; @@ -72,7 +72,7 @@ using unique_identifier_msgs::msg::UUID; struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; - TrafficSignal signal; + TrafficLightGroup signal; }; struct BoolStamped @@ -269,4 +269,4 @@ struct PlannerData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp similarity index 95% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp index b6dc2997c11e8..97f249e0e7d6e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_interface.hpp @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" -#include -#include +#include +#include #include #include #include @@ -36,8 +36,8 @@ #include #include -#include #include +#include #include #include #include @@ -58,7 +58,6 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; @@ -66,6 +65,7 @@ using steering_factor_interface::SteeringFactorInterface; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::generateUUID; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_planning_msgs::msg::StopReasonArray; @@ -520,7 +520,7 @@ class SceneModuleInterface void setObjectsOfInterestData( const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) + const autoware_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) { for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { if (ptr) { @@ -645,4 +645,4 @@ class SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp similarity index 96% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 2eb56104ec831..55747d5fc0a36 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -13,10 +13,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" #include @@ -348,4 +348,4 @@ class SceneModuleManagerInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp similarity index 84% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp index 1bce30b18edd7..e4c999b43f04d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" @@ -44,4 +44,4 @@ class SceneModuleVisitor }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp similarity index 85% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp index fb0cb97a3db95..31f169471cc9e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ #include @@ -52,4 +52,4 @@ class SteeringFactorInterface } // namespace steering_factor_interface -#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp similarity index 91% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp index 1c066ff744f46..ebc48f9c16cc8 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/colors.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ #include "tier4_autoware_utils/ros/marker_helper.hpp" @@ -91,4 +91,4 @@ inline std::vector colors_list(float a = 0.99) } } // namespace marker_utils::colors -#endif // BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp similarity index 84% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp index 6426f16a44259..4abbfd419c43b 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/marker_utils/utils.hpp @@ -11,19 +11,19 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -34,9 +34,8 @@ namespace marker_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; @@ -50,6 +49,7 @@ using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; @@ -121,4 +121,4 @@ MarkerArray showFilteredObjects( const ColorRGBA & color, int32_t id); } // namespace marker_utils -#endif // BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp similarity index 92% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp index dbe27d856bc40..21d4906380b59 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ #include @@ -79,4 +79,4 @@ struct BehaviorPathPlannerParameters double base_link2rear; }; -#endif // BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp similarity index 88% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp index 29fe05775739e..4dda036fecdb7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/turn_signal_decider.hpp @@ -12,22 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include -#include +#include +#include #include #include #include #include -#include -#include -#include +#include +#include #include +#include #include @@ -42,13 +42,13 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using route_handler::RouteHandler; +using tier4_planning_msgs::msg::PathWithLaneId; const std::map g_signal_map = { {"left", TurnIndicatorsCommand::ENABLE_LEFT}, @@ -64,6 +64,17 @@ struct TurnSignalInfo hazard_signal.command = HazardLightsCommand::NO_COMMAND; } + TurnSignalInfo(const Pose & start, const Pose & end) + { + turn_signal.command = TurnIndicatorsCommand::NO_COMMAND; + hazard_signal.command = HazardLightsCommand::NO_COMMAND; + + desired_start_point = start; + desired_end_point = end; + required_start_point = start; + required_end_point = end; + } + // desired turn signal TurnIndicatorsCommand turn_signal; HazardLightsCommand hazard_signal; @@ -93,6 +104,11 @@ class TurnSignalDecider const TurnSignalInfo & intersection_signal_info, const TurnSignalInfo & behavior_signal_info, const double nearest_dist_threshold, const double nearest_yaw_threshold); + TurnSignalInfo overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const; + TurnSignalInfo use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, @@ -282,4 +298,4 @@ class TurnSignalDecider }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp similarity index 91% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp index 7e4cf94c890d8..f6d45f5179c8c 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -134,5 +134,5 @@ std::vector calculate_smoothed_curvatures( } // namespace drivable_area_expansion // clang-format off -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT // clang-format on diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp similarity index 74% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index 2a4343e2c3f6a..423148c713d10 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include -#include +#include #include namespace drivable_area_expansion @@ -43,7 +43,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths MultiPolygon2d create_object_footprints( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp similarity index 76% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp index dc4a174c2ebf7..6216f9216b33b 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -41,4 +41,4 @@ SegmentRtree extract_uncrossable_segments( bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp similarity index 95% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 055e20a4cda02..32613948814ae 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ #include #include @@ -126,4 +126,4 @@ struct DrivableAreaExpansionParameters }; } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp similarity index 94% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 2aeac123623ce..c272c4975d802 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -180,4 +180,6 @@ inline LineString2d sub_linestring( } } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +// clang-format off +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ // NOLINT +// clang-format on diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp similarity index 90% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index e47adbd9f6680..fcaafae738eba 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT -#include +#include #include #include @@ -102,4 +102,6 @@ std::vector combineDrivableLanes( } // namespace behavior_path_planner::utils -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ +// clang-format off +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ // NOLINT +// clang-format on diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp similarity index 73% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index e438105c6645e..5a26cf516c5aa 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include #include +#include #include @@ -29,12 +29,12 @@ namespace drivable_area_expansion { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiLineString2d; @@ -71,4 +71,4 @@ struct Expansion std::vector min_lane_widths; }; } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp similarity index 88% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp index 5b1e9b4b4c419..f1376d73132c2 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ #include -#include #include #include +#include #include @@ -99,8 +99,7 @@ class OccupancyGridBasedCollisionDetector bool hasObstacleOnPath( const geometry_msgs::msg::PoseArray & path, const bool check_out_of_range) const; bool hasObstacleOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const bool check_out_of_range) const; + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } bool detectCollision(const IndexXYT & base_index, const bool check_out_of_range) const; virtual ~OccupancyGridBasedCollisionDetector() = default; @@ -146,4 +145,4 @@ class OccupancyGridBasedCollisionDetector } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR__OCCUPANCY_GRID_BASED_COLLISION_DETECTOR_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp similarity index 70% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp index 74f6b843803df..9afb6dd494ba3 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include +#include #include namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; @@ -42,4 +42,4 @@ struct StartGoalPlannerData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__COMMON_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp similarity index 88% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 1489dd1beff07..867e76f0db614 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -12,18 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/parameters.hpp" #include -#include -#include +#include #include #include +#include #include @@ -33,10 +33,10 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct ParallelParkingParameters { @@ -148,4 +148,6 @@ class GeometricParallelParking }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ +// clang-format off +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__GEOMETRIC_PARALLEL_PARKING_HPP_ // NOLINT +// clang-format on diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp similarity index 86% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp index e1f3402e8dfdf..af1c176601c9f 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -83,4 +83,4 @@ std::pair calcEndArcLength( } // namespace behavior_path_planner::utils::parking_departure -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PARKING_DEPARTURE__UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp similarity index 93% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index a0b2d0afa0fa8..7a8219ee8c4a9 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include -#include +#include +#include +#include #include @@ -31,8 +31,8 @@ namespace behavior_path_planner::utils::path_safety_checker::filter { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using tier4_planning_msgs::msg::PathPointWithLaneId; bool velocity_filter( const PredictedObject & object, double velocity_threshold, double max_velocity); @@ -47,9 +47,9 @@ bool position_filter( namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using tier4_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters objects based on object centroid position. @@ -320,4 +320,4 @@ void filterObjects(std::vector & objects, Func filter) } } // namespace behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp similarity index 93% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index d83c748f214b4..85c40be5e18b4 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #include -#include +#include #include #include @@ -31,7 +31,7 @@ namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using tier4_autoware_utils::Polygon2d; @@ -77,8 +77,8 @@ struct ExtendedPredictedObject geometry_msgs::msg::PoseWithCovariance initial_pose; geometry_msgs::msg::TwistWithCovariance initial_twist; geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector classification; + autoware_perception_msgs::msg::Shape shape; + std::vector classification; std::vector predicted_paths; ExtendedPredictedObject() = default; @@ -230,7 +230,7 @@ struct CollisionCheckDebug std::vector obj_predicted_path; ///< object's predicted path. Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. - autoware_auto_perception_msgs::msg::Shape obj_shape; ///< object's shape. + autoware_perception_msgs::msg::Shape obj_shape; ///< object's shape. }; using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = @@ -239,5 +239,5 @@ using CollisionCheckDebugMap = } // namespace behavior_path_planner::utils::path_safety_checker // clang-format off -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT // clang-format on diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp similarity index 91% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index c4cfed01450b7..d0693a271379e 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include -#include -#include +#include +#include #include #include @@ -31,9 +31,9 @@ namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; @@ -157,4 +157,4 @@ void updateCollisionCheckDebugMap( CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); } // namespace behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp similarity index 94% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 880e55b64d170..724e784c2ff02 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #include #include #include -#include #include +#include #include #include @@ -29,11 +29,11 @@ #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using tier4_autoware_utils::generateUUID; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; struct ShiftLine @@ -248,4 +248,4 @@ class PathShifter } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp similarity index 84% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp index f109bb2cbb213..e58190fa49650 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/path_utils.hpp @@ -12,19 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include -#include +#include +#include #include -#include -#include -#include +#include +#include #include +#include #include @@ -35,11 +35,11 @@ namespace behavior_path_planner::utils { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_planning_msgs::msg::Path; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; +using tier4_planning_msgs::msg::PathWithLaneId; std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, @@ -113,4 +113,4 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr +#include #include #include #include -#include -#include +#include +#include #include #include @@ -33,8 +33,8 @@ namespace behavior_path_planner::utils::traffic_light { -using autoware_perception_msgs::msg::TrafficSignal; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; using geometry_msgs::msg::Pose; /** @@ -109,4 +109,4 @@ bool isTrafficSignalStop( const lanelet::ConstLanelets & lanelets, const std::shared_ptr & planner_data); } // namespace behavior_path_planner::utils::traffic_light -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp similarity index 92% rename from planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp rename to planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp index b61ef0579cab0..53bd7c508d246 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/autoware_behavior_path_planner_common/include/autoware_behavior_path_planner_common/utils/utils.hpp @@ -12,25 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include +#include +#include #include @@ -41,19 +41,19 @@ namespace behavior_path_planner::utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Vector3; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct PolygonPoint { @@ -350,4 +350,4 @@ size_t findNearestSegmentIndex( } } // namespace behavior_path_planner::utils -#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/package.xml b/planning/autoware_behavior_path_planner_common/package.xml similarity index 93% rename from planning/behavior_path_planner_common/package.xml rename to planning/autoware_behavior_path_planner_common/package.xml index cc6bb451719c8..21da8dcef1c2d 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/autoware_behavior_path_planner_common/package.xml @@ -1,9 +1,9 @@ - behavior_path_planner_common + autoware_behavior_path_planner_common 0.1.0 - The behavior_path_planner_common package + The autoware_behavior_path_planner_common package Zulfaqar Azmi @@ -43,9 +43,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs freespace_planning_algorithms geometry_msgs interpolation diff --git a/planning/behavior_path_planner_common/src/interface/scene_module_visitor.cpp b/planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp similarity index 82% rename from planning/behavior_path_planner_common/src/interface/scene_module_visitor.cpp rename to planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp index af1f9c4c43f70..cf1873ee581fe 100644 --- a/planning/behavior_path_planner_common/src/interface/scene_module_visitor.cpp +++ b/planning/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" namespace behavior_path_planner { diff --git a/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp b/planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp similarity index 95% rename from planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp rename to planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp index cbced86865a99..a231f00d75fdc 100644 --- a/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp +++ b/planning/autoware_behavior_path_planner_common/src/interface/steering_factor_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/interface/steering_factor_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp" namespace steering_factor_interface { diff --git a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp b/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp similarity index 98% rename from planning/behavior_path_planner_common/src/marker_utils/utils.cpp rename to planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp index d97ea2e09a89d..2b150b807c6ce 100644 --- a/planning/behavior_path_planner_common/src/marker_utils/utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/marker_utils/utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/marker_utils/colors.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/colors.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include #include diff --git a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp b/planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp similarity index 93% rename from planning/behavior_path_planner_common/src/turn_signal_decider.cpp rename to planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp index fb15391550980..dbdd6fb4d7059 100644 --- a/planning/behavior_path_planner_common/src/turn_signal_decider.cpp +++ b/planning/autoware_behavior_path_planner_common/src/turn_signal_decider.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include @@ -392,6 +392,40 @@ TurnIndicatorsCommand TurnSignalDecider::resolve_turn_signal( return intersection_signal_info.turn_signal; } +TurnSignalInfo TurnSignalDecider::overwrite_turn_signal( + const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, + const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, + const double nearest_dist_threshold, const double nearest_yaw_threshold) const +{ + if (original_signal.turn_signal.command == TurnIndicatorsCommand::NO_COMMAND) { + return new_signal; + } + + if (original_signal.turn_signal.command == TurnIndicatorsCommand::DISABLE) { + return new_signal; + } + + const auto get_distance = [&](const Pose & input_point) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, input_point, nearest_dist_threshold, nearest_yaw_threshold); + return motion_utils::calcSignedArcLength( + path.points, current_pose.position, current_seg_idx, input_point.position, + nearest_seg_idx) - + base_link2front_; + }; + + const auto & original_desired_end_point = original_signal.desired_end_point; + const auto & new_desired_start_point = new_signal.desired_start_point; + + const double dist_to_original_desired_end = get_distance(original_desired_end_point); + const double dist_to_new_desired_start = get_distance(new_desired_start_point); + if (dist_to_new_desired_start > dist_to_original_desired_end) { + return original_signal; + } + + return new_signal; +} + TurnSignalInfo TurnSignalDecider::use_prior_turn_signal( const PathWithLaneId & path, const Pose & current_pose, const size_t current_seg_idx, const TurnSignalInfo & original_signal, const TurnSignalInfo & new_signal, @@ -615,6 +649,8 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const double current_shift_length, const bool is_driving_forward, const bool egos_lane_is_shifted, const bool override_ego_stopped_check, const bool is_pull_out) const { + using tier4_autoware_utils::getPose; + const auto & p = parameters; const auto & rh = route_handler; const auto & ego_pose = self_odometry->pose.pose; @@ -667,16 +703,19 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( const auto relative_shift_length = end_shift_length - start_shift_length; + const auto p_path_start = getPose(path.path.points.front()); + const auto p_path_end = getPose(path.path.points.back()); + // If shift length is shorter than the threshold, it does not need to turn on blinkers if (std::fabs(relative_shift_length) < p.turn_signal_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the vehicle does not shift anymore, we turn off the blinker if ( std::fabs(end_shift_length - current_shift_length) < p.turn_signal_remaining_shift_length_threshold) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto get_command = [](const auto & shift_length) { @@ -691,7 +730,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( p.vehicle_info.max_longitudinal_offset_m; if (signal_prepare_distance < ego_front_to_shift_start) { - return std::make_pair(TurnSignalInfo{}, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } const auto blinker_start_pose = path.path.points.at(shift_line.start_idx).point.pose; @@ -708,13 +747,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( turn_signal_info.turn_signal.command = get_command(relative_shift_length); if (!p.turn_signal_on_swerving) { - return std::make_pair(turn_signal_info, false); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), false); } lanelet::ConstLanelet lanelet; const auto query_pose = (egos_lane_is_shifted) ? shift_line.end : shift_line.start; if (!rh->getClosestLaneletWithinRoute(query_pose, &lanelet)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } const auto left_same_direction_lane = rh->getLeftLanelet(lanelet, true, true); @@ -729,13 +768,13 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( !is_pull_out && !existShiftSideLane( start_shift_length, end_shift_length, !has_left_lane, !has_right_lane, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // Check if the ego will cross lane bounds. // Note that pull out requires blinkers, even if the ego does not cross lane bounds if (!is_pull_out && !straddleRoadBound(path, shift_line, current_lanelets, p.vehicle_info)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } // If the ego has stopped and its close to completing its shift, turn off the blinkers @@ -744,7 +783,7 @@ std::pair TurnSignalDecider::getBehaviorTurnSignalInfo( if (isNearEndOfShift( start_shift_length, end_shift_length, ego_pose.position, current_lanelets, p.turn_signal_shift_length_threshold)) { - return std::make_pair(TurnSignalInfo{}, true); + return std::make_pair(TurnSignalInfo(p_path_start, p_path_end), true); } } diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp similarity index 97% rename from planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 42c2a375d77cf..018734aa2bedb 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include #include diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp similarity index 90% rename from planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 787af681bf69b..4159ce5000b66 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" #include #include @@ -41,7 +41,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 } MultiPolygon2d create_object_footprints( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { MultiPolygon2d footprints; diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp similarity index 95% rename from planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp index 8420ee96b0ba6..5f9eb817cd049 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" #include diff --git a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp similarity index 99% rename from planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 571a40859202f..165e4bba95ae3 100644 --- a/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -11,9 +11,9 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include diff --git a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp similarity index 96% rename from planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index df48b48d51241..adf8cb62492c8 100644 --- a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include @@ -200,8 +200,7 @@ bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( } bool OccupancyGridBasedCollisionDetector::hasObstacleOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const bool check_out_of_range) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool check_out_of_range) const { for (const auto & p : path.points) { const auto pose_local = global2local(costmap_, p.point.pose); diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp similarity index 98% rename from planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 9400abd20b984..26c089ee0ba09 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -31,7 +31,6 @@ #include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::utils::getArcCoordinates; @@ -40,6 +39,7 @@ using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPoint; using tier4_autoware_utils::normalizeRadian; using tier4_autoware_utils::transformPose; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp similarity index 97% rename from planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp index 97eba861f658a..99f809300c160 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/parking_departure/utils.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp similarity index 98% rename from planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index cdb4b778d3ab3..cc93c067eca96 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include @@ -398,7 +398,7 @@ TargetObjectsOnLane createTargetObjectsOnLane( bool isTargetObjectType( const PredictedObject & object, const ObjectTypesToCheck & target_object_types) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; const auto t = utils::getHighestProbLabel(object.classification); return ( (t == ObjectClassification::CAR && target_object_types.check_car) || diff --git a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp similarity index 99% rename from planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index e1884e57f7221..233e6f47b92cc 100644 --- a/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" diff --git a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp similarity index 99% rename from planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index 1ea555f4675ef..379fc8ca7c8c1 100644 --- a/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include #include diff --git a/planning/behavior_path_planner_common/src/utils/path_utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp similarity index 98% rename from planning/behavior_path_planner_common/src/utils/path_utils.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp index d11bc5a68e9b5..9193db9df11e5 100644 --- a/planning/behavior_path_planner_common/src/utils/path_utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/path_utils.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include @@ -67,7 +67,7 @@ PathWithLaneId resamplePathWithSpline( return path; } - std::vector transformed_path(path.points.size()); + std::vector transformed_path(path.points.size()); for (size_t i = 0; i < path.points.size(); ++i) { transformed_path.at(i) = path.points.at(i).point; } diff --git a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp similarity index 98% rename from planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp index ecd02bfd63485..74141c0d91bd5 100644 --- a/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp similarity index 99% rename from planning/behavior_path_planner_common/src/utils/utils.cpp rename to planning/autoware_behavior_path_planner_common/src/utils/utils.cpp index d674f7770ac67..4d5097ab3738f 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" @@ -40,7 +40,7 @@ namespace { double calcInterpolatedZ( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, + const tier4_planning_msgs::msg::PathWithLaneId & input, const geometry_msgs::msg::Point target_pos, const size_t seg_idx) { const double closest_to_target_dist = motion_utils::calcSignedArcLength( @@ -58,7 +58,7 @@ double calcInterpolatedZ( } double calcInterpolatedVelocity( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t seg_idx) { const double seg_dist = motion_utils::calcSignedArcLength(input.points, seg_idx, seg_idx + 1); @@ -71,8 +71,8 @@ double calcInterpolatedVelocity( namespace behavior_path_planner::utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; @@ -272,9 +272,8 @@ bool exists(std::vector vec, T element) } std::optional findIndexOutOfGoalSearchRange( - const std::vector & points, - const Pose & goal, const int64_t goal_lane_id, - const double max_dist = std::numeric_limits::max()) + const std::vector & points, const Pose & goal, + const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) { if (points.empty()) { return std::nullopt; diff --git a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp b/planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp similarity index 96% rename from planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp rename to planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 77728cc87604d..36a556647f91e 100644 --- a/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp +++ b/planning/autoware_behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include @@ -176,7 +176,7 @@ TEST(DrivableAreaExpansionProjection, expand_drivable_area) { drivable_area_expansion::DrivableAreaExpansionParameters params; drivable_area_expansion::PredictedObjects dynamic_objects; - autoware_auto_mapping_msgs::msg::HADMapBin map; + autoware_map_msgs::msg::LaneletMapBin map; lanelet::LaneletMapPtr empty_lanelet_map_ptr = std::make_shared(); lanelet::utils::conversion::toBinMsg(empty_lanelet_map_ptr, &map); route_handler::RouteHandler route_handler(map); diff --git a/planning/behavior_path_planner_common/test/test_safety_check.cpp b/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp similarity index 94% rename from planning/behavior_path_planner_common/test/test_safety_check.cpp rename to planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp index f85f52bae53ad..3cb71b1f9c4e2 100644 --- a/planning/behavior_path_planner_common/test/test_safety_check.cpp +++ b/planning/autoware_behavior_path_planner_common/test/test_safety_check.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include @@ -27,7 +27,7 @@ constexpr double epsilon = 1e-6; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -143,7 +143,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.resize(5); shape.footprint.points.at(0).x = 3.0; shape.footprint.points.at(0).y = 0.0; diff --git a/planning/behavior_path_planner_common/test/test_turn_signal.cpp b/planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp similarity index 98% rename from planning/behavior_path_planner_common/test/test_turn_signal.cpp rename to planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp index 89101ca5c1ed7..df6f0a7ef81b9 100644 --- a/planning/behavior_path_planner_common/test/test_turn_signal.cpp +++ b/planning/autoware_behavior_path_planner_common/test/test_turn_signal.cpp @@ -11,21 +11,20 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" #include #include #include -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_planning_msgs::msg::PathPoint; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using behavior_path_planner::PathWithLaneId; using behavior_path_planner::Pose; using behavior_path_planner::TurnSignalDecider; @@ -34,6 +33,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Twist; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromYaw; +using tier4_planning_msgs::msg::PathPointWithLaneId; constexpr double nearest_dist_threshold = 5.0; constexpr double nearest_yaw_threshold = M_PI / 3.0; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp index a63c283704001..15ddc3703ead2 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__DATA_STRUCTS_HPP_ +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp index 5da56a320ad3b..7deed888f081e 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__HELPER_HPP_ +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp index 1616339840ba4..a15422d8ead78 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/manager.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__MANAGER_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include #include diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp index 56b9bad535e7d..10c0f57cfb6fe 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp @@ -19,14 +19,14 @@ #include #include -#include +#include #include #include namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; AvoidanceParameters getParameter(rclcpp::Node * node) diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp index 57090e9f551cf..f12c6e56bf06d 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp @@ -15,12 +15,12 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SCENE_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include #include diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp index 21eaef884b5e6..53dbbe2ac4eaf 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__SHIFT_LINE_GENERATOR_HPP_ +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/helper.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp index afdfba30ce267..8bbe4b550ff00 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/type_alias.hpp @@ -23,20 +23,20 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include #include namespace behavior_path_planner { // auto msgs -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedPath; +using tier4_planning_msgs::msg::PathWithLaneId; // ROS 2 general msgs using geometry_msgs::msg::Point; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp index 420edd17ca372..8a5c42a0e210d 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp @@ -15,9 +15,9 @@ #ifndef AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ #define AUTOWARE_BEHAVIOR_PATH_STATIC_OBSTACLE_AVOIDANCE_MODULE__UTILS_HPP_ +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index f243e166e6790..a36a5c73729cb 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -21,11 +21,9 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_behavior_path_planner_common autoware_perception_msgs behavior_path_planner - behavior_path_planner_common geometry_msgs lanelet2_extension magic_enum diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json index e0f1156172932..cb150514ac372 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json @@ -1,9 +1,9 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for behavior_path_avoidance_module", + "title": "Parameters for behavior_path_static_obstacle_avoidance_module", "type": "object", "definitions": { - "behavior_path_avoidance_module": { + "behavior_path_static_obstacle_avoidance_module": { "type": "object", "properties": { "resample_interval_for_planning": { @@ -1473,7 +1473,7 @@ "type": "object", "properties": { "avoidance": { - "$ref": "#/definitions/behavior_path_avoidance_module" + "$ref": "#/definitions/behavior_path_static_obstacle_avoidance_module" } }, "required": ["avoidance"], diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp index b8e6bb99fbebb..f1d59c34a22a6 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include @@ -44,7 +44,7 @@ MarkerArray createObjectsCubeMarkerArray( const ObjectDataArray & objects, std::string && ns, const Vector3 & scale, const ColorRGBA & color) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; MarkerArray msg; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp index bfc95e0730cd6..d43c7243fb238 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp @@ -28,7 +28,7 @@ namespace behavior_path_planner { void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; // init manager interface @@ -42,7 +42,7 @@ void StaticObstacleAvoidanceModuleManager::init(rclcpp::Node * node) void StaticObstacleAvoidanceModuleManager::updateModuleParams( const std::vector & parameters) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::updateParam; auto p = parameters_; diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp index 0a6d490f7fbf1..767c1d21dfc90 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp @@ -14,15 +14,15 @@ #include "autoware_behavior_path_static_obstacle_avoidance_module/scene.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/debug.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -867,10 +867,12 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() if (data.state == AvoidanceState::SUCCEEDED) { removeRegisteredShiftLines(State::SUCCEEDED); + return getPreviousModuleOutput(); } if (data.state == AvoidanceState::CANCEL) { removeRegisteredShiftLines(State::FAILED); + return getPreviousModuleOutput(); } if (data.yield_required) { @@ -915,11 +917,21 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() ignore_signal_ = is_ignore ? std::make_optional(uuid) : std::nullopt; }; + const auto is_large_deviation = [this](const auto & path) { + constexpr double threshold = 1.0; + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(path.points); + const auto lateral_deviation = + motion_utils::calcLateralOffset(path.points, getEgoPosition(), current_seg_idx); + return std::abs(lateral_deviation) > threshold; + }; + // turn signal info if (path_shifter_.getShiftLines().empty()) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else if (is_ignore_signal(path_shifter_.getShiftLines().front().id)) { output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + } else if (is_large_deviation(spline_shift_path.path)) { + output.turn_signal_info = getPreviousModuleOutput().turn_signal_info; } else { const auto original_signal = getPreviousModuleOutput().turn_signal_info; @@ -930,7 +942,7 @@ BehaviorModuleOutput StaticObstacleAvoidanceModule::plan() helper_->getEgoShift(), is_driving_forward, egos_lane_is_shifted); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(spline_shift_path.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( spline_shift_path.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp index f9e35ebd4c1b2..0a362d48574fc 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp @@ -14,8 +14,8 @@ #include "autoware_behavior_path_static_obstacle_avoidance_module/shift_line_generator.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" namespace behavior_path_planner::utils::static_obstacle_avoidance { diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp index 660b8d228b6f2..8671c71430b3d 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/data_structs.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include @@ -40,7 +40,7 @@ namespace behavior_path_planner::utils::static_obstacle_avoidance { -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_perception_msgs::msg::TrafficLightElement; using behavior_path_planner::utils::traffic_light::calcDistanceToRedTrafficLight; using behavior_path_planner::utils::traffic_light::getDistanceToNextTrafficLight; diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index a68705e3f73a2..0bbc2fad3c5a8 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -14,7 +14,7 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Intersection](../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](../behavior_velocity_virtual_traffic_light_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) @@ -29,21 +29,21 @@ So for example, in order to stop at a stop line with the vehicles' front on the ## Input topics -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | -| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | -| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | -| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | -| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | -| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | +| Name | Type | Description | +| ----------------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| `~input/path_with_lane_id` | tier4_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | +| `~input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | +| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | ## Output topics | Name | Type | Description | | ---------------------- | ----------------------------------------- | -------------------------------------- | -| `~output/path` | autoware_auto_planning_msgs::msg::Path | path to be followed | +| `~output/path` | autoware_planning_msgs::msg::Path | path to be followed | | `~output/stop_reasons` | tier4_planning_msgs::msg::StopReasonArray | reasons that cause the vehicle to stop | ## Node parameters diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index a9a096dae7d59..3d51c6b855eb7 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -34,9 +34,9 @@ rosidl_default_generators - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_map_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_velocity_smoother behavior_velocity_planner_common diagnostic_msgs @@ -65,6 +65,7 @@ ament_cmake_ros ament_lint_auto + autoware_behavior_velocity_virtual_traffic_light_module autoware_lint_common behavior_velocity_blind_spot_module behavior_velocity_crosswalk_module @@ -78,7 +79,6 @@ behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_traffic_light_module - behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index dbbbe143658f4..ffbc4ef9174dc 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -57,10 +57,10 @@ namespace autoware::behavior_velocity_planner namespace { -autoware_auto_planning_msgs::msg::Path to_path( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path_with_id) +autoware_planning_msgs::msg::Path to_path( + const tier4_planning_msgs::msg::PathWithLaneId & path_with_id) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; for (const auto & path_point : path_with_id.points) { path.points.push_back(path_point.point); } @@ -79,13 +79,13 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Trigger Subscriber trigger_sub_path_with_lane_id_ = - this->create_subscription( + this->create_subscription( "~/input/path_with_lane_id", 1, std::bind(&BehaviorVelocityPlannerNode::onTrigger, this, _1), createSubscriptionOptions(this)); // Subscribers sub_predicted_objects_ = - this->create_subscription( + this->create_subscription( "~/input/dynamic_objects", 1, std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1), createSubscriptionOptions(this)); @@ -99,12 +99,12 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&BehaviorVelocityPlannerNode::onAcceleration, this, _1), createSubscriptionOptions(this)); - sub_lanelet_map_ = this->create_subscription( + sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&BehaviorVelocityPlannerNode::onLaneletMap, this, _1), createSubscriptionOptions(this)); sub_traffic_signals_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorVelocityPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); @@ -130,7 +130,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio onParam(); // Publishers - path_pub_ = this->create_publisher("~/output/path", 1); + path_pub_ = this->create_publisher("~/output/path", 1); stop_reason_diag_pub_ = this->create_publisher("~/output/stop_reason", 1); debug_viz_pub_ = this->create_publisher("~/debug/path", 1); @@ -231,7 +231,7 @@ void BehaviorVelocityPlannerNode::onOccupancyGrid( } void BehaviorVelocityPlannerNode::onPredictedObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.predicted_objects = msg; @@ -315,7 +315,7 @@ void BehaviorVelocityPlannerNode::onParam() } void BehaviorVelocityPlannerNode::onLaneletMap( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -324,7 +324,7 @@ void BehaviorVelocityPlannerNode::onLaneletMap( } void BehaviorVelocityPlannerNode::onTrafficSignals( - const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -333,28 +333,30 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( const auto traffic_light_id_map_last_observed_old = planner_data_.traffic_light_id_map_last_observed_; planner_data_.traffic_light_id_map_last_observed_.clear(); - for (const auto & signal : msg->signals) { + for (const auto & signal : msg->traffic_light_groups) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_light_group_id] = traffic_signal; const bool is_unknown_observation = std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { - return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + return element.color == autoware_perception_msgs::msg::TrafficLightElement::UNKNOWN; }); // if the observation is UNKNOWN and past observation is available, only update the timestamp // and keep the body of the info - const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + const auto old_data = + traffic_light_id_map_last_observed_old.find(signal.traffic_light_group_id); if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { // copy last observation - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = old_data->second; // update timestamp - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id].stamp = msg->stamp; } else { // if (1)the observation is not UNKNOWN or (2)the very first observation is UNKNOWN - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = + traffic_signal; } } } @@ -373,7 +375,7 @@ void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( } void BehaviorVelocityPlannerNode::onTrigger( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { std::unique_lock lk(mutex_); @@ -396,7 +398,7 @@ void BehaviorVelocityPlannerNode::onTrigger( return; } - const autoware_auto_planning_msgs::msg::Path output_path_msg = + const autoware_planning_msgs::msg::Path output_path_msg = generatePath(input_path_msg, planner_data_); lk.unlock(); @@ -410,11 +412,11 @@ void BehaviorVelocityPlannerNode::onTrigger( } } -autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, +autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data) { - autoware_auto_planning_msgs::msg::Path output_path_msg; + autoware_planning_msgs::msg::Path output_path_msg; // TODO(someone): support backward path const auto is_driving_forward = motion_utils::isDrivingForward(input_path_msg->points); @@ -456,8 +458,7 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath return output_path_msg; } -void BehaviorVelocityPlannerNode::publishDebugMarker( - const autoware_auto_planning_msgs::msg::Path & path) +void BehaviorVelocityPlannerNode::publishDebugMarker(const autoware_planning_msgs::msg::Path & path) { visualization_msgs::msg::MarkerArray output_msg; for (size_t i = 0; i < path.points.size(); ++i) { diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index 6e74203460542..62ceef5f04ea6 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -24,15 +24,15 @@ #include #include -#include -#include -#include -#include +#include +#include +#include #include #include #include #include #include +#include #include #include @@ -46,9 +46,9 @@ namespace autoware::behavior_velocity_planner { -using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; +using autoware_map_msgs::msg::LaneletMapBin; using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; @@ -63,31 +63,30 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // subscriber - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr trigger_sub_path_with_lane_id_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_objects_; rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; rclcpp::Subscription::SharedPtr sub_acceleration_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr sub_traffic_signals_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; rclcpp::Subscription::SharedPtr sub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_external_velocity_limit_; - void onTrigger( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); + void onTrigger(const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); void onPredictedObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); - void onLaneletMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onLaneletMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); void onTrafficSignals( - const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); void onVirtualTrafficLightStates( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void onOccupancyGrid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); @@ -95,11 +94,11 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onParam(); // publisher - rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr path_pub_; rclcpp::Publisher::SharedPtr stop_reason_diag_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; - void publishDebugMarker(const autoware_auto_planning_msgs::msg::Path & path); + void publishDebugMarker(const autoware_planning_msgs::msg::Path & path); // parameter double forward_path_length_; @@ -110,7 +109,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node PlannerData planner_data_; BehaviorVelocityPlannerManager planner_manager_; bool is_driving_forward_{true}; - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; bool has_received_map_; rclcpp::Service::SharedPtr srv_load_plugin_; @@ -127,8 +126,8 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // function geometry_msgs::msg::PoseStamped getCurrentPose(); bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const; - autoware_auto_planning_msgs::msg::Path generatePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, + autoware_planning_msgs::msg::Path generatePath( + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg, const PlannerData & planner_data); std::unique_ptr logger_configure_; diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index f939f67a06f9e..f462fc963f17b 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -96,11 +96,11 @@ void BehaviorVelocityPlannerManager::removeScenePlugin( } } -autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( +tier4_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg) + const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg) { - autoware_auto_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; + tier4_planning_msgs::msg::PathWithLaneId output_path_msg = input_path_msg; int first_stop_path_point_index = static_cast(output_path_msg.points.size() - 1); std::string stop_reason_msg("path_end"); diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index f9c1d0253de62..9e7f2942bb067 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -20,12 +20,12 @@ #include #include -#include -#include -#include -#include +#include +#include +#include #include #include +#include #include #include @@ -48,9 +48,9 @@ class BehaviorVelocityPlannerManager void launchScenePlugin(rclcpp::Node & node, const std::string & name); void removeScenePlugin(rclcpp::Node & node, const std::string & name); - autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( + tier4_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg); + const tier4_planning_msgs::msg::PathWithLaneId & input_path_msg); diagnostic_msgs::msg::DiagnosticStatus getStopReasonDiag() const; 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 3f60c904d6eb4..7cbe0f65a1fc2 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 @@ -54,12 +54,19 @@ std::shared_ptr generateNode() const auto velocity_smoother_dir = ament_index_cpp::get_package_share_directory("autoware_velocity_smoother"); - const auto get_behavior_velocity_module_config = [](const std::string & module) { + // TODO(esteve): delete when all the modules are migrated to autoware_behavior_velocity_* + const auto get_behavior_velocity_module_config_no_prefix = [](const std::string & module) { const auto package_name = "behavior_velocity_" + module + "_module"; const auto package_path = ament_index_cpp::get_package_share_directory(package_name); return package_path + "/config/" + module + ".param.yaml"; }; + const auto get_behavior_velocity_module_config = [](const std::string & module) { + const auto package_name = "autoware_behavior_velocity_" + module + "_module"; + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return package_path + "/config/" + module + ".param.yaml"; + }; + std::vector module_names; module_names.emplace_back("behavior_velocity_planner::CrosswalkModulePlugin"); module_names.emplace_back("behavior_velocity_planner::WalkwayModulePlugin"); @@ -68,7 +75,7 @@ std::shared_ptr generateNode() module_names.emplace_back("behavior_velocity_planner::MergeFromPrivateModulePlugin"); module_names.emplace_back("behavior_velocity_planner::BlindSpotModulePlugin"); module_names.emplace_back("behavior_velocity_planner::DetectionAreaModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::VirtualTrafficLightModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); @@ -89,20 +96,20 @@ std::shared_ptr generateNode() velocity_smoother_dir + "/config/default_velocity_smoother.param.yaml", velocity_smoother_dir + "/config/Analytical.param.yaml", behavior_velocity_planner_dir + "/config/behavior_velocity_planner.param.yaml", - get_behavior_velocity_module_config("blind_spot"), - get_behavior_velocity_module_config("crosswalk"), - get_behavior_velocity_module_config("walkway"), - get_behavior_velocity_module_config("detection_area"), - get_behavior_velocity_module_config("intersection"), - get_behavior_velocity_module_config("no_stopping_area"), - get_behavior_velocity_module_config("occlusion_spot"), - get_behavior_velocity_module_config("run_out"), - get_behavior_velocity_module_config("speed_bump"), - get_behavior_velocity_module_config("stop_line"), - get_behavior_velocity_module_config("traffic_light"), + get_behavior_velocity_module_config_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_no_prefix("detection_area"), + get_behavior_velocity_module_config_no_prefix("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_no_prefix("speed_bump"), + get_behavior_velocity_module_config_no_prefix("stop_line"), + get_behavior_velocity_module_config_no_prefix("traffic_light"), get_behavior_velocity_module_config("virtual_traffic_light"), - get_behavior_velocity_module_config("out_of_lane"), - get_behavior_velocity_module_config("no_drivable_lane")}); + get_behavior_velocity_module_config_no_prefix("out_of_lane"), + get_behavior_velocity_module_config_no_prefix("no_drivable_lane")}); // TODO(Takagi, Isamu): set launch_modules // TODO(Kyoichi Sugahara) set to true launch_virtual_traffic_light diff --git a/planning/autoware_behavior_velocity_template_module/README.md b/planning/autoware_behavior_velocity_template_module/README.md index 2cc3e6db8a555..130da3f919482 100644 --- a/planning/autoware_behavior_velocity_template_module/README.md +++ b/planning/autoware_behavior_velocity_template_module/README.md @@ -50,13 +50,13 @@ The managing of your modules is defined in manager.hpp and manager.cpp. The mana #### `launchNewModules()` Method -- This is a private method that takes an argument of type `autoware_auto_planning_msgs::msg::PathWithLaneId`. +- This is a private method that takes an argument of type `tier4_planning_msgs::msg::PathWithLaneId`. - It is responsible for launching new modules based on the provided path information (PathWithLaneId). The implementation of this method involves initializing and configuring modules specific to your behavior velocity planner by using the `TemplateModule` class. - In the provided source code, it initializes a `module_id` to 0 and checks if a module with the same ID is already registered. If not, it registers a new `TemplateModule` with the module ID. Note that each module managed by the `TemplateModuleManager` should have a unique ID. The template code registers a single module, so the `module_id` is set as 0 for simplicity. #### `getModuleExpiredFunction()` Method -- This is a private method that takes an argument of type `autoware_auto_planning_msgs::msg::PathWithLaneId`. +- This is a private method that takes an argument of type `tier4_planning_msgs::msg::PathWithLaneId`. - It returns a `std::function&)>`. This function is used by the behavior velocity planner to determine whether a particular module has expired or not based on the given path. - The implementation of this method is expected to return a function that can be used to check the expiration status of modules. diff --git a/planning/autoware_behavior_velocity_template_module/package.xml b/planning/autoware_behavior_velocity_template_module/package.xml index fa3b20f5b5af8..a6bcbf5c34e76 100644 --- a/planning/autoware_behavior_velocity_template_module/package.xml +++ b/planning/autoware_behavior_velocity_template_module/package.xml @@ -13,7 +13,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/autoware_behavior_velocity_template_module/src/manager.cpp index c5861d032dee1..8f6621aaab835 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.cpp @@ -38,7 +38,7 @@ TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) } void TemplateModuleManager::launchNewModules( - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) { int64_t module_id = 0; if (!isModuleRegistered(module_id)) { @@ -49,7 +49,7 @@ void TemplateModuleManager::launchNewModules( std::function &)> TemplateModuleManager::getModuleExpiredFunction( - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) { return []([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { return false; diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/autoware_behavior_velocity_template_module/src/manager.hpp index aaaf0710eff46..8e95f516c337d 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -63,7 +63,7 @@ class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleMan * * @param path The path with lane ID information to determine module launch. */ - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; /** * @brief Get a function to check module expiration. @@ -75,7 +75,7 @@ class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleMan * @return A function for checking module expiration. */ std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; /** diff --git a/planning/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/autoware_behavior_velocity_template_module/src/scene.hpp index e8c00efcb3ee1..3ce5ddbd8729d 100644 --- a/planning/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/scene.hpp @@ -23,9 +23,9 @@ namespace autoware::behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using ::behavior_velocity_planner::SceneModuleInterface; using ::behavior_velocity_planner::StopReason; +using tier4_planning_msgs::msg::PathWithLaneId; class TemplateModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt b/planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt similarity index 83% rename from planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt index a1c00cf49db29..9d1fe762262dd 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_virtual_traffic_light_module) +project(autoware_behavior_velocity_virtual_traffic_light_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_virtual_traffic_light_module/README.md b/planning/autoware_behavior_velocity_virtual_traffic_light_module/README.md similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/README.md rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/README.md diff --git a/planning/behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/config/virtual_traffic_light.param.yaml diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/V2X_support_traffic_light.png diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/intersection-coordination.png diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/keep_stopping.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/restart.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/restart.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg b/planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg similarity index 100% rename from planning/behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/docs/restart_prevention.svg diff --git a/planning/behavior_velocity_virtual_traffic_light_module/package.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml similarity index 86% rename from planning/behavior_velocity_virtual_traffic_light_module/package.xml rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index 56d958d29bdec..cd35d4308c26a 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_virtual_traffic_light_module + autoware_behavior_velocity_virtual_traffic_light_module 0.1.0 - The behavior_velocity_virtual_traffic_light_module package + The autoware_behavior_velocity_virtual_traffic_light_module package Kosuke Takeuchi Tomoya Kimura @@ -17,7 +17,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml new file mode 100644 index 0000000000000..2402fc13469b9 --- /dev/null +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp similarity index 98% rename from planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp index 83b5e6317aeb0..2f389d5d1104f 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -28,7 +28,7 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::toMsg; using namespace std::literals::string_literals; -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -147,4 +147,4 @@ visualization_msgs::msg::MarkerArray VirtualTrafficLightModule::createDebugMarke return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp similarity index 89% rename from planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 5310fae78c294..5a32cfd2f74f0 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -24,10 +24,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::VirtualTrafficLight; using tier4_autoware_utils::getOrDeclareParameter; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -49,7 +50,7 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node } void VirtualTrafficLightModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -67,7 +68,7 @@ void VirtualTrafficLightModuleManager::launchNewModules( std::function &)> VirtualTrafficLightModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -76,9 +77,9 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( return id_set.count(scene_module->getModuleId()) == 0; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::VirtualTrafficLightModulePlugin, + autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp similarity index 75% rename from planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index 15bd6f468132e..c73bb0d706008 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -22,13 +22,16 @@ #include #include -#include +#include #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; class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface { public: @@ -38,16 +41,16 @@ class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface private: VirtualTrafficLightModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class VirtualTrafficLightModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp similarity index 97% rename from planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 09e2219646e82..7d91a954171dd 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -23,8 +23,13 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PlanningBehavior; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::VelocityFactor; +namespace arc_lane_utils = ::behavior_velocity_planner::arc_lane_utils; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { using tier4_autoware_utils::calcDistance2d; @@ -145,7 +150,7 @@ std::optional findLastCollisionBeforeEndLine( return {}; } -void insertStopVelocityFromStart(autoware_auto_planning_msgs::msg::PathWithLaneId * path) +void insertStopVelocityFromStart(tier4_planning_msgs::msg::PathWithLaneId * path) { for (auto & p : path->points) { p.point.longitudinal_velocity_mps = 0.0; @@ -154,7 +159,7 @@ void insertStopVelocityFromStart(autoware_auto_planning_msgs::msg::PathWithLaneI std::optional insertStopVelocityAtCollision( const SegmentIndexWithPoint & collision, const double offset, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) + tier4_planning_msgs::msg::PathWithLaneId * path) { const auto collision_offset = motion_utils::calcLongitudinalOffsetToSegment(path->points, collision.index, collision.point); @@ -523,7 +528,7 @@ bool VirtualTrafficLightModule::hasRightOfWay( } void VirtualTrafficLightModule::insertStopVelocityAtStopLine( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) { const auto collision = @@ -586,7 +591,7 @@ void VirtualTrafficLightModule::insertStopVelocityAtStopLine( } void VirtualTrafficLightModule::insertStopVelocityAtEndLine( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx) { const auto collision = @@ -618,4 +623,4 @@ void VirtualTrafficLightModule::insertStopVelocityAtEndLine( module_data_.stop_head_pose_at_end_line = calcHeadPose(stop_pose, planner_data_->vehicle_info_.max_longitudinal_offset_m); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp similarity index 89% rename from planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp rename to planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index c8541555a6e3a..c83ff4e0607ef 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -30,8 +30,12 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PathWithLaneId; +using ::behavior_velocity_planner::Pose; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::StopReason; class VirtualTrafficLightModule : public SceneModuleInterface { public: @@ -57,7 +61,7 @@ class VirtualTrafficLightModule : public SceneModuleInterface struct ModuleData { geometry_msgs::msg::Pose head_pose{}; - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; std::optional stop_head_pose_at_stop_line; std::optional stop_head_pose_at_end_line; }; @@ -119,12 +123,12 @@ class VirtualTrafficLightModule : public SceneModuleInterface bool hasRightOfWay(const tier4_v2x_msgs::msg::VirtualTrafficLightState & state); void insertStopVelocityAtStopLine( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); void insertStopVelocityAtEndLine( - autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::PathWithLaneId * path, tier4_planning_msgs::msg::StopReason * stop_reason, const size_t end_line_idx); }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/autoware_path_optimizer/README.md b/planning/autoware_path_optimizer/README.md index 29ffee76b0c49..9222e5d32ea7d 100644 --- a/planning/autoware_path_optimizer/README.md +++ b/planning/autoware_path_optimizer/README.md @@ -19,16 +19,16 @@ Note that the velocity is just taken over from the input path. ### input -| Name | Type | Description | -| ------------------ | ------------------------------------ | -------------------------------------------------- | -| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | -| `~/input/odometry` | nav_msgs/msg/Odometry | Current Velocity of ego vehicle | +| Name | Type | Description | +| ------------------ | ------------------------------- | -------------------------------------------------- | +| `~/input/path` | autoware_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current Velocity of ego vehicle | ### output -| Name | Type | Description | -| --------------------- | ------------------------------------------ | ----------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/msg/Trajectory | Optimized trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs/msg/Trajectory | Optimized trajectory that is feasible to drive and collision-free | ## Flowchart diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp index 3ef47b865bf8e..d1d2abaeaad61 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp @@ -29,7 +29,7 @@ namespace autoware_path_optimizer { MarkerArray getDebugMarker( const DebugData & debug_data, - const std::vector & optimized_points, + const std::vector & optimized_points, const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); } // namespace autoware_path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp index f249b7486bce6..755d0b2ace297 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp @@ -15,10 +15,10 @@ #ifndef AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ #define AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -33,10 +33,10 @@ namespace autoware_path_optimizer // std_msgs using std_msgs::msg::Header; // planning -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // visualization diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp index d21d353f951c4..32ef8cd5941fc 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp @@ -25,8 +25,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp index c4d3f9c063433..1056d80ef37aa 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp @@ -24,8 +24,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/autoware_path_optimizer/package.xml b/planning/autoware_path_optimizer/package.xml index 9b2d1e952d91a..f02473ebd015e 100644 --- a/planning/autoware_path_optimizer/package.xml +++ b/planning/autoware_path_optimizer/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index e05098d126a74..9d93cdc26a7ed 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -20,8 +20,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 66d0fc08ccf06..7983c5c2a3c2f 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -21,8 +21,8 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp index 82636af20579d..08a6e1b9d6960 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager.hpp @@ -35,15 +35,14 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include +#include +#include +#include #include #include #include @@ -57,6 +56,7 @@ #include #include #include +#include #include #include #include @@ -72,13 +72,12 @@ namespace planning_test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Point; using geometry_msgs::msg::PoseStamped; @@ -92,6 +91,7 @@ using tier4_api_msgs::msg::CrosswalkStatus; using tier4_api_msgs::msg::IntersectionStatus; using tier4_planning_msgs::msg::ExpandStopRange; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::VelocityLimit; using tier4_v2x_msgs::msg::VirtualTrafficLightStateArray; @@ -190,7 +190,7 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr expand_stop_range_pub_; rclcpp::Publisher::SharedPtr occupancy_grid_pub_; rclcpp::Publisher::SharedPtr cost_map_pub_; - rclcpp::Publisher::SharedPtr map_pub_; + rclcpp::Publisher::SharedPtr map_pub_; rclcpp::Publisher::SharedPtr scenario_pub_; rclcpp::Publisher::SharedPtr parking_scenario_pub_; rclcpp::Publisher::SharedPtr lane_driving_scenario_pub_; @@ -201,7 +201,7 @@ class PlanningInterfaceTestManager rclcpp::Publisher::SharedPtr initial_pose_tf_pub_; rclcpp::Publisher::SharedPtr lateral_offset_pub_; rclcpp::Publisher::SharedPtr operation_mode_state_pub_; - rclcpp::Publisher::SharedPtr traffic_signals_pub_; + rclcpp::Publisher::SharedPtr traffic_signals_pub_; rclcpp::Publisher::SharedPtr virtual_traffic_light_states_pub_; // Subscriber diff --git a/planning/autoware_planning_test_manager/package.xml b/planning/autoware_planning_test_manager/package.xml index e2c00756c2ba4..3170ff5f854b9 100644 --- a/planning/autoware_planning_test_manager/package.xml +++ b/planning/autoware_planning_test_manager/package.xml @@ -13,12 +13,11 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils lanelet2_extension diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 9ee162ec02d1f..01a8d89ca8b4d 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -171,7 +171,7 @@ void PlanningInterfaceTestManager::publishTrafficSignals( rclcpp::Node::SharedPtr target_node, std::string topic_name) { test_utils::publishToTargetNode( - test_node_, target_node, topic_name, traffic_signals_pub_, TrafficSignalArray{}); + test_node_, target_node, topic_name, traffic_signals_pub_, TrafficLightGroupArray{}); } void PlanningInterfaceTestManager::publishVirtualTrafficLightState( @@ -293,7 +293,7 @@ void PlanningInterfaceTestManager::publishNominalPath( { test_utils::publishToTargetNode( test_node_, target_node, topic_name, normal_path_pub_, - motion_utils::convertToPath( + motion_utils::convertToPath( test_utils::loadPathWithLaneIdInYaml()), 5); } diff --git a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp index 2a88cdb57abf4..c87c397bd07aa 100644 --- a/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp +++ b/planning/autoware_remaining_distance_time_calculator/src/remaining_distance_time_calculator_node.hpp @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include #include @@ -50,7 +50,7 @@ class RemainingDistanceTimeCalculatorNode : public rclcpp::Node private: using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using Odometry = nav_msgs::msg::Odometry; using VelocityLimit = tier4_planning_msgs::msg::VelocityLimit; using MissionRemainingDistanceTime = autoware_internal_msgs::msg::MissionRemainingDistanceTime; diff --git a/planning/autoware_static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml index 62cdecc1f1eb2..4cf19c1da0e6a 100644 --- a/planning/autoware_static_centerline_generator/package.xml +++ b/planning/autoware_static_centerline_generator/package.xml @@ -15,11 +15,11 @@ rosidl_default_generators - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_behavior_path_planner_common + autoware_map_msgs autoware_path_optimizer - behavior_path_planner_common + autoware_perception_msgs + autoware_planning_msgs geography_utils geometry_msgs global_parameter_loader diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py index fec3d21d20bec..3672165caed85 100755 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py @@ -26,7 +26,7 @@ from PyQt5.QtWidgets import QSizePolicy from PyQt5.QtWidgets import QSlider from PyQt5.QtWidgets import QWidget -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 74002916bb23c..24e95353eec93 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -173,7 +173,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( { // publishers pub_map_bin_ = - create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); + create_publisher("lanelet2_map_topic", utils::create_transient_local_qos()); pub_whole_centerline_ = create_publisher("output_whole_centerline", utils::create_transient_local_qos()); pub_centerline_ = @@ -357,7 +357,7 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ std::filesystem::copy_options::overwrite_existing); // load map by the map_loader package - map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { + map_bin_ptr_ = [&]() -> LaneletMapBin::ConstSharedPtr { // load map map_projector_info_ = std::make_unique(load_info_from_lanelet2_map(lanelet2_input_file_path)); @@ -379,7 +379,7 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ const auto map_bin_msg = Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); - return std::make_shared(map_bin_msg); + return std::make_shared(map_bin_msg); }(); // check if map_bin_ptr_ is not null pointer diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp index dfe5af68c270b..f8a65f2794809 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -80,7 +80,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node const CenterlineWithRoute & centerline_with_route); lanelet::LaneletMapPtr original_map_ptr_{nullptr}; - HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; std::unique_ptr map_projector_info_{nullptr}; @@ -95,7 +95,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node OptimizationTrajectoryBasedCenterline optimization_trajectory_based_centerline_; // publisher - rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; + rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; rclcpp::Publisher::SharedPtr pub_whole_centerline_{nullptr}; rclcpp::Publisher::SharedPtr pub_centerline_{nullptr}; diff --git a/planning/autoware_static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp index fb54804db105d..d40e519ef2704 100644 --- a/planning/autoware_static_centerline_generator/src/type_alias.hpp +++ b/planning/autoware_static_centerline_generator/src/type_alias.hpp @@ -17,29 +17,29 @@ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_mapping_msgs/msg/had_map_bin.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/lanelet_route.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace autoware::static_centerline_generator { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index 2dbd82772f7ef..46044a83f3764 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -14,8 +14,8 @@ #include "utils.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" diff --git a/planning/autoware_velocity_smoother/README.ja.md b/planning/autoware_velocity_smoother/README.ja.md index 68957078e2bdb..d36a328ddf7ca 100644 --- a/planning/autoware_velocity_smoother/README.ja.md +++ b/planning/autoware_velocity_smoother/README.ja.md @@ -80,29 +80,29 @@ ### Input -| Name | Type | Description | -| ------------------------------------------ | ---------------------------------------- | ----------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | -| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | -| `/tf` | `tf2_msgs/TFMessage` | TF | -| `/tf_static` | `tf2_msgs/TFMessage` | TF static | +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | ### Output -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory | -| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | -| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | -| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | -| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | -| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | -| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | -| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | -| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | -| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | -| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | ## Parameters diff --git a/planning/autoware_velocity_smoother/README.md b/planning/autoware_velocity_smoother/README.md index b8a8a9eb7bb5f..1a506d8612aa6 100644 --- a/planning/autoware_velocity_smoother/README.md +++ b/planning/autoware_velocity_smoother/README.md @@ -92,30 +92,30 @@ After the optimization, a resampling called `post resampling` is performed befor ### Input -| Name | Type | Description | -| ------------------------------------------ | ---------------------------------------- | ----------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | -| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | -| `/tf` | `tf2_msgs/TFMessage` | TF | -| `/tf_static` | `tf2_msgs/TFMessage` | TF static | +| Name | Type | Description | +| ------------------------------------------ | ----------------------------------- | ----------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `/planning/scenario_planning/max_velocity` | `std_msgs/Float32` | External velocity limit [m/s] | +| `/localization/kinematic_state` | `nav_msgs/Odometry` | Current odometry | +| `/tf` | `tf2_msgs/TFMessage` | TF | +| `/tf_static` | `tf2_msgs/TFMessage` | TF static | ### Output -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------- | --------------------------------------------------------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Modified trajectory | -| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | -| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | -| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | -| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | -| `~/debug/trajectory_raw` | `autoware_auto_planning_msgs/Trajectory` | Extracted trajectory (for debug) | -| `~/debug/trajectory_external_velocity_limited` | `autoware_auto_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | -| `~/debug/trajectory_lateral_acc_filtered` | `autoware_auto_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | -| `~/debug/trajectory_steering_rate_limited` | `autoware_auto_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | -| `~/debug/trajectory_time_resampled` | `autoware_auto_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | -| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | -| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------- | --------------------------------------------------------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Modified trajectory | +| `/planning/scenario_planning/current_max_velocity` | `std_msgs/Float32` | Current external velocity limit [m/s] | +| `~/closest_velocity` | `std_msgs/Float32` | Planned velocity closest to ego base_link (for debug) | +| `~/closest_acceleration` | `std_msgs/Float32` | Planned acceleration closest to ego base_link (for debug) | +| `~/closest_jerk` | `std_msgs/Float32` | Planned jerk closest to ego base_link (for debug) | +| `~/debug/trajectory_raw` | `autoware_planning_msgs/Trajectory` | Extracted trajectory (for debug) | +| `~/debug/trajectory_external_velocity_limited` | `autoware_planning_msgs/Trajectory` | External velocity limited trajectory (for debug) | +| `~/debug/trajectory_lateral_acc_filtered` | `autoware_planning_msgs/Trajectory` | Lateral acceleration limit filtered trajectory (for debug) | +| `~/debug/trajectory_steering_rate_limited` | `autoware_planning_msgs/Trajectory` | Steering angle rate limit filtered trajectory (for debug) | +| `~/debug/trajectory_time_resampled` | `autoware_planning_msgs/Trajectory` | Time resampled trajectory (for debug) | +| `~/distance_to_stopline` | `std_msgs/Float32` | Distance to stop line from current ego pose (max 50 m) (for debug) | +| `~/stop_speed_exceeded` | `std_msgs/Bool` | It publishes `true` if planned velocity on the point which the maximum velocity is zero is over threshold | ## Parameters diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp index c2fe415091a0a..aecfab342e7e0 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -37,8 +37,8 @@ #include #include "autoware_adapi_v1_msgs/msg/operation_mode_state.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" // temporary @@ -55,8 +55,8 @@ namespace autoware_velocity_smoother { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using autoware_adapi_v1_msgs::msg::OperationModeState; using geometry_msgs::msg::AccelWithCovarianceStamped; diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp index 4c820b4d04baa..dc85c2b2f336f 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp @@ -15,7 +15,7 @@ #ifndef AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ #define AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -24,7 +24,7 @@ namespace autoware_velocity_smoother { namespace resampling { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; struct ResampleParam diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index 08d0a1cd1a8e1..e54a6e7b8afad 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -21,7 +21,7 @@ #include "rclcpp/rclcpp.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 596a8aa94c707..3ab4fce11fd0b 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -18,7 +18,7 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include @@ -29,8 +29,8 @@ namespace autoware_velocity_smoother { namespace analytical_velocity_planning_utils { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; bool calcStopDistWithJerkAndAccConstraints( const double v0, const double a0, const double jerk_acc, const double jerk_dec, diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 57414b8e8109f..09c1e7e96be7b 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -20,7 +20,7 @@ #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 3d94102495839..066e0acef67f6 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -20,7 +20,7 @@ #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index c8bdc9d3c939c..b54de318e9702 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -20,7 +20,7 @@ #include "osqp_interface/osqp_interface.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "boost/optional.hpp" diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp index 63d5e5e887124..43ccfcf193f14 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp @@ -18,14 +18,14 @@ #include "autoware_velocity_smoother/resample.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include #include namespace autoware_velocity_smoother { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; class SmootherBase diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp index 775f4109a4ad1..d35f80fb7fad8 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp @@ -15,8 +15,8 @@ #ifndef AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #define AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" -#include "geometry_msgs/msg/detail/pose__struct.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" +#include "geometry_msgs/msg/pose.hpp" #include #include @@ -25,7 +25,7 @@ namespace autoware_velocity_smoother::trajectory_utils { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Pose; diff --git a/planning/autoware_velocity_smoother/package.xml b/planning/autoware_velocity_smoother/package.xml index b390dcc12edc5..4b5a46e495545 100644 --- a/planning/autoware_velocity_smoother/package.xml +++ b/planning/autoware_velocity_smoother/package.xml @@ -20,7 +20,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index d2956e67c6ac1..9cdfc515d51b7 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -26,7 +26,7 @@ namespace { -using TrajectoryPoints = std::vector; +using TrajectoryPoints = std::vector; geometry_msgs::msg::Pose lerpByPose( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) diff --git a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index ace1d1f14d776..51ee84f535ca4 100644 --- a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -18,7 +18,7 @@ #include -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using autoware_velocity_smoother::trajectory_utils::TrajectoryPoints; TrajectoryPoints genStraightTrajectory(const size_t size) diff --git a/planning/behavior_path_avoidance_by_lane_change_module/README.md b/planning/behavior_path_avoidance_by_lane_change_module/README.md index d91d7116ee056..036de718ccde8 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/README.md +++ b/planning/behavior_path_avoidance_by_lane_change_module/README.md @@ -13,7 +13,7 @@ This module is designed as one of the obstacle avoidance features and generates ## Inner-workings / Algorithms -Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](../behavior_path_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. +Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Static Object Avoidance Module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md) and the path generation logic of the [Normal Lane Change Module](../behavior_path_lane_change_module/README.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. Check that the following conditions are satisfied after the filtering process for the avoidance target. diff --git a/planning/behavior_path_avoidance_by_lane_change_module/package.xml b/planning/behavior_path_avoidance_by_lane_change_module/package.xml index 84540618e152d..1b11b6d8d445d 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/package.xml +++ b/planning/behavior_path_avoidance_by_lane_change_module/package.xml @@ -18,10 +18,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_path_planner_common autoware_behavior_path_static_obstacle_avoidance_module behavior_path_lane_change_module behavior_path_planner - behavior_path_planner_common lanelet2_extension motion_utils pluginlib diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp index a2e26557d9726..bae9112e6d840 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/interface.cpp @@ -14,8 +14,8 @@ #include "behavior_path_avoidance_by_lane_change_module/interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" #include #include diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp index c7c00d38956c1..868b2585170f9 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp @@ -30,7 +30,7 @@ namespace behavior_path_planner void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; // init manager interface diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp index 80ae361ee3ea1..e0ebeebba955c 100644 --- a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -14,11 +14,11 @@ #include "behavior_path_avoidance_by_lane_change_module/scene.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "autoware_behavior_path_static_obstacle_avoidance_module/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/README.md b/planning/behavior_path_goal_planner_module/README.md index c8cef3d825490..7cca725fecca5 100644 --- a/planning/behavior_path_goal_planner_module/README.md +++ b/planning/behavior_path_goal_planner_module/README.md @@ -356,7 +356,7 @@ Perform safety checks on moving objects. If the object is determined to be dange This module has two methods of safety check, `RSS` and `integral_predicted_polygon`. -`RSS` method is a method commonly used by other behavior path planner modules, see [RSS based safety check utils explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md). +`RSS` method is a method commonly used by other behavior path planner modules, see [RSS based safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md). `integral_predicted_polygon` is a more safety-oriented method. This method is implemented because speeds during pull over are lower than during driving, and fewer objects travel along the edge of the lane. (It is sometimes too reactive and may be less available.) This method integrates the footprints of egos and objects at a given time and checks for collisions between them. diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp index 4da625a7a9087..58203573d0c35 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp @@ -17,7 +17,7 @@ #include "behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" -#include +#include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp index 0926d1010b5c3..4a29553802e23 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp @@ -15,16 +15,16 @@ #ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__FIXED_GOAL_PLANNER_BASE_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" -#include #include +#include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp index 5ce6d83795069..ae1ee8c66a990 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/freespace_pull_over.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp index e67d458874d17..0009fbccdb5d8 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/geometric_pull_over.hpp @@ -15,12 +15,12 @@ #ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GEOMETRIC_PULL_OVER_HPP_ +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_goal_planner_module/pull_over_planner_base.hpp" -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include -#include +#include #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp index 8f456a57e4c78..8938e48e0ceb5 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -15,18 +15,18 @@ #ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_MODULE_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" #include "behavior_path_goal_planner_module/freespace_pull_over.hpp" #include "behavior_path_goal_planner_module/geometric_pull_over.hpp" #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" #include "behavior_path_goal_planner_module/goal_searcher.hpp" #include "behavior_path_goal_planner_module/shift_pull_over.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -34,8 +34,8 @@ #include #include -#include -#include +#include +#include #include @@ -50,7 +50,7 @@ namespace behavior_path_planner { -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::HazardLightsCommand; using geometry_msgs::msg::PoseArray; using lane_departure_checker::LaneDepartureChecker; using nav_msgs::msg::OccupancyGrid; diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp index f6c39ea2d6176..e5b284d03d9c4 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -16,8 +16,8 @@ #ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_PLANNER_PARAMETERS_HPP_ -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp index 89f4086874183..af4723327732a 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_HPP_ +#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_goal_planner_module/goal_searcher_base.hpp" -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp index f5d879358cd38..3ed5e7243b9c5 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/goal_searcher_base.hpp @@ -15,9 +15,9 @@ #ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__GOAL_SEARCHER_BASE_HPP_ +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp index aefd4cac3ee35..4b06236755df7 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/manager.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__MANAGER_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "behavior_path_goal_planner_module/goal_planner_module.hpp" -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp index 9700c44445a65..ca84e7256a01f 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/pull_over_planner_base.hpp @@ -15,19 +15,19 @@ #ifndef BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ #define BEHAVIOR_PATH_GOAL_PLANNER_MODULE__PULL_OVER_PLANNER_BASE_HPP_ +#include "autoware_behavior_path_planner_common/data_manager.hpp" #include "behavior_path_goal_planner_module/goal_planner_parameters.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include #include +#include #include #include #include -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; namespace behavior_path_planner { diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp index d0b0aee83e20c..635816f17497f 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/shift_pull_over.hpp @@ -19,7 +19,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp index be3dd9ba96e38..83e817a8d5a45 100644 --- a/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp @@ -21,11 +21,11 @@ #include #include "visualization_msgs/msg/detail/marker_array__struct.hpp" -#include -#include -#include +#include +#include #include #include +#include #include @@ -35,11 +35,11 @@ namespace behavior_path_planner::goal_planner_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathWithLaneId; using visualization_msgs::msg::Marker; using visualization_msgs::msg::MarkerArray; using Polygon2d = tier4_autoware_utils::Polygon2d; diff --git a/planning/behavior_path_goal_planner_module/package.xml b/planning/behavior_path_goal_planner_module/package.xml index a3023389cdd32..c5201c1d2d9a2 100644 --- a/planning/behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_goal_planner_module/package.xml @@ -20,8 +20,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_path_planner_common behavior_path_planner - behavior_path_planner_common motion_utils pluginlib rclcpp diff --git a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp index cbbe5f585dbe2..30914935ae7e8 100644 --- a/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_goal_planner_module/src/default_fixed_goal_planner.cpp @@ -14,9 +14,9 @@ #include "behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -26,7 +26,7 @@ namespace behavior_path_planner { using Point2d = tier4_autoware_utils::Point2d; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const { @@ -93,7 +93,7 @@ PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the // refine_goal_search_radius_range is recommended bool is_valid_path{false}; - autoware_auto_planning_msgs::msg::PathWithLaneId refined_path; + tier4_planning_msgs::msg::PathWithLaneId refined_path; while (goal_search_radius >= 0 && !is_valid_path) { refined_path = utils::refinePathForGoal(goal_search_radius, M_PI * 0.5, path, refined_goal, goal_lane_id); diff --git a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp index 6d3eb09bf4352..c17d7d38709e5 100644 --- a/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/freespace_pull_over.cpp @@ -14,11 +14,11 @@ #include "behavior_path_goal_planner_module/freespace_pull_over.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp index 0779002690f8f..2f621d30ddf8e 100644 --- a/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/geometric_pull_over.cpp @@ -14,8 +14,8 @@ #include "behavior_path_goal_planner_module/geometric_pull_over.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp index 9bfef90668e8e..d85205e001d0b 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -14,14 +14,14 @@ #include "behavior_path_goal_planner_module/goal_planner_module.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -1092,7 +1092,7 @@ void GoalPlannerModule::setTurnSignalInfo(BehaviorModuleOutput & output) const auto original_signal = getPreviousModuleOutput().turn_signal_info; const auto new_signal = calcTurnSignalInfo(); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, original_signal, new_signal, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp index df91687622349..efd9eda322585 100644 --- a/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -14,10 +14,10 @@ #include "behavior_path_goal_planner_module/goal_searcher.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" diff --git a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp index 376adb178708a..0def2f74d07cf 100644 --- a/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp +++ b/planning/behavior_path_goal_planner_module/src/shift_pull_over.cpp @@ -14,9 +14,9 @@ #include "behavior_path_goal_planner_module/shift_pull_over.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 "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" #include #include diff --git a/planning/behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_goal_planner_module/src/util.cpp index e94ab74a3bbed..202eefe37bd83 100644 --- a/planning/behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_goal_planner_module/src/util.cpp @@ -14,8 +14,8 @@ #include "behavior_path_goal_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index 9cbd864be9dbb..a0870fe428c8a 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -288,7 +288,7 @@ stop #### Candidate Path's Safety check -See [safety check utils explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) +See [safety check utils explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) #### Objects selection and classification @@ -296,7 +296,7 @@ First, we divide the target objects into obstacles in the target lane, obstacles ![object lanes](./images/lane_objects.drawio.svg) -Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../behavior_path_avoidance_module/README.md). +Furthermore, to change lanes behind a vehicle waiting at a traffic light, we skip the safety check for the stopping vehicles near the traffic light. The explanation for parked car detection is written in [documentation for avoidance module](../autoware_behavior_path_static_obstacle_avoidance_module/README.md). The detection area for the target lane can be expanded beyond its original boundaries to enable detection of objects that are outside the target lane's limits. diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp index 4c3d129ac687e..cb23d82f2d60a 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp @@ -15,19 +15,19 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__INTERFACE_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_lane_change_module/scene.hpp" #include "behavior_path_lane_change_module/utils/base_class.hpp" #include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include -#include #include #include +#include #include @@ -37,11 +37,11 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using objects_of_interest_marker_interface::ColorName; using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; +using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeInterface : public SceneModuleInterface { diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp index 31561b6210591..0430d5a8ad89d 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/manager.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__MANAGER_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "behavior_path_lane_change_module/utils/data_structs.hpp" -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "route_handler/route_handler.hpp" #include diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 76181f2195a84..b66437f4cff0e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -23,7 +23,6 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; @@ -35,6 +34,7 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::Direction; +using tier4_planning_msgs::msg::PathWithLaneId; using utils::path_safety_checker::ExtendedPredictedObjects; class NormalLaneChange : public LaneChangeBase diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 792e8924e5df6..f34d8a867e4ca 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -14,21 +14,21 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_ +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include #include -#include #include #include +#include #include #include @@ -36,13 +36,13 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using data::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::Direction; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; class LaneChangeBase { diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index 0947f0e5aff94..43a2a3b5836e6 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -14,8 +14,8 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp index fbdd742a8457b..93e45888005a6 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp @@ -17,7 +17,7 @@ #include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" -#include +#include #include diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp index bc4413e1c69d3..2b1b226e30779 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp @@ -15,9 +15,9 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_ +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_lane_change_module/utils/debug_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index 3af4f487a0fa0..1c43559601e19 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -15,18 +15,18 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ +#include "autoware_behavior_path_planner_common/turn_signal_decider.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "behavior_path_lane_change_module/utils/data_structs.hpp" -#include "behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::TurnSignalInfo; +using tier4_planning_msgs::msg::PathWithLaneId; struct LaneChangePath { PathWithLaneId path{}; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 2d427213141b4..e9a9ab15f6bc0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -15,20 +15,20 @@ #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ +#include "autoware_behavior_path_planner_common/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" -#include "behavior_path_planner_common/parameters.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include "rclcpp/logger.hpp" #include #include -#include -#include +#include #include #include +#include #include @@ -38,10 +38,9 @@ namespace behavior_path_planner::utils::lane_change { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; @@ -54,6 +53,7 @@ using geometry_msgs::msg::Twist; using path_safety_checker::CollisionCheckDebugMap; using route_handler::Direction; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); diff --git a/planning/behavior_path_lane_change_module/package.xml b/planning/behavior_path_lane_change_module/package.xml index d4eca72b239cb..ee4089375376f 100644 --- a/planning/behavior_path_lane_change_module/package.xml +++ b/planning/behavior_path_lane_change_module/package.xml @@ -19,8 +19,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_path_planner_common behavior_path_planner - behavior_path_planner_common motion_utils pluginlib rclcpp diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 1669842117f9f..92d7a29f1983b 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -14,11 +14,11 @@ #include "behavior_path_lane_change_module/interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" #include "behavior_path_lane_change_module/utils/markers.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" #include diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2d88a820e0fae..69beca9804e08 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -14,13 +14,13 @@ #include "behavior_path_lane_change_module/scene.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/traffic_light_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_lane_change_module/utils/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -204,7 +204,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() } // check the priority of turn signals - return getTurnSignalDecider().use_prior_turn_signal( + return getTurnSignalDecider().overwrite_turn_signal( path, current_pose, current_nearest_seg_idx, original_turn_signal_info, current_turn_signal_info, nearest_dist_threshold, nearest_yaw_threshold); } @@ -226,7 +226,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const output.path = abort_path_->path; extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -252,7 +252,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); @@ -297,7 +297,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() extendOutputDrivableArea(output); const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points); - output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( + output.turn_signal_info = planner_data_->turn_signal_decider.overwrite_turn_signal( output.path, getEgoPose(), current_seg_idx, prev_module_output_.turn_signal_info, output.turn_signal_info, planner_data_->parameters.ego_nearest_dist_threshold, planner_data_->parameters.ego_nearest_yaw_threshold); diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp index 9e44e51d8b72d..8a505ddf47984 100644 --- a/planning/behavior_path_lane_change_module/src/utils/markers.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/markers.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/marker_utils/colors.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/colors.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include #include #include -#include -#include +#include #include +#include #include #include diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 4eccd00d78c09..20b502886ccef 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -14,14 +14,14 @@ #include "behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_lane_change_module/utils/data_structs.hpp" #include "behavior_path_lane_change_module/utils/path.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/parameters.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -57,17 +57,17 @@ namespace behavior_path_planner::utils::lane_change { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +using tier4_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using lanelet::ArcCoordinates; +using tier4_planning_msgs::msg::PathPointWithLaneId; rclcpp::Logger get_logger() { diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index a799fad0a1c36..2e79589c9f1bd 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -89,32 +89,32 @@ The Planner Manager's responsibilities include: ### Input -| Name | Required? | Type | Description | -| :---------------------------- | :-------: | :----------------------------------------------------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | -| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | -| ~/input/objects | ○ | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | -| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficSignalArray` | traffic signals information from the perception module | -| ~/input/vector_map | ○ | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map information. | -| ~/input/route | ○ | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | -| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | -| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | +| Name | Required? | Type | Description | +| :---------------------------- | :-------: | :------------------------------------------------------ | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | +| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | +| ~/input/objects | ○ | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficLightGroupArray` | traffic signals information from the perception module | +| ~/input/vector_map | ○ | `autoware_map_msgs::msg::LaneletMapBin` | vector map information. | +| ~/input/route | ○ | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | +| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | +| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | - ○ Mandatory: Planning Module would not work if anyone of this is not present. - △ Optional: Some module would not work, but Planning Module can still be operated. ### Output -| Name | Type | Description | QoS Durability | -| :---------------------------- | :------------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | -| ~/output/path | `autoware_auto_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | -| ~/output/turn_indicators_cmd | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | -| ~/output/hazard_lights_cmd | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | -| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | -| ~/output/stop_reasons | `tier4_planning_msgs::msg::StopReasonArray` | describe the reason for ego vehicle stop | `volatile` | -| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | +| Name | Type | Description | QoS Durability | +| :---------------------------- | :-------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | +| ~/output/path | `tier4_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | +| ~/output/turn_indicators_cmd | `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | +| ~/output/hazard_lights_cmd | `autoware_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | +| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | +| ~/output/stop_reasons | `tier4_planning_msgs::msg::StopReasonArray` | describe the reason for ego vehicle stop | `volatile` | +| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | ### Debug @@ -125,8 +125,8 @@ The Planner Manager's responsibilities include: | ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | | ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | | ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | -| ~/planning/path_candidate/\* | `autoware_auto_planning_msgs::msg::Path` | the path before approval. | `volatile` | -| ~/planning/path_reference/\* | `autoware_auto_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | +| ~/planning/path_candidate/\* | `autoware_planning_msgs::msg::Path` | the path before approval. | `volatile` | +| ~/planning/path_reference/\* | `autoware_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | !!! note @@ -168,7 +168,7 @@ The shifted path generation logic enables the behavior path planner to dynamical !!! note - If you're a math lover, refer to [Path Generation Design](../behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. + If you're a math lover, refer to [Path Generation Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. ## Collision Assessment / Safety check @@ -185,7 +185,7 @@ However, the module does have a limitation concerning the yaw angle of each poin !!! note - For further reading on the collision assessment method, please refer to [Safety check utils](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) + For further reading on the collision assessment method, please refer to [Safety check utils](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) ## Generating Drivable Area @@ -208,7 +208,7 @@ Static drivable area expansion operates under assumptions about the correct arra !!! note - Further details can is provided in [Drivable Area Design](../behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). + Further details can is provided in [Drivable Area Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_drivable_area_design.md). ### Dynamic Drivable Area Logic @@ -220,7 +220,7 @@ Large vehicles require much more space, which sometimes causes them to veer out ## Generating Turn Signal -The Behavior Path Planner module uses the `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` to output turn signal commands (see [TurnIndicatorsCommand.idl](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl)). The system evaluates the driving context and determines when to activate turn signals based on its maneuver planning—like turning, lane changing, or obstacle avoidance. +The Behavior Path Planner module uses the `autoware_vehicle_msgs::msg::TurnIndicatorsCommand` to output turn signal commands (see [TurnIndicatorsCommand.idl](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg)). The system evaluates the driving context and determines when to activate turn signals based on its maneuver planning—like turning, lane changing, or obstacle avoidance. Within this framework, the system differentiates between **desired** and **required** blinker activations. **Desired** activations are those recommended by traffic laws for typical driving scenarios, such as signaling before a lane change or turn. **Required** activations are those that are deemed mandatory for safety reasons, like signaling an abrupt lane change to avoid an obstacle. @@ -228,7 +228,7 @@ The `TurnIndicatorsCommand` message structure has a command field that can take !!! note - For more in-depth information, refer to [Turn Signal Design](../behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md) document. + For more in-depth information, refer to [Turn Signal Design](../autoware_behavior_path_planner_common/docs/behavior_path_planner_turn_signal_design.md) document. ## Rerouting diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md index 7188c03bd2604..5ffdcafdb2497 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -119,8 +119,8 @@ Scene modules receives necessary data and RTC command, and outputs candidate pat | IN | `tier4_planning_msgs::srv::CooperateCommands` | contains approval data for scene module's path modification. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md)) | | OUT | `behavior_path_planner::BehaviorModuleOutput` | contains modified path, turn signal information, etc... | | OUT | `tier4_planning_msgs::msg::CooperateStatus` | contains RTC cooperate status. ([details](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/rtc_interface/README.md)) | -| OUT | `autoware_auto_planning_msgs::msg::Path` | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) | -| OUT | `autoware_auto_planning_msgs::msg::Path` | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization) | +| OUT | `autoware_planning_msgs::msg::Path` | candidate path output by a module that has not received approval for path change. when it approved, the ego's following path is switched to this path. (just for visualization) | +| OUT | `autoware_planning_msgs::msg::Path` | reference path generated from the centerline of the lane the ego is going to follow. (just for visualization) | | OUT | `visualization_msgs::msg::MarkerArray` | virtual wall, debug info, etc... | Scene modules running on the manager are stored on the **candidate modules stack** or **approved modules stack** depending on the condition whether the path modification has been approved or not. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 5a49f4d9ab66e..48a14f71b5ddd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -15,28 +15,28 @@ #ifndef BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ #define BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/steering_factor_interface.hpp" #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/interface/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include #include -#include -#include -#include -#include -#include -#include +#include +#include #include +#include #include +#include +#include #include #include #include #include #include +#include #include #include #include @@ -52,22 +52,22 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_perception_msgs::msg::TrafficSignalArray; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightGroupArray; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PoseWithUuidStamped; +using autoware_vehicle_msgs::msg::HazardLightsCommand; +using autoware_vehicle_msgs::msg::TurnIndicatorsCommand; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; using tier4_planning_msgs::msg::StopReasonArray; @@ -87,14 +87,14 @@ class BehaviorPathPlannerNode : public rclcpp::Node private: rclcpp::Subscription::SharedPtr route_subscriber_; - rclcpp::Subscription::SharedPtr vector_map_subscriber_; + rclcpp::Subscription::SharedPtr vector_map_subscriber_; rclcpp::Subscription::SharedPtr velocity_subscriber_; rclcpp::Subscription::SharedPtr acceleration_subscriber_; rclcpp::Subscription::SharedPtr scenario_subscriber_; rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; rclcpp::Subscription::SharedPtr costmap_subscriber_; - rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; + rclcpp::Subscription::SharedPtr traffic_signals_subscriber_; rclcpp::Subscription::SharedPtr lateral_offset_subscriber_; rclcpp::Subscription::SharedPtr operation_mode_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; @@ -118,7 +118,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node std::unique_ptr steering_factor_interface_ptr_; Scenario::SharedPtr current_scenario_{nullptr}; - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; LaneletRoute::ConstSharedPtr route_ptr_{nullptr}; bool has_received_map_{false}; bool has_received_route_{false}; @@ -140,8 +140,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); void onCostMap(const OccupancyGrid::ConstSharedPtr msg); - void onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg); - void onMap(const HADMapBin::ConstSharedPtr map_msg); + void onTrafficSignals(const TrafficLightGroupArray::ConstSharedPtr msg); + void onMap(const LaneletMapBin::ConstSharedPtr map_msg); void onRoute(const LaneletRoute::ConstSharedPtr route_msg); void onOperationMode(const OperationModeState::ConstSharedPtr msg); void onLateralOffset(const LateralOffset::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 877a8c62cbb63..d376dbb6db68a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -15,16 +15,16 @@ #ifndef BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ #define BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" -#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include #include -#include +#include #include #include @@ -39,8 +39,8 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 91a2ef5951bda..e4fe3d0e9d3e4 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -37,12 +37,12 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_behavior_path_planner_common autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager - behavior_path_planner_common + autoware_vehicle_msgs + behaviortree_cpp_v3 freespace_planning_algorithms frenet_planner geometry_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index a12cadb0c4994..e6ebe66f3f440 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -14,9 +14,9 @@ #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "motion_utils/trajectory/conversion.hpp" #include @@ -96,7 +96,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/costmap", 1, std::bind(&BehaviorPathPlannerNode::onCostMap, this, _1), createSubscriptionOptions(this)); traffic_signals_subscriber_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&BehaviorPathPlannerNode::onTrafficSignals, this, _1), createSubscriptionOptions(this)); lateral_offset_subscriber_ = this->create_subscription( @@ -119,7 +119,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod createSubscriptionOptions(this)); // route_handler - vector_map_subscriber_ = create_subscription( + vector_map_subscriber_ = create_subscription( "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1), createSubscriptionOptions(this)); route_subscriber_ = create_subscription( @@ -339,7 +339,7 @@ void BehaviorPathPlannerNode::run() } // check for map update - HADMapBin::ConstSharedPtr map_ptr{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr{nullptr}; { std::lock_guard lk_map(mutex_map_); // for has_received_map_ and map_ptr_ if (has_received_map_) { @@ -711,8 +711,8 @@ Path BehaviorPathPlannerNode::convertToPath( return output; } - output = motion_utils::convertToPath( - *path_candidate_ptr); + output = + motion_utils::convertToPath(*path_candidate_ptr); // header is replaced by the input one, so it is substituted again output.header = planner_data->route_handler->getRouteHeader(); output.header.stamp = this->now(); @@ -790,19 +790,19 @@ void BehaviorPathPlannerNode::onCostMap(const OccupancyGrid::ConstSharedPtr msg) const std::lock_guard lock(mutex_pd_); planner_data_->costmap = msg; } -void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSharedPtr msg) +void BehaviorPathPlannerNode::onTrafficSignals(const TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); planner_data_->traffic_light_id_map.clear(); - for (const auto & signal : msg->signals) { + for (const auto & signal : msg->traffic_light_groups) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_->traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; + planner_data_->traffic_light_id_map[signal.traffic_light_group_id] = traffic_signal; } } -void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) +void BehaviorPathPlannerNode::onMap(const LaneletMapBin::ConstSharedPtr msg) { const std::lock_guard lock(mutex_map_); map_ptr_ = msg; diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 6cb2fccb31117..499468cd2796c 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -14,9 +14,9 @@ #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" diff --git a/planning/behavior_path_planner/test/input.cpp b/planning/behavior_path_planner/test/input.cpp index c2735abd3e932..ba87af1525692 100644 --- a/planning/behavior_path_planner/test/input.cpp +++ b/planning/behavior_path_planner/test/input.cpp @@ -15,7 +15,7 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Twist; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample) diff --git a/planning/behavior_path_planner/test/input.hpp b/planning/behavior_path_planner/test/input.hpp index ededb32f78be0..33e538156990b 100644 --- a/planning/behavior_path_planner/test/input.hpp +++ b/planning/behavior_path_planner/test/input.hpp @@ -17,21 +17,21 @@ #endif // INPUT_HPP_ -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" +#include "tier4_planning_msgs/msg/path_point_with_lane_id.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; PathWithLaneId generateStraightSamplePathWithLaneId( float initial_pose_value, float pose_increment, size_t point_sample); diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/test/test_utils.cpp index fb2f46cb8b9c0..21fe759a7f001 100644 --- a/planning/behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/test/test_utils.cpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "input.hpp" #include "lanelet2_core/Attribute.h" #include "lanelet2_core/geometry/LineString.h" diff --git a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp index 1fc1534d915d5..84408f72c109c 100644 --- a/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp +++ b/planning/behavior_path_sampling_planner_module/include/behavior_path_sampling_planner_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ #define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__MANAGER_HPP_ -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "behavior_path_sampling_planner_module/sampling_planner_module.hpp" #include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" 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 6fac6216eb740..3f3d144952d8e 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 @@ -15,12 +15,12 @@ #ifndef BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ #define BEHAVIOR_PATH_SAMPLING_PLANNER_MODULE__SAMPLING_PLANNER_MODULE_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_sampling_planner_module/sampling_planner_parameters.hpp" #include "behavior_path_sampling_planner_module/util.hpp" #include "bezier_sampler/bezier_sampling.hpp" @@ -41,8 +41,8 @@ #include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include "tier4_planning_msgs/msg/lateral_offset.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -58,7 +58,7 @@ #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; struct SamplingPlannerData { // input diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index c3c5624f0d182..e6fb0d4d0f573 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -13,12 +13,11 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_behavior_path_planner_common autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager - behavior_path_planner_common + autoware_vehicle_msgs bezier_sampler frenet_planner geometry_msgs diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp index c9edd3d6bf6a6..8059c46423dac 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/data_structs.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ #define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__DATA_STRUCTS_HPP_ -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp index ef8a48bc29760..21621e1c7128c 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ #define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__MANAGER_HPP_ -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "behavior_path_side_shift_module/scene.hpp" #include diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp index 08c2345df5814..accba00b81439 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/scene.hpp @@ -15,14 +15,14 @@ #ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ #define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__SCENE_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.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 -#include #include +#include #include #include @@ -32,10 +32,10 @@ namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using nav_msgs::msg::OccupancyGrid; using tier4_planning_msgs::msg::LateralOffset; +using tier4_planning_msgs::msg::PathWithLaneId; class SideShiftModule : public SceneModuleInterface { diff --git a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp index 210dd4e8a8d2a..f53bd42363b55 100644 --- a/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp +++ b/planning/behavior_path_side_shift_module/include/behavior_path_side_shift_module/utils.hpp @@ -15,17 +15,17 @@ #ifndef BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ #define BEHAVIOR_PATH_SIDE_SHIFT_MODULE__UTILS_HPP_ -#include #include #include #include +#include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; +using tier4_planning_msgs::msg::PathWithLaneId; void setOrientation(PathWithLaneId * path); diff --git a/planning/behavior_path_side_shift_module/package.xml b/planning/behavior_path_side_shift_module/package.xml index 5c421b04c0e39..af2b3c121f090 100644 --- a/planning/behavior_path_side_shift_module/package.xml +++ b/planning/behavior_path_side_shift_module/package.xml @@ -18,9 +18,9 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_behavior_path_planner_common + autoware_planning_msgs behavior_path_planner - behavior_path_planner_common geometry_msgs motion_utils pluginlib diff --git a/planning/behavior_path_side_shift_module/src/scene.cpp b/planning/behavior_path_side_shift_module/src/scene.cpp index 36321050da1ad..c2b239b5dd4fa 100644 --- a/planning/behavior_path_side_shift_module/src/scene.cpp +++ b/planning/behavior_path_side_shift_module/src/scene.cpp @@ -14,10 +14,10 @@ #include "behavior_path_side_shift_module/scene.hpp" -#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_side_shift_module/utils.hpp" #include diff --git a/planning/behavior_path_side_shift_module/src/utils.cpp b/planning/behavior_path_side_shift_module/src/utils.cpp index a674d57597bc7..888a2e9e24d9f 100644 --- a/planning/behavior_path_side_shift_module/src/utils.cpp +++ b/planning/behavior_path_side_shift_module/src/utils.cpp @@ -14,7 +14,7 @@ #include "behavior_path_side_shift_module/utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/behavior_path_start_planner_module/README.md b/planning/behavior_path_start_planner_module/README.md index ab2f0460b3fcb..71aa0a1abe704 100644 --- a/planning/behavior_path_start_planner_module/README.md +++ b/planning/behavior_path_start_planner_module/README.md @@ -250,7 +250,7 @@ This ordering is beneficial when the priority is to minimize the backward distan ### 2. Collision detection with dynamic obstacles -- **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) +- **Applying RSS in Dynamic Collision Detection**: Collision detection is based on the RSS (Responsibility-Sensitive Safety) model to evaluate if a safe distance is maintained. See [safety check feature explanation](../autoware_behavior_path_planner_common/docs/behavior_path_planner_safety_check.md) - **Collision check performed range**: Safety checks for collisions with dynamic objects are conducted within the defined boundaries between the start and end points of each maneuver, ensuring the ego vehicle does not impede or hinder the progress of dynamic objects that come from behind it. diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp index 04248ee7bd5fb..d8b94bb33d00b 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp @@ -16,8 +16,8 @@ #ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ #define BEHAVIOR_PATH_START_PLANNER_MODULE__DATA_STRUCTS_HPP_ -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -29,7 +29,7 @@ namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp index 75e55d6eab410..561ef337fa68c 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/freespace_pull_out.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 138aaaddd6981..ccf9e933f9383 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -15,13 +15,13 @@ #ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ #define BEHAVIOR_PATH_START_PLANNER_MODULE__GEOMETRIC_PULL_OUT_HPP_ -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/pull_out_planner_base.hpp" #include -#include +#include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp index 3fb64baaa2bba..ac752918a0e32 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/manager.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ #define BEHAVIOR_PATH_START_PLANNER_MODULE__MANAGER_HPP_ -#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "behavior_path_start_planner_module/start_planner_module.hpp" #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp index 68fcb25cac88e..f7f443c297c61 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_path.hpp @@ -15,16 +15,16 @@ #ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PATH_HPP_ -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include +#include #include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; struct PullOutPath { std::vector partial_paths{}; diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp index 9a2749759d03e..f2c437883b8a1 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/pull_out_planner_base.hpp @@ -15,22 +15,22 @@ #ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ #define BEHAVIOR_PATH_START_PLANNER_MODULE__PULL_OUT_PLANNER_BASE_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/util.hpp" -#include #include +#include #include namespace behavior_path_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using tier4_autoware_utils::LinearRing2d; +using tier4_planning_msgs::msg::PathWithLaneId; enum class PlannerType { NONE = 0, diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index f831eeda9778d..3bded7ee99fbb 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index ea5afb133d57f..c8e74009d267a 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -15,12 +15,12 @@ #ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ #define BEHAVIOR_PATH_START_PLANNER_MODULE__START_PLANNER_MODULE_HPP_ -#include "behavior_path_planner_common/interface/scene_module_interface.hpp" -#include "behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" -#include "behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/common_module_data.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_start_planner_module/data_structs.hpp" #include "behavior_path_start_planner_module/freespace_pull_out.hpp" #include "behavior_path_start_planner_module/geometric_pull_out.hpp" @@ -31,7 +31,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp index 5d55ce1519217..9185873cfd770 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/util.hpp @@ -15,19 +15,19 @@ #ifndef BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ #define BEHAVIOR_PATH_START_PLANNER_MODULE__UTIL_HPP_ -#include "behavior_path_planner_common/data_manager.hpp" -#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "autoware_behavior_path_planner_common/data_manager.hpp" +#include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "behavior_path_start_planner_module/pull_out_path.hpp" #include -#include -#include -#include +#include +#include #include #include +#include #include @@ -36,13 +36,13 @@ namespace behavior_path_planner::start_planner_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::RouteHandler; +using tier4_planning_msgs::msg::PathWithLaneId; PathWithLaneId getBackwardPath( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_start_planner_module/package.xml b/planning/behavior_path_start_planner_module/package.xml index f897188d60444..23c57b6d6024a 100644 --- a/planning/behavior_path_start_planner_module/package.xml +++ b/planning/behavior_path_start_planner_module/package.xml @@ -20,8 +20,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_path_planner_common behavior_path_planner - behavior_path_planner_common motion_utils pluginlib rclcpp diff --git a/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp b/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp index dd068c3c26cf8..d29047d23fffd 100644 --- a/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/freespace_pull_out.cpp @@ -14,9 +14,9 @@ #include "behavior_path_start_planner_module/freespace_pull_out.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_start_planner_module/util.hpp" #include diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index 0f7587fdff476..18d02ade489e8 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -14,9 +14,9 @@ #include "behavior_path_start_planner_module/geometric_pull_out.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_start_planner_module/util.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 592f57ed97dcf..298510126a4c3 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -14,10 +14,10 @@ #include "behavior_path_start_planner_module/shift_pull_out.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 7e1f38f30c378..82a4709c6b5be 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -14,10 +14,10 @@ #include "behavior_path_start_planner_module/start_planner_module.hpp" -#include "behavior_path_planner_common/utils/parking_departure/utils.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/parking_departure/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" #include "behavior_path_start_planner_module/debug.hpp" #include "behavior_path_start_planner_module/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" diff --git a/planning/behavior_path_start_planner_module/src/util.cpp b/planning/behavior_path_start_planner_module/src/util.cpp index 4ed499826dbd1..b1bcf59db86cb 100644 --- a/planning/behavior_path_start_planner_module/src/util.cpp +++ b/planning/behavior_path_start_planner_module/src/util.cpp @@ -14,9 +14,9 @@ #include "behavior_path_start_planner_module/util.hpp" -#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner_common/utils/utils.hpp" +#include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "autoware_behavior_path_planner_common/utils/path_utils.hpp" +#include "autoware_behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index 1148379c26f42..ebf760405a256 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp index d3e439b3663f8..6e4135c50b253 100644 --- a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp @@ -24,7 +24,7 @@ namespace behavior_velocity_planner */ template void BlindSpotModule::setRTCStatusByDecision( - const T &, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const T &, const tier4_planning_msgs::msg::PathWithLaneId & path) { static_assert("Unsupported type passed to setRTCStatus"); return; @@ -33,7 +33,7 @@ void BlindSpotModule::setRTCStatusByDecision( template void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const T & decision, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { static_assert("Unsupported type passed to reactRTCApprovalByDecision"); @@ -45,7 +45,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( template <> void BlindSpotModule::setRTCStatusByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) { return; } @@ -53,7 +53,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const InternalError & decision, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { return; @@ -65,7 +65,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( template <> void BlindSpotModule::setRTCStatusByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path) { return; } @@ -73,7 +73,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( [[maybe_unused]] const OverPassJudge & decision, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) { return; @@ -84,7 +84,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( */ template <> void BlindSpotModule::setRTCStatusByDecision( - const Unsafe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const Unsafe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) { setSafe(false); const auto & current_pose = planner_data_->current_odometry->pose; @@ -95,7 +95,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Unsafe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const Unsafe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { if (!isActivated()) { @@ -120,7 +120,7 @@ void BlindSpotModule::reactRTCApprovalByDecision( */ template <> void BlindSpotModule::setRTCStatusByDecision( - const Safe & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const Safe & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) { setSafe(true); const auto & current_pose = planner_data_->current_odometry->pose; @@ -131,8 +131,7 @@ void BlindSpotModule::setRTCStatusByDecision( template <> void BlindSpotModule::reactRTCApprovalByDecision( - const Safe & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, - StopReason * stop_reason) + const Safe & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { if (!isActivated()) { constexpr double stop_vel = 0.0; diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index 07742a1217ab3..f1b0a56018d9b 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -52,8 +52,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); } -void BlindSpotModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -86,7 +85,7 @@ void BlindSpotModuleManager::launchNewModules( std::function &)> BlindSpotModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp index 9aeaa0abfc4b7..7d6f936cc9d5b 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC private: BlindSpotModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class BlindSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 6c7c12b556581..14ebe70216d5a 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -153,7 +153,7 @@ template VisitorSwitch(Ts...) -> VisitorSwitch; void BlindSpotModule::setRTCStatus( - const BlindSpotDecision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path) { std::visit( VisitorSwitch{[&](const auto & sub_decision) { setRTCStatusByDecision(sub_decision, path); }}, @@ -185,7 +185,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto } static bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const lanelet::Id id) { for (const auto & pid : p.lane_ids) { if (pid == id) { @@ -196,7 +196,7 @@ static bool hasLaneIds( } static std::optional> findLaneIdInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) + const tier4_planning_msgs::msg::PathWithLaneId & p, const lanelet::Id id) { bool found = false; size_t start = 0; @@ -223,7 +223,7 @@ static std::optional> findLaneIdInterval( } std::optional BlindSpotModule::generateInterpolatedPathInfo( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const + const tier4_planning_msgs::msg::PathWithLaneId & input_path) const { constexpr double ds = 0.2; InterpolatedPathInfo interpolated_path_info; @@ -276,8 +276,7 @@ static std::optional getFirstPointIntersectsLineByFootprint( } static std::optional getDuplicatedPointIdx( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & point) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { for (size_t i = 0; i < path.points.size(); i++) { const auto & p = path.points.at(i).point.pose.position; @@ -292,8 +291,7 @@ static std::optional getDuplicatedPointIdx( } static std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); @@ -306,8 +304,7 @@ static std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) size_t insert_idx = closest_idx; - autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point = - inout_path->points.at(closest_idx); + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -326,7 +323,7 @@ static std::optional insertPointIndex( std::optional> BlindSpotModule::generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) const + tier4_planning_msgs::msg::PathWithLaneId * path) const { // NOTE: this is optionally int for later subtraction const int margin_idx_dist = @@ -388,9 +385,9 @@ std::optional> BlindSpotModule::generateStopLine( return std::make_pair(stopline_idx_default_opt.value(), stopline_idx_critical_opt.value()); } -autoware_auto_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWithDuration( +autoware_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictPathWithDuration( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & object_original, + const autoware_perception_msgs::msg::PredictedObject & object_original, const double time_thr) const { auto object = object_original; @@ -413,7 +410,7 @@ autoware_auto_perception_msgs::msg::PredictedObject BlindSpotModule::cutPredictP } std::optional BlindSpotModule::isOverPassJudge( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const geometry_msgs::msg::Pose & stop_point_pose) const { const auto & current_pose = planner_data_->current_odometry->pose; @@ -459,8 +456,7 @@ double BlindSpotModule::computeTimeToPassStopLine( planner_data_->current_velocity->twist.linear.x); } -std::optional -BlindSpotModule::isCollisionDetected( +std::optional BlindSpotModule::isCollisionDetected( const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, const double ego_time_to_reach_stop_line) @@ -596,7 +592,7 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) } lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const + const tier4_planning_msgs::msg::PathWithLaneId & path) const { /* get lanelet map */ const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); @@ -681,7 +677,7 @@ lanelet::ConstLanelets BlindSpotModule::generateBlindSpotLanelets( } std::optional BlindSpotModule::generateBlindSpotPolygons( - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[maybe_unused]] const tier4_planning_msgs::msg::PathWithLaneId & path, [[maybe_unused]] const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose) const { @@ -694,15 +690,15 @@ std::optional BlindSpotModule::generateBlindSpotPoly } bool BlindSpotModule::isTargetObjectType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { if ( object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE || + autoware_perception_msgs::msg::ObjectClassification::BICYCLE || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 709942794ec1e..b3bf90f5b91d1 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -20,10 +20,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -42,7 +42,7 @@ namespace behavior_velocity_planner struct InterpolatedPathInfo { /** the interpolated path */ - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ @@ -67,7 +67,7 @@ struct OverPassJudge struct Unsafe { const size_t stop_line_idx; - const std::optional collision_obstacle; + const std::optional collision_obstacle; }; struct Safe @@ -86,7 +86,7 @@ class BlindSpotModule : public SceneModuleInterface { std::optional virtual_wall_pose{std::nullopt}; std::optional detection_area; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_perception_msgs::msg::PredictedObjects conflicting_targets; }; public: @@ -135,21 +135,20 @@ class BlindSpotModule : public SceneModuleInterface BlindSpotDecision modifyPathVelocityDetail(PathWithLaneId * path, StopReason * stop_reason); // setSafe(), setDistance() void setRTCStatus( - const BlindSpotDecision & decision, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const BlindSpotDecision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); template void setRTCStatusByDecision( - const Decision & decision, const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const Decision & decision, const tier4_planning_msgs::msg::PathWithLaneId & path); // stop/GO void reactRTCApproval( const BlindSpotDecision & decision, PathWithLaneId * path, StopReason * stop_reason); template void reactRTCApprovalByDecision( - const Decision & decision, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const Decision & decision, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); std::optional generateInterpolatedPathInfo( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path) const; + const tier4_planning_msgs::msg::PathWithLaneId & input_path) const; std::optional getSiblingStraightLanelet( const std::shared_ptr planner_data) const; @@ -165,10 +164,10 @@ class BlindSpotModule : public SceneModuleInterface */ std::optional> generateStopLine( const InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; + tier4_planning_msgs::msg::PathWithLaneId * path) const; std::optional isOverPassJudge( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & stop_point_pose) const; double computeTimeToPassStopLine( @@ -185,7 +184,7 @@ class BlindSpotModule : public SceneModuleInterface * @param closest_idx closest path point index from ego car in path points * @return true when an object is detected in blind spot */ - std::optional isCollisionDetected( + std::optional isCollisionDetected( const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & stop_line_pose, const lanelet::CompoundPolygon3d & area, const double ego_time_to_reach_stop_line); @@ -203,7 +202,7 @@ class BlindSpotModule : public SceneModuleInterface const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; lanelet::ConstLanelets generateBlindSpotLanelets( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) const; + const tier4_planning_msgs::msg::PathWithLaneId & path) const; /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. @@ -213,7 +212,7 @@ class BlindSpotModule : public SceneModuleInterface * @return Blind spot polygons */ std::optional generateBlindSpotPolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const lanelet::ConstLanelets & blind_spot_lanelets, const geometry_msgs::msg::Pose & pose) const; @@ -222,17 +221,16 @@ class BlindSpotModule : public SceneModuleInterface * @param object Dynamic object * @return True when object belong to targeted classes */ - bool isTargetObjectType(const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + bool isTargetObjectType(const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. * @param objects_ptr target objects * @param time_thr time threshold to cut path */ - autoware_auto_perception_msgs::msg::PredictedObject cutPredictPathWithDuration( + autoware_perception_msgs::msg::PredictedObject cutPredictPathWithDuration( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & object, - const double time_thr) const; + const autoware_perception_msgs::msg::PredictedObject & object, const double time_thr) const; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index b82ea7887dc98..23222d2841bad 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -221,21 +221,21 @@ To inflate the masking behind objects, their footprint can be made bigger using ![stuck_vehicle_attention_range](docs/with_occlusion.svg){width=600} -| Parameter | Unit | Type | Description | -| ---------------------------------------------- | ----- | ----------- | ----------------------------------------------------------------------------------------------------------------------------------------------- | -| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | -| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | -| `slow_down_velocity` | [m/s] | double | slow down velocity | -| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | -| `min_size` | [m] | double | minimum size of an occlusion (square side size) | -| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | -| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | -| `ignore_with_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | -| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | -| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | -| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_auto_perception_msgs::msg::ObjectClassification` for all the labels) | -| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | -| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | +| Parameter | Unit | Type | Description | +| ---------------------------------------------- | ----- | ----------- | ------------------------------------------------------------------------------------------------------------------------------------------ | +| `enable` | [-] | bool | if true, ego will slow down around crosswalks that are occluded | +| `occluded_object_velocity` | [m/s] | double | assumed velocity of objects that may come out of the occluded space | +| `slow_down_velocity` | [m/s] | double | slow down velocity | +| `time_buffer` | [s] | double | consecutive time with/without an occlusion to add/remove the slowdown | +| `min_size` | [m] | double | minimum size of an occlusion (square side size) | +| `free_space_max` | [-] | double | maximum value of a free space cell in the occupancy grid | +| `occupied_min` | [-] | double | minimum value of an occupied cell in the occupancy grid | +| `ignore_with_traffic_light` | [-] | bool | if true, occlusions at crosswalks with traffic lights are ignored | +| `ignore_behind_predicted_objects` | [-] | bool | if true, occlusions behind predicted objects are ignored | +| `ignore_velocity_thresholds.default` | [m/s] | double | occlusions are only ignored behind objects with a higher or equal velocity | +| `ignore_velocity_thresholds.custom_labels` | [-] | string list | labels for which to define a non-default velocity threshold (see `autoware_perception_msgs::msg::ObjectClassification` for all the labels) | +| `ignore_velocity_thresholds.custom_thresholds` | [-] | double list | velocities of the custom labels | +| `extra_predicted_objects_size` | [m] | double | extra size added to the objects for masking the occlusions | ### Others diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 67b923a8d4cd8..e0d25c56b1610 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -83,6 +83,6 @@ ignore_behind_predicted_objects: true # [-] if true, occlusions behind predicted objects are ignored ignore_velocity_thresholds: default: 0.5 # [m/s] occlusions are only ignored behind objects with a higher or equal velocity - custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels) + custom_labels: ["PEDESTRIAN"] # labels for which to define a non-default velocity threshold (see autoware_perception_msgs::msg::ObjectClassification for all the labels) custom_thresholds: [0.0] # velocities of the custom labels extra_predicted_objects_size: 0.5 # [m] extra size added to the objects for masking the occlusions diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index d287d7e705542..690eca196d536 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -35,14 +35,14 @@ #include #include -#include +#include #include namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; @@ -87,14 +87,12 @@ struct DebugData std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs); bool checkRegulatoryElementExistence(const lanelet::LaneletMapPtr & lanelet_map_ptr); diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 90b5592bb3ddb..2fd5e2bb514c3 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,9 +21,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 5110ff9993a62..fe4282893674c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -143,19 +143,17 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_behind_predicted_objects"); cp.occlusion_ignore_velocity_thresholds.resize( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1, getOrDeclareParameter(node, ns + ".occlusion.ignore_velocity_thresholds.default")); const auto get_label = [](const std::string & s) { - if (s == "CAR") return autoware_auto_perception_msgs::msg::ObjectClassification::CAR; - if (s == "TRUCK") return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK; - if (s == "BUS") return autoware_auto_perception_msgs::msg::ObjectClassification::BUS; - if (s == "TRAILER") return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER; - if (s == "MOTORCYCLE") - return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE; - if (s == "BICYCLE") return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; - if (s == "PEDESTRIAN") - return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + if (s == "CAR") return autoware_perception_msgs::msg::ObjectClassification::CAR; + if (s == "TRUCK") return autoware_perception_msgs::msg::ObjectClassification::TRUCK; + if (s == "BUS") return autoware_perception_msgs::msg::ObjectClassification::BUS; + if (s == "TRAILER") return autoware_perception_msgs::msg::ObjectClassification::TRAILER; + if (s == "MOTORCYCLE") return autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE; + if (s == "BICYCLE") return autoware_perception_msgs::msg::ObjectClassification::BICYCLE; + if (s == "PEDESTRIAN") return autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; }; const auto custom_labels = getOrDeclareParameter>( node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels"); diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index 38bb63c121699..ba7e75193deab 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -23,8 +23,8 @@ #include #include -#include #include +#include #include #include @@ -35,7 +35,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC { diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index 2455b36d5883f..b6adb32f642eb 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -84,11 +84,11 @@ std::vector calculate_detection_areas( return detection_areas; } -std::vector select_and_inflate_objects( - const std::vector & objects, +std::vector select_and_inflate_objects( + const std::vector & objects, const std::vector velocity_thresholds, const double inflate_size) { - std::vector selected_objects; + std::vector selected_objects; for (const auto & o : objects) { const auto vel_threshold = velocity_thresholds[o.classification.front().label]; if (o.kinematics.initial_twist_with_covariance.twist.linear.x >= vel_threshold) { @@ -103,7 +103,7 @@ std::vector select_and_infl void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, - const std::vector & objects) + const std::vector & objects) { const auto angle_cmp = [&](const auto & p1, const auto & p2) { const auto d1 = p1 - grid_map.getPosition(); @@ -139,7 +139,7 @@ bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, - const std::vector & dynamic_objects, + const std::vector & dynamic_objects, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index a76fdeb549b88..4aab9d3bfc888 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -61,7 +61,7 @@ bool is_crosswalk_occluded( const lanelet::ConstLanelet & crosswalk_lanelet, const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, - const std::vector & dynamic_objects, + const std::vector & dynamic_objects, const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief calculate the distance away from the crosswalk that should be checked for occlusions @@ -78,8 +78,8 @@ double calculate_detection_range( /// @param skip_pedestrians if true, pedestrians are not selected regardless of their velocities /// @param inflate_size [m] size by which the shape of the selected objects are inflated /// @return selected and inflated objects -std::vector select_and_inflate_objects( - const std::vector & objects, +std::vector select_and_inflate_objects( + const std::vector & objects, const double velocity_threshold, const bool skip_pedestrians, const double inflate_size); /// @brief clear occlusions behind the given objects @@ -88,7 +88,7 @@ std::vector select_and_infl /// @param objects objects void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, - const std::vector & objects); + const std::vector & objects); } // namespace behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 721af25e75ece..191eea8feabed 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -674,7 +674,7 @@ std::optional CrosswalkModule::findEgoPassageDirectionAlongPath( } std::optional CrosswalkModule::findObjectPassageDirectionAlongVehicleLane( - const autoware_auto_perception_msgs::msg::PredictedPath & path) const + const autoware_perception_msgs::msg::PredictedPath & path) const { using tier4_autoware_utils::Segment2d; @@ -1127,8 +1127,7 @@ bool CrosswalkModule::isRedSignalForPedestrians() const } for (const auto & element : lights) { - if ( - element.color == TrafficSignalElement::RED && element.shape == TrafficSignalElement::CIRCLE) + if (element.color == TrafficLightElement::RED && element.shape == TrafficLightElement::CIRCLE) return true; } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index faba8730aa6d9..107a904dd076b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -49,15 +49,15 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrafficLightElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; +using tier4_planning_msgs::msg::PathWithLaneId; namespace { @@ -353,7 +353,7 @@ class CrosswalkModule : public SceneModuleInterface std::optional findEgoPassageDirectionAlongPath( const PathWithLaneId & sparse_resample_path) const; std::optional findObjectPassageDirectionAlongVehicleLane( - const autoware_auto_perception_msgs::msg::PredictedPath & path) const; + const autoware_perception_msgs::msg::PredictedPath & path) const; std::optional getCollisionPoint( const PathWithLaneId & ego_path, const PredictedObject & object, diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index ee1b6347c9e73..d276ae95e06b6 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -55,8 +55,7 @@ using tier4_autoware_utils::Point2d; std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::vector> crosswalks; @@ -90,8 +89,7 @@ std::vector> getCrosswalksOnPath( std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { std::set crosswalk_id_set; diff --git a/planning/behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_detection_area_module/package.xml index 66b8a62e83a2c..6bcca43edc0cb 100644 --- a/planning/behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_detection_area_module/package.xml @@ -17,7 +17,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 8e9a6b6a58a96..feb5bf6bb50ef 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -51,7 +51,7 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) } void DetectionAreaModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & detection_area_with_lane_id : planning_utils::getRegElemMapOnPath( @@ -74,7 +74,7 @@ void DetectionAreaModuleManager::launchNewModules( std::function &)> DetectionAreaModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto detection_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp index 10fca7182d09a..71cfa0a5eef96 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: DetectionAreaModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DetectionAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_detection_area_module/src/scene.hpp index c3f3f81cd6dfd..f135c3b2660cb 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.hpp @@ -36,7 +36,7 @@ namespace behavior_velocity_planner using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class DetectionAreaModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml index 7e9599c49bc2d..84d01d04a09a4 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs libboost-dev diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp index d57789d574ad9..8497369917232 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -62,7 +62,7 @@ std::optional find_closest_collision_point( std::vector find_collisions( const EgoData & ego_data, - const std::vector & objects, + const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & object_forward_footprints) { std::vector collisions; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp index b65baaeb46aa3..27a48afa032b1 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -38,7 +38,7 @@ std::optional find_closest_collision_point( /// @return the point of earliest collision along the ego path std::vector find_collisions( const EgoData & ego_data, - const std::vector & objects, + const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints); } // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp index b134ad3c39628..7397f63ca079c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index 6a0c61963ac81..a58e4e9b88c75 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -29,7 +29,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop { tier4_autoware_utils::MultiPolygon2d make_forward_footprints( - const std::vector & obstacles, + const std::vector & obstacles, const PlannerParam & params, const double hysteresis) { tier4_autoware_utils::MultiPolygon2d forward_footprints; @@ -41,7 +41,7 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( } tier4_autoware_utils::Polygon2d make_forward_footprint( - const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis) { const auto & shape = obstacle.shape.dimensions; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index c28e89ac6c9f6..0050a4e2c9259 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -30,7 +30,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param [in] hysteresis [m] extra lateral distance to add to the footprints /// @return forward footprint of the obstacle tier4_autoware_utils::MultiPolygon2d make_forward_footprints( - const std::vector & obstacles, + const std::vector & obstacles, const PlannerParam & params, const double hysteresis); /// @brief create the footprint of the given obstacle and its projection over a fixed time horizon /// @param [in] obstacle obstacle @@ -38,7 +38,7 @@ tier4_autoware_utils::MultiPolygon2d make_forward_footprints( /// @param [in] hysteresis [m] extra lateral distance to add to the footprint /// @return forward footprint of the obstacle tier4_autoware_utils::Polygon2d make_forward_footprint( - const autoware_auto_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, + const autoware_perception_msgs::msg::PredictedObject & obstacle, const PlannerParam & params, const double hysteresis); /// @brief project a footprint to the given pose /// @param [in] base_footprint footprint to project diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index 92004b4b9f249..63f1f3025f94a 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -50,7 +50,7 @@ DynamicObstacleStopModuleManager::DynamicObstacleStopModuleManager(rclcpp::Node } void DynamicObstacleStopModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -62,7 +62,7 @@ void DynamicObstacleStopModuleManager::launchNewModules( std::function &)> DynamicObstacleStopModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp index e461cc9c16445..eb7bf4c6dbc98 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -43,10 +43,10 @@ class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class DynamicObstacleStopModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp index c95db485781f9..205e110298f67 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp @@ -30,20 +30,19 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param params parameters /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects -std::vector filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, +std::vector filter_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis) { - std::vector filtered_objects; + std::vector filtered_objects; const auto is_vehicle = [](const auto & o) { return std::find_if(o.classification.begin(), o.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || - c.label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - c.label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE; + return c.label == autoware_perception_msgs::msg::ObjectClassification::CAR || + c.label == autoware_perception_msgs::msg::ObjectClassification::BUS || + c.label == autoware_perception_msgs::msg::ObjectClassification::TRUCK || + c.label == autoware_perception_msgs::msg::ObjectClassification::TRAILER || + c.label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + c.label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE; }) != o.classification.end(); }; const auto is_in_range = [&](const auto & o) { diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index 22857f6db1bda..5daa0cda44203 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include @@ -30,8 +30,8 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop /// @param params parameters /// @param hysteresis [m] extra distance threshold used for filtering /// @return filtered predicted objects -std::vector filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, +std::vector filter_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); } // namespace behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp index 55eaf079afee6..1df8a1ed6a4a3 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp @@ -23,7 +23,7 @@ namespace behavior_velocity_planner::dynamic_obstacle_stop void update_object_map( ObjectStopDecisionMap & object_map, const std::vector & collisions, const rclcpp::Time & now, - const std::vector & path_points, + const std::vector & path_points, const PlannerParam & params) { for (auto & [object, decision] : object_map) decision.collision_detected = false; diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp index cddff65da36d3..5756c8589fb73 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp @@ -61,7 +61,7 @@ using ObjectStopDecisionMap = std::unordered_map & collisions, const rclcpp::Time & now, - const std::vector & path_points, + const std::vector & path_points, const PlannerParam & params); /// @brief find the earliest collision requiring a stop along the ego path diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp index 21fca83565ee6..79911c40a195d 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp index 8575e4729d7a3..532d770608dd3 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -18,9 +18,9 @@ #include #include -#include -#include +#include #include +#include #include @@ -52,7 +52,7 @@ struct PlannerParam struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double longitudinal_offset_to_first_path_idx; // [m] geometry_msgs::msg::Pose pose; diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0bed7d32f412f..3694a395b3a53 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -18,9 +18,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common fmt geometry_msgs diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index bd2ad1935406b..252d7c2a9f61e 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -406,8 +406,8 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } if (debug_data_.traffic_light_observation) { - const auto GREEN = autoware_perception_msgs::msg::TrafficSignalElement::GREEN; - const auto YELLOW = autoware_perception_msgs::msg::TrafficSignalElement::AMBER; + const auto GREEN = autoware_perception_msgs::msg::TrafficLightElement::GREEN; + const auto YELLOW = autoware_perception_msgs::msg::TrafficLightElement::AMBER; const auto [ego, tl_point, id, color] = debug_data_.traffic_light_observation.value(); geometry_msgs::msg::Point tl_point_point; diff --git a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp index 9002c88354d68..b6ae84aa8488b 100644 --- a/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -15,7 +15,7 @@ #ifndef INTERPOLATED_PATH_INFO_HPP_ #define INTERPOLATED_PATH_INFO_HPP_ -#include +#include #include @@ -32,7 +32,7 @@ namespace behavior_velocity_planner::intersection struct InterpolatedPathInfo { /** the interpolated path */ - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; /** discretization interval of interpolation */ double ds{0.0}; /** the intersection lanelet id */ diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 424fb6841eca9..ac32ad553d2b7 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -297,12 +297,12 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) decision_state_pub_ = node.create_publisher("~/debug/intersection/decision_state", 1); - tl_observation_pub_ = node.create_publisher( + tl_observation_pub_ = node.create_publisher( "~/debug/intersection_traffic_signal", 1); } void IntersectionModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -356,7 +356,7 @@ void IntersectionModuleManager::launchNewModules( std::function &)> IntersectionModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); @@ -446,7 +446,7 @@ void IntersectionModuleManager::setActivation() } void IntersectionModuleManager::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -483,7 +483,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node } void MergeFromPrivateModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto routing_graph = planner_data_->route_handler_->getRoutingGraphPtr(); const auto lanelet_map = planner_data_->route_handler_->getLaneletMapPtr(); @@ -550,7 +550,7 @@ void MergeFromPrivateModuleManager::launchNewModules( std::function &)> MergeFromPrivateModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_intersection_module/src/manager.hpp b/planning/behavior_velocity_intersection_module/src/manager.hpp index 46281df2f29c7..7bbc8d51bbe9e 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/manager.hpp @@ -23,9 +23,9 @@ #include #include -#include #include #include +#include #include #include @@ -46,10 +46,10 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC // additional for INTERSECTION_OCCLUSION RTCInterface occlusion_rtc_interface_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; @@ -57,10 +57,11 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC void sendRTC(const Time & stamp) override; void setActivation() override; /* called from SceneModuleInterface::updateSceneModuleInstances */ - void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; rclcpp::Publisher::SharedPtr decision_state_pub_; - rclcpp::Publisher::SharedPtr tl_observation_pub_; + rclcpp::Publisher::SharedPtr + tl_observation_pub_; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -73,10 +74,10 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface private: MergeFromPrivateRoadModule::PlannerParam merge_from_private_area_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool hasSameParentLaneletAndTurnDirectionWithRegistered(const lanelet::ConstLanelet & lane) const; }; diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.cpp b/planning/behavior_velocity_intersection_module/src/object_manager.cpp index 420031e4df1cf..d2c905673cee9 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.cpp @@ -40,7 +40,7 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) tier4_autoware_utils::Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) + const autoware_perception_msgs::msg::Shape & shape) { namespace bg = boost::geometry; const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); @@ -73,7 +73,7 @@ ObjectInfo::ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid) : uuid_st } void ObjectInfo::initialize( - const autoware_auto_perception_msgs::msg::PredictedObject & object, + const autoware_perception_msgs::msg::PredictedObject & object, std::optional attention_lanelet_opt_, std::optional stopline_opt_) { @@ -250,9 +250,8 @@ std::vector> ObjectInfoManager::allObjects() const } std::optional findPassageInterval( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, - const lanelet::BasicPolygon2d & ego_lane_poly, + 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) { diff --git a/planning/behavior_velocity_intersection_module/src/object_manager.hpp b/planning/behavior_velocity_intersection_module/src/object_manager.hpp index 77e39637523a9..180496bd6b18d 100644 --- a/planning/behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/behavior_velocity_intersection_module/src/object_manager.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -104,7 +104,7 @@ class ObjectInfo public: explicit ObjectInfo(const unique_identifier_msgs::msg::UUID & uuid); - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object() const + const autoware_perception_msgs::msg::PredictedObject & predicted_object() const { return predicted_object_; }; @@ -126,7 +126,7 @@ class ObjectInfo * @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline */ void initialize( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, + const autoware_perception_msgs::msg::PredictedObject & predicted_object, std::optional attention_lanelet_opt, std::optional stopline_opt); @@ -193,7 +193,7 @@ class ObjectInfo const std::string uuid_str; private: - autoware_auto_perception_msgs::msg::PredictedObject predicted_object_; + autoware_perception_msgs::msg::PredictedObject predicted_object_; //! null if the object in intersection_area but not in attention_area std::optional attention_lanelet_opt{std::nullopt}; @@ -283,9 +283,8 @@ class ObjectInfoManager * @brief return the CollisionInterval struct if the predicted path collides ego path geometrically */ std::optional findPassageInterval( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, - const lanelet::BasicPolygon2d & ego_lane_poly, + 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); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 08e2d38a5905f..c27483ea1fa51 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -496,9 +496,8 @@ VisitorSwitch(Ts...) -> VisitorSwitch; template void prepareRTCByDecisionResult( - const T & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - bool * default_safety, double * default_distance, bool * occlusion_safety, - double * occlusion_distance) + const T & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + double * default_distance, bool * occlusion_safety, double * occlusion_distance) { static_assert("Unsupported type passed to prepareRTCByDecisionResult"); return; @@ -507,7 +506,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const intersection::InternalError & result, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[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) { @@ -517,7 +516,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( [[maybe_unused]] const intersection::OverPassJudge & result, - [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + [[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) { @@ -526,9 +525,9 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::StuckStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, - double * default_distance, bool * occlusion_safety, double * occlusion_distance) + const intersection::StuckStop & 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"), "StuckStop"); const auto closest_idx = result.closest_idx; @@ -547,7 +546,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::YieldStuckStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -562,7 +561,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::NonOccludedCollisionStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -581,7 +580,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::FirstWaitBeforeOcclusion & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -600,7 +599,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::PeekingTowardOcclusion & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -619,7 +618,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::OccludedAbsenceTrafficLight & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -636,7 +635,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::OccludedCollisionStop & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -654,7 +653,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( - const intersection::Safe & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const intersection::Safe & result, const tier4_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { @@ -674,7 +673,7 @@ void prepareRTCByDecisionResult( template <> void prepareRTCByDecisionResult( const intersection::FullyPrioritized & result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, + 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"); @@ -692,7 +691,7 @@ void prepareRTCByDecisionResult( void IntersectionModule::prepareRTCStatus( const intersection::DecisionResult & decision_result, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { bool default_safety = true; double default_distance = std::numeric_limits::lowest(); @@ -713,7 +712,7 @@ template void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const T & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { static_assert("Unsupported type passed to reactRTCByDecisionResult"); @@ -727,7 +726,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const intersection::InternalError & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) @@ -742,7 +741,7 @@ void reactRTCApprovalByDecisionResult( [[maybe_unused]] const intersection::OverPassJudge & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, [[maybe_unused]] const double baselink2front, - [[maybe_unused]] autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason, [[maybe_unused]] VelocityFactorInterface * velocity_factor, [[maybe_unused]] IntersectionModule::DebugData * debug_data) @@ -755,7 +754,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::StuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -802,7 +801,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::YieldStuckStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -835,7 +834,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::NonOccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -879,7 +878,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::FirstWaitBeforeOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -930,7 +929,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::PeekingTowardOcclusion & decision_result, const IntersectionModule::PlannerParam & planner_param, const double baselink2front, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { RCLCPP_DEBUG( @@ -986,7 +985,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::OccludedCollisionStop & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1034,7 +1033,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::OccludedAbsenceTrafficLight & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1088,7 +1087,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::Safe & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1131,7 +1130,7 @@ void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, const intersection::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, - const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, + const double baselink2front, tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, IntersectionModule::DebugData * debug_data) { @@ -1172,7 +1171,7 @@ void reactRTCApprovalByDecisionResult( void IntersectionModule::reactRTCApproval( const intersection::DecisionResult & decision_result, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason) { const double baselink2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; std::visit( @@ -1187,7 +1186,7 @@ void IntersectionModule::reactRTCApproval( bool IntersectionModule::isGreenSolidOn() const { - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; if (!last_tl_valid_observation_) { return false; @@ -1205,7 +1204,7 @@ bool IntersectionModule::isGreenSolidOn() const IntersectionModule::TrafficPrioritizedLevel IntersectionModule::getTrafficPrioritizedLevel() const { - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; auto corresponding_arrow = [&](const TrafficSignalElement & element) { if (turn_direction_ == "straight" && element.shape == TrafficSignalElement::UP_ARROW) { @@ -1293,7 +1292,7 @@ void IntersectionModule::updateTrafficSignalObservation() } IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines) { const auto & current_pose = planner_data_->current_odometry->pose; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8c8874156f07b..b86fd77491f54 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -27,8 +27,8 @@ #include #include -#include #include +#include #include #include @@ -225,13 +225,13 @@ class IntersectionModule : public SceneModuleInterface std::optional> yield_stuck_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; - autoware_auto_perception_msgs::msg::PredictedObjects unsafe_targets; - autoware_auto_perception_msgs::msg::PredictedObjects misjudge_targets; - autoware_auto_perception_msgs::msg::PredictedObjects too_late_detect_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects parked_targets; + autoware_perception_msgs::msg::PredictedObjects safe_under_traffic_control_targets; + autoware_perception_msgs::msg::PredictedObjects unsafe_targets; + autoware_perception_msgs::msg::PredictedObjects misjudge_targets; + autoware_perception_msgs::msg::PredictedObjects too_late_detect_targets; + autoware_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_perception_msgs::msg::PredictedObjects parked_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; @@ -513,15 +513,14 @@ class IntersectionModule : public SceneModuleInterface * @brief set RTC value according to calculated DecisionResult */ void prepareRTCStatus( - const intersection::DecisionResult &, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const intersection::DecisionResult &, const tier4_planning_msgs::msg::PathWithLaneId & path); /** * @brief act based on current RTC approval */ void reactRTCApproval( const intersection::DecisionResult & decision_result, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); + tier4_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason); /** @}*/ private: @@ -569,7 +568,7 @@ class IntersectionModule : public SceneModuleInterface const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const; + tier4_planning_msgs::msg::PathWithLaneId * original_path) const; /** * @brief generate IntersectionLanelets @@ -639,15 +638,15 @@ class IntersectionModule : public SceneModuleInterface * intersection_lanelets.first_conflicting_lane(). They are ensured in prepareIntersectionData() */ std::optional isStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::IntersectionStopLines & intersection_stoplines, const intersection::PathLanelets & path_lanelets) const; bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; bool isTargetYieldStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief check stuck @@ -669,7 +668,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.default_stopline, intersection_stoplines.first_attention_stopline */ std::optional isYieldStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, const intersection::IntersectionStopLines & intersection_stoplines) const; @@ -726,7 +725,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.occlusion_stopline */ PassJudgeStatus isOverPassJudgeLinesStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_occlusion_state, const intersection::IntersectionStopLines & intersection_stoplines); /** @} */ @@ -739,7 +738,7 @@ class IntersectionModule : public SceneModuleInterface * @{ */ bool isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief find the objects on attention_area/intersection_area and update positional information @@ -759,7 +758,7 @@ class IntersectionModule : public SceneModuleInterface void cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, - autoware_auto_perception_msgs::msg::PredictedPath * predicted_path) const; + autoware_perception_msgs::msg::PredictedPath * predicted_path) const; /** * @brief check if there are any objects around the stoplines on the attention areas when ego @@ -790,7 +789,7 @@ class IntersectionModule : public SceneModuleInterface * situation */ std::string generateEgoRiskEvasiveDiagnosis( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const TimeDistanceArray & ego_time_distance_array, const std::vector< std::pair>> & @@ -818,7 +817,7 @@ class IntersectionModule : public SceneModuleInterface * intersection_stoplines.first_attention_stopline */ TimeDistanceArray calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::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/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 8388bc15492a3..131081c5e8ca0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -35,31 +35,30 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; bool IntersectionModule::isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.collision_detection.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; @@ -208,8 +207,8 @@ void IntersectionModule::updateObjectInfoManagerCollision( bool safe_under_traffic_control = false; const auto label = predicted_object.classification.at(0).label; const auto expected_deceleration = - (label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || - label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) + (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE) ? planner_param_.collision_detection.ignore_on_amber_traffic_light .object_expected_deceleration.bike : planner_param_.collision_detection.ignore_on_amber_traffic_light @@ -232,7 +231,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( // check the PredictedPath in the ascending order of its confidence to save the safe/unsafe // CollisionKnowledge for most probable path // ========================================================================================== - std::list sorted_predicted_paths; + std::list sorted_predicted_paths; for (unsigned i = 0; i < predicted_object.kinematics.predicted_paths.size(); ++i) { sorted_predicted_paths.push_back(&predicted_object.kinematics.predicted_paths.at(i)); } @@ -400,7 +399,7 @@ void IntersectionModule::updateObjectInfoManagerCollision( void IntersectionModule::cutPredictPathWithinDuration( const builtin_interfaces::msg::Time & object_stamp, const double time_thr, - autoware_auto_perception_msgs::msg::PredictedPath * path) const + autoware_perception_msgs::msg::PredictedPath * path) const { const rclcpp::Time current_time = clock_->now(); const auto original_path = path->path; @@ -597,7 +596,7 @@ std::string IntersectionModule::generateDetectionBlameDiagnosis( } std::string IntersectionModule::generateEgoRiskEvasiveDiagnosis( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const IntersectionModule::TimeDistanceArray & ego_time_distance_array, const std::vector>> & @@ -811,7 +810,7 @@ std::optional IntersectionModule::checkAngleForTargetLanelets( } IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, + const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized, const intersection::IntersectionStopLines & intersection_stoplines, tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index 97d05aef26137..9ea5360c3a176 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -65,7 +65,7 @@ lanelet::ConstLanelets getPrevLanelets( // end inclusive lanelet::ConstLanelet generatePathLanelet( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width, const double interval) { lanelet::Points3d lefts; @@ -96,7 +96,7 @@ lanelet::ConstLanelet generatePathLanelet( } std::optional> getFirstPointInsidePolygons( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const std::vector & polygons, const bool search_forward = true) { @@ -356,7 +356,7 @@ IntersectionModule::generateIntersectionStopLines( const lanelet::ConstLanelet & first_attention_lane, const std::optional & second_attention_area_opt, const intersection::InterpolatedPathInfo & interpolated_path_info, - autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) const + tier4_planning_msgs::msg::PathWithLaneId * original_path) const { const bool use_stuck_stopline = planner_param_.stuck_vehicle.use_stuck_stopline; const double stopline_margin = planner_param_.common.default_stopline_margin; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index b26f960ec28f9..498a902c032db 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -119,7 +119,7 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; std::optional IntersectionModule::isStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::IntersectionStopLines & intersection_stoplines, const intersection::PathLanelets & path_lanelets) const { @@ -171,62 +171,60 @@ std::optional IntersectionModule::isStuckStatus( } bool IntersectionModule::isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.stuck_vehicle.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; } bool IntersectionModule::isTargetYieldStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { const auto label = object.classification.at(0).label; const auto & p = planner_param_.yield_stuck.target_type; - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::CAR && p.car) { + if (label == autoware_perception_msgs::msg::ObjectClassification::CAR && p.car) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BUS && p.bus) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BUS && p.bus) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRUCK && p.truck) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { + if (label == autoware_perception_msgs::msg::ObjectClassification::TRAILER && p.trailer) { return true; } - if ( - label == autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE && p.motorcycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { + if (label == autoware_perception_msgs::msg::ObjectClassification::BICYCLE && p.bicycle) { return true; } - if (label == autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { + if (label == autoware_perception_msgs::msg::ObjectClassification::UNKNOWN && p.unknown) { return true; } return false; @@ -309,7 +307,7 @@ bool IntersectionModule::checkStuckVehicleInIntersection( } std::optional IntersectionModule::isYieldStuckStatus( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const intersection::InterpolatedPathInfo & interpolated_path_info, const intersection::IntersectionStopLines & intersection_stoplines) const { diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index a44b99c97457d..0b783cf2a7ebd 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -19,10 +19,10 @@ #include #include -#include -#include -#include +#include +#include #include +#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index dfdfb01fb2234..9c492e7a64cde 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -45,8 +45,7 @@ namespace behavior_velocity_planner::util namespace bg = boost::geometry; static std::optional getDuplicatedPointIdx( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & point) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { for (size_t i = 0; i < path.points.size(); i++) { const auto & p = path.points.at(i).point.pose.position; @@ -61,8 +60,7 @@ static std::optional getDuplicatedPointIdx( } std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { const auto duplicate_idx_opt = getDuplicatedPointIdx(*inout_path, in_pose.position); @@ -75,8 +73,7 @@ std::optional insertPointIndex( // vector.insert(i) inserts element on the left side of v[i] // the velocity need to be zero order hold(from prior point) int insert_idx = closest_idx; - autoware_auto_planning_msgs::msg::PathPointWithLaneId inserted_point = - inout_path->points.at(closest_idx); + tier4_planning_msgs::msg::PathPointWithLaneId inserted_point = inout_path->points.at(closest_idx); if (planning_utils::isAheadOf(in_pose, inout_path->points.at(closest_idx).point.pose)) { ++insert_idx; } else { @@ -94,8 +91,7 @@ std::optional insertPointIndex( } bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, - const std::set & ids) + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) { for (const auto & pid : p.lane_ids) { if (ids.find(pid) != ids.end()) { @@ -106,7 +102,7 @@ bool hasLaneIds( } std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) + const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) { bool found = false; size_t start = 0; @@ -183,7 +179,7 @@ getFirstPointInsidePolygonsByFootprint( } std::optional getFirstPointInsidePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward) { @@ -316,7 +312,7 @@ mergeLaneletsByTopologicalSort( } bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { @@ -327,7 +323,7 @@ bool isOverTargetIndex( } bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx) { if (closest_idx == target_idx) { @@ -362,7 +358,7 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { intersection::InterpolatedPathInfo interpolated_path_info; @@ -378,7 +374,7 @@ std::optional generateInterpolatedPath( } geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( - const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state) { if (obj_state.initial_twist_with_covariance.twist.linear.x >= 0) { return obj_state.initial_pose_with_covariance.pose; diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index e37bb3ee88cc1..878253e6943a7 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -38,16 +38,14 @@ namespace behavior_velocity_planner::util * @return if insertion was successful return the inserted point index */ std::optional insertPointIndex( - const geometry_msgs::msg::Pose & in_pose, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, + const geometry_msgs::msg::Pose & in_pose, tier4_planning_msgs::msg::PathWithLaneId * inout_path, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold); /** * @brief check if a PathPointWithLaneId contains any of the given lane ids */ bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, - const std::set & ids); + const tier4_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); /** * @brief find the first contiguous interval of the path points that contains the specified lane ids @@ -55,7 +53,7 @@ bool hasLaneIds( * found, returns the pair (start-1, end) */ std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); + const tier4_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** * @brief return the index of the first point which is inside the given polygon @@ -63,7 +61,7 @@ std::optional> findLaneIdsInterval( * @param[in] search_forward flag for search direction */ std::optional getFirstPointInsidePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::pair lane_interval, const lanelet::CompoundPolygon3d & polygon, const bool search_forward = true); @@ -75,7 +73,7 @@ std::optional getFirstPointInsidePolygon( * @return true if ego is over the target_idx */ bool isOverTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); /** @@ -86,7 +84,7 @@ bool isOverTargetIndex( * @return true if ego is over the target_idx */ bool isBeforeTargetIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t closest_idx, const geometry_msgs::msg::Pose & current_pose, const size_t target_idx); std::optional getIntersectionArea( @@ -102,11 +100,11 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); */ std::optional generateInterpolatedPath( const lanelet::Id lane_id, const std::set & associative_lane_ids, - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( - const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state); /** * @brief this function sorts the set of lanelets topologically using topological sort and merges diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_no_drivable_lane_module/package.xml index 061cf8d367ba9..79eea68142cc8 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/package.xml @@ -19,7 +19,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index f1fa4c47e951b..0b7a8a8c28dd5 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -32,7 +32,7 @@ NoDrivableLaneModuleManager::NoDrivableLaneModuleManager(rclcpp::Node & node) } void NoDrivableLaneModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & ll : planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -56,7 +56,7 @@ void NoDrivableLaneModuleManager::launchNewModules( std::function &)> NoDrivableLaneModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lane_id_set = planning_utils::getLaneIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp index 2b3b80510c9e4..90455bd4b1c62 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoDrivableLaneModuleManager : public SceneModuleManagerInterface private: NoDrivableLaneModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoDrivableLaneModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp index 51d3339339051..fb90e023fedcd 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class NoDrivableLaneModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp index 1cd527d93ef74..11953fd5dd177 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -15,7 +15,7 @@ #ifndef UTIL_HPP_ #define UTIL_HPP_ -#include +#include #include #include @@ -33,8 +33,8 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and no drivable lane polygon struct PathWithNoDrivableLanePolygonIntersection diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index a9c21f1aba189..0991b37120a6f 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index 46cc224ea0f6b..faa265afe1559 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -52,7 +52,7 @@ NoStoppingAreaModuleManager::NoStoppingAreaModuleManager(rclcpp::Node & node) } void NoStoppingAreaModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & m : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -76,7 +76,7 @@ void NoStoppingAreaModuleManager::launchNewModules( std::function &)> NoStoppingAreaModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto no_stopping_area_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index f7a9a5433e900..baf5901ccc124 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC private: NoStoppingAreaModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class NoStoppingAreaModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 7b886fea09b34..3f2e7581a4d14 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -56,8 +56,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( } boost::optional NoStoppingAreaModule::getStopLineGeometry2d( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double stop_line_margin) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const { // get stop line from map { @@ -216,8 +215,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & - predicted_obj_arr_ptr) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr) { // stuck points by predicted objects for (const auto & object : predicted_obj_arr_ptr->objects) { @@ -250,7 +248,7 @@ bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( return false; } bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly) { const double stop_vel = std::numeric_limits::min(); // stuck points by stop line @@ -278,14 +276,14 @@ bool NoStoppingAreaModule::checkStopLinesInNoStoppingArea( } Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, + const double margin, const double extra_dist) const { Polygon2d ego_area; // open polygon double dist_from_start_sum = 0.0; const double interpolation_interval = 0.5; bool is_in_area = false; - autoware_auto_planning_msgs::msg::PathWithLaneId interpolated_path; + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; if (!splineInterpolate(path, interpolation_interval, interpolated_path, logger_)) { return ego_area; } @@ -350,19 +348,19 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } bool NoStoppingAreaModule::isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { if ( object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + autoware_perception_msgs::msg::ObjectClassification::CAR || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + autoware_perception_msgs::msg::ObjectClassification::BUS || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + autoware_perception_msgs::msg::ObjectClassification::TRUCK || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + autoware_perception_msgs::msg::ObjectClassification::TRAILER || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; @@ -404,13 +402,13 @@ bool NoStoppingAreaModule::isStoppable( } void NoStoppingAreaModule::insertStopPoint( - autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point) { size_t insert_idx = static_cast(stop_point.first + 1); const auto stop_pose = stop_point.second; // To PathPointWithLaneId - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(insert_idx); stop_point_with_lane_id.point.pose = stop_pose; stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0; diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 84f260e5e2c2b..62ec0b88b328e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -24,10 +24,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include @@ -101,7 +101,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return true if the object has a target type */ bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief Check if there is a stopped vehicle in stuck vehicle detect area. @@ -111,8 +111,7 @@ class NoStoppingAreaModule : public SceneModuleInterface */ bool checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & - predicted_obj_arr_ptr); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr); /** * @brief Check if there is a stop line in "stop line detect area". @@ -121,7 +120,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return true if exists */ bool checkStopLinesInNoStoppingArea( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); + const tier4_planning_msgs::msg::PathWithLaneId & path, const Polygon2d & poly); /** * @brief Calculate the polygon of the path from the ego-car position to the end of the @@ -133,7 +132,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return generated polygon */ Polygon2d generateEgoNoStoppingAreaLanePolygon( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & ego_pose, const double margin, const double extra_dist) const; /** @@ -144,8 +143,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return generated stop line */ boost::optional getStopLineGeometry2d( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double stop_line_margin) const; + const tier4_planning_msgs::msg::PathWithLaneId & path, const double stop_line_margin) const; /** * @brief Calculate if it's possible for ego-vehicle to stop before area consider jerk limit @@ -162,7 +160,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @param stop_point stop line point on the lane */ void insertStopPoint( - autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); + tier4_planning_msgs::msg::PathWithLaneId & path, const PathIndexWithPose & stop_point); // Key Feature const lanelet::autoware::NoStoppingArea & no_stopping_area_reg_elem_; diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml index cb5bd744edfa8..5817c2203cc5d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs grid_map_ros diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index c9dbe13474b06..710f671df609f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -55,8 +55,8 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; namespace grid_utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index f257012c05519..64adc112aab2f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -117,7 +117,7 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node) } void OcclusionSpotModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -130,7 +130,7 @@ void OcclusionSpotModuleManager::launchNewModules( std::function &)> OcclusionSpotModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index c153a727b8a7e..0955e4ae9aab5 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -23,11 +23,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include #include @@ -53,10 +53,10 @@ class OcclusionSpotModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class OcclusionSpotModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index f22cedbe67c8f..a650a6fa5b39f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -94,8 +94,8 @@ bool buildDetectionAreaPolygon( } void calcSlowDownPointsForPossibleCollision( - const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const double offset, std::vector & possible_collisions) + const int closest_idx, const tier4_planning_msgs::msg::PathWithLaneId & path, const double offset, + std::vector & possible_collisions) { if (possible_collisions.empty()) { return; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 7f495f042d7f8..2b6d89680cd37 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -23,11 +23,11 @@ #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -48,19 +48,19 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::BasicLineString2d; using lanelet::BasicPoint2d; using lanelet::BasicPolygon2d; using lanelet::LaneletMapPtr; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using DetectionAreaIdx = std::optional>; using BasicPolygons2d = std::vector; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 9a0278bfad5e5..8d7e9d2fdedd5 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -17,7 +17,7 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 8ac3fe26c1c33..1f5ca1bab904e 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -42,7 +42,7 @@ namespace { namespace utils = behavior_velocity_planner::occlusion_spot_utils; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; std::vector extractStuckVehicle( const std::vector & vehicles, const double stop_velocity) { diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 2cd7ea3582452..b7da7c073cbd9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -22,11 +22,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 85ae85f1895d9..05e73855f2642 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -24,11 +24,11 @@ using Point = geometry_msgs::msg::Point; using Vector3 = geometry_msgs::msg::Vector3; -using DynamicObjects = autoware_auto_perception_msgs::msg::PredictedObjects; -using DynamicObject = autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using DynamicObjects = autoware_perception_msgs::msg::PredictedObjects; +using DynamicObject = autoware_perception_msgs::msg::PredictedObject; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using tier4_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { @@ -41,8 +41,7 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) std::vector possible_collisions; size_t num = 2000; // make a path with 2000 points from x=0 to x=4 - autoware_auto_planning_msgs::msg::PathWithLaneId path = - test::generatePath(0.0, 3.0, 4.0, 3.0, num); + tier4_planning_msgs::msg::PathWithLaneId path = test::generatePath(0.0, 3.0, 4.0, 3.0, num); // make 2000 possible collision from x=0 to x=10 test::generatePossibleCollisions(possible_collisions, 0.0, 3.0, 4.0, 3.0, num); diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp index cf24be4cc0b5c..f7bb3d4b1ad6f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -19,22 +19,22 @@ #include -#include -#include +#include +#include #include namespace test { -inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( +inline tier4_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + tier4_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; @@ -88,7 +88,7 @@ inline void generatePossibleCollisions( intersection_pose.position.x = y0 + y_step * i; // collision path point - autoware_auto_planning_msgs::msg::PathPoint collision_with_margin{}; + autoware_planning_msgs::msg::PathPoint collision_with_margin{}; collision_with_margin.pose.position.x = x0 + x_step * i + lon; collision_with_margin.pose.position.y = y0 + y_step * i; diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index b61441132969c..d3742ea7d305f 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -14,8 +14,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 489bb3f5abc78..862ca351a118a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -113,8 +113,8 @@ void add_lanelet_markers( void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, - const double z, const size_t prev_nb) + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_ego_idx, const double z, + const size_t prev_nb) { auto debug_marker = get_base_marker(); debug_marker.ns = "ranges"; diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp index 2b6b65638ec40..0802ae78d7a26 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -62,7 +62,7 @@ void add_lanelet_markers( /// @param [in] prev_nb previous number of markers (used to delete the extra ones) void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, const double z, const size_t prev_nb); } // namespace behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index e948bd74eba45..126c75c2f80b9 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -61,7 +61,7 @@ bool object_is_incoming( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger) { @@ -155,7 +155,7 @@ std::optional> object_time_to_range( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger) { const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 0612cc041c1ef..4f2b0a6dad89b 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -53,7 +53,7 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit @@ -67,7 +67,7 @@ std::optional> object_time_to_range( /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index de12334b91a19..80cd106bf52ab 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -27,7 +27,7 @@ namespace behavior_velocity_planner::out_of_lane { void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) { auto stop_line_idx = 0UL; @@ -57,7 +57,7 @@ void cut_predicted_path_beyond_line( } std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) + const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data) { lanelet::ConstLanelets lanelets; lanelet::BasicLineString2d query_line; @@ -81,8 +81,8 @@ std::optional find_next_stop_line( } void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const PlannerData & planner_data, const double object_front_overhang) + autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, + const double object_front_overhang) { const auto stop_line = find_next_stop_line(predicted_path, planner_data); if (stop_line) { @@ -95,15 +95,15 @@ void cut_predicted_path_beyond_red_lights( } } -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params) { - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + autoware_perception_msgs::msg::PredictedObjects filtered_objects; filtered_objects.header = planner_data.predicted_objects->header; for (const auto & object : planner_data.predicted_objects->objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; }) != object.classification.end(); if (is_pedestrian) continue; diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index bb6b5f4d00005..be3e8809d2e3d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -28,7 +28,7 @@ namespace behavior_velocity_planner::out_of_lane /// @param [in] stop_line stop line used for cutting /// @param [in] object_front_overhang extra distance to cut ahead of the stop line void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); /// @brief find the next red light stop line along the given path @@ -36,21 +36,21 @@ void cut_predicted_path_beyond_line( /// @param [in] planner_data planner data with stop line information /// @return the first red light stop line found along the path (if any) std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); + const autoware_perception_msgs::msg::PredictedPath & path, const PlannerData & planner_data); /// @brief cut predicted path beyond stop lines of red lights /// @param [inout] predicted_path predicted path to cut /// @param [in] planner_data planner data to get the map and traffic light information void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const PlannerData & planner_data, const double object_front_overhang); + autoware_perception_msgs::msg::PredictedPath & predicted_path, const PlannerData & planner_data, + const double object_front_overhang); /// @brief filter predicted objects and their predicted paths /// @param [in] planner_data planner data /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 7d764722405af..840054d92252f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -82,8 +82,7 @@ OutOfLaneModuleManager::OutOfLaneModuleManager(rclcpp::Node & node) pp.right_offset = vehicle_info.min_lateral_offset_m; } -void OutOfLaneModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void OutOfLaneModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) return; // general @@ -95,7 +94,7 @@ void OutOfLaneModuleManager::launchNewModules( std::function &)> OutOfLaneModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { return [path]([[maybe_unused]] const std::shared_ptr & scene_module) { return false; diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp index c8ef397913c8f..9da1751576a7f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -22,11 +22,11 @@ #include #include -#include -#include -#include +#include +#include #include #include +#include #include #include @@ -51,10 +51,10 @@ class OutOfLaneModuleManager : public SceneModuleManagerInterface PlannerParam planner_param_; int64_t module_id_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class OutOfLaneModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 6133bb1ea0d6e..1d51c45c6afd1 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index ff690699ee638..cb2945f8b32b8 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -17,9 +17,9 @@ #include -#include -#include +#include #include +#include #include @@ -99,7 +99,7 @@ struct Slowdown struct SlowdownToInsert { Slowdown slowdown; - autoware_auto_planning_msgs::msg::PathWithLaneId::_points_type::value_type point; + tier4_planning_msgs::msg::PathWithLaneId::_points_type::value_type point; }; /// @brief bound of an overlap range (either the first, or last bound) @@ -135,7 +135,7 @@ struct OverlapRange std::vector overlaps; std::optional decision; RangeTimes times; - std::optional object{}; + std::optional object{}; } debug; }; using OverlapRanges = std::vector; @@ -172,7 +172,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; size_t first_path_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] @@ -184,7 +184,7 @@ struct DecisionInputs { OverlapRanges ranges{}; EgoData ego_data; - autoware_auto_perception_msgs::msg::PredictedObjects objects{}; + autoware_perception_msgs::msg::PredictedObjects objects{}; std::shared_ptr route_handler{}; lanelet::ConstLanelets lanelets{}; }; @@ -201,7 +201,7 @@ struct DebugData lanelet::ConstLanelets path_lanelets; lanelet::ConstLanelets ignored_lanelets; lanelet::ConstLanelets other_lanelets; - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; size_t first_path_idx; size_t prev_footprints = 0; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index c85da1e551391..51511b94f3e33 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -66,7 +66,7 @@ struct PlannerData geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; static constexpr double velocity_buffer_time_sec = 10.0; std::deque velocity_buffer; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp index 624da6b170d06..dcdb4a7052cc0 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -30,10 +30,10 @@ class PluginInterface public: virtual ~PluginInterface() = default; virtual void init(rclcpp::Node & node) = 0; - virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) = 0; + virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) = 0; virtual void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::optional getFirstStopPathPointIndex() = 0; virtual const char * getModuleName() = 0; }; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp index 7aa3e6bfef9ab..abb14dd8b2356 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -28,13 +28,13 @@ class PluginWrapper : public PluginInterface { public: void init(rclcpp::Node & node) override { scene_manager_ = std::make_unique(node); } - void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override { scene_manager_->plan(path); }; void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override + const tier4_planning_msgs::msg::PathWithLaneId & path) override { scene_manager_->updateSceneModuleInstances(planner_data, path); } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 3a2b474e3d730..3e7992207f3f1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -26,9 +26,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -48,7 +48,6 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; using motion_utils::PlanningBehavior; using motion_utils::VelocityFactor; @@ -58,6 +57,7 @@ using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; using tier4_autoware_utils::getOrDeclareParameter; using tier4_debug_msgs::msg::Float64Stamped; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; using tier4_rtc_msgs::msg::Module; @@ -67,10 +67,10 @@ using unique_identifier_msgs::msg::UUID; struct ObjectOfInterest { geometry_msgs::msg::Pose pose; - autoware_auto_perception_msgs::msg::Shape shape; + autoware_perception_msgs::msg::Shape shape; ColorName color; ObjectOfInterest( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name) : pose(pose), shape(shape), color(color_name) { @@ -144,14 +144,14 @@ class SceneModuleInterface void syncActivation() { setActivation(isSafe()); } void setObjectsOfInterestData( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name) { objects_of_interest_.emplace_back(pose, shape, color_name); } size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; }; class SceneModuleManagerInterface @@ -167,22 +167,19 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path); - virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) - { - modifyPathVelocity(path); - } + virtual void plan(tier4_planning_msgs::msg::PathWithLaneId * path) { modifyPathVelocity(path); } protected: - virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path); + virtual void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path); - virtual void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + virtual void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::function &)> - getModuleExpiredFunction(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; + getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + virtual void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path); bool isModuleRegistered(const int64_t module_id) { @@ -194,7 +191,7 @@ class SceneModuleManagerInterface void unregisterModule(const std::shared_ptr & scene_module); size_t findEgoSegmentIndex( - const std::vector & points) const; + const std::vector & points) const; std::set> scene_modules_; std::set registered_module_id_set_; @@ -210,7 +207,7 @@ class SceneModuleManagerInterface rclcpp::Logger logger_; rclcpp::Publisher::SharedPtr pub_virtual_wall_; rclcpp::Publisher::SharedPtr pub_debug_; - rclcpp::Publisher::SharedPtr pub_debug_path_; + rclcpp::Publisher::SharedPtr pub_debug_path_; rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_velocity_factor_; @@ -226,7 +223,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface SceneModuleManagerInterfaceWithRTC( rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + void plan(tier4_planning_msgs::msg::PathWithLaneId * path) override; protected: RTCInterface rtc_interface_; @@ -261,7 +258,7 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishObjectsOfInterestMarker(); - void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void deleteExpiredModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; bool getEnableRTC(rclcpp::Node & node, const std::string & param_name) { diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 98cf5114fb0e7..c9d292536ac13 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -159,8 +159,7 @@ std::optional findOffsetSegment( } std::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, - const double offset); + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset); template geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) @@ -192,7 +191,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse } std::optional createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const size_t lane_id, const double margin, const double vehicle_offset); } // namespace arc_lane_utils diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 0f3cc7203ef1f..bf238ecad55cb 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -17,12 +17,12 @@ #include -#include -#include -#include +#include +#include #include #include #include +#include #include #include @@ -37,13 +37,13 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( geometry_msgs::msg::PoseWithCovarianceStamped, double, cs::cartesian, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, - pose.position.y, pose.position.z) + autoware_planning_msgs::msg::PathPoint, double, cs::cartesian, pose.position.x, pose.position.y, + pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, - point.pose.position.x, point.pose.position.y, point.pose.position.z) + tier4_planning_msgs::msg::PathPointWithLaneId, double, cs::cartesian, point.pose.position.x, + point.pose.position.y, point.pose.position.z) BOOST_GEOMETRY_REGISTER_POINT_3D( - autoware_auto_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, + autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index 98df29d1c44c0..ab44af265fbaa 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -17,10 +17,10 @@ #include -#include -#include +#include #include #include +#include #include #include @@ -35,11 +35,11 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPathMarkerArray( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPointsMarkerArray( diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 667c915ac1d7f..55a82db1ae390 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -17,22 +17,22 @@ #include -#include -#include +#include +#include #include namespace behavior_velocity_planner { bool splineInterpolate( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); -autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval); -autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( - const autoware_auto_planning_msgs::msg::Path & path); -autoware_auto_planning_msgs::msg::Path filterStopPathPoint( - const autoware_auto_planning_msgs::msg::Path & path); + const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, + tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger); +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval); +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path); +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path); } // namespace behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 2184be4fbd2fe..2aadb7883a857 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -28,9 +28,9 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index e042338c53485..4ef4bb91a295d 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -17,10 +17,10 @@ #include -#include -#include -#include +#include +#include #include +#include #include #include @@ -53,7 +53,7 @@ struct DetectionRange struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; - autoware_perception_msgs::msg::TrafficSignal signal; + autoware_perception_msgs::msg::TrafficLightGroup signal; }; using Pose = geometry_msgs::msg::Pose; @@ -62,16 +62,16 @@ using LineString2d = tier4_autoware_utils::LineString2d; using Polygon2d = tier4_autoware_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; namespace planning_utils { size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx); // create detection area from given range return false if creation failure bool createDetectionAreaPolygons( @@ -96,7 +96,7 @@ inline int64_t bitShift(int64_t original_id) bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path); + const tier4_planning_msgs::msg::PathWithLaneId & path); Polygon2d generatePathPolygon( const PathWithLaneId & path, const size_t start_idx, const size_t end_idx, const double width); double calcJudgeLineDistWithAccLimit( @@ -210,9 +210,8 @@ std::set getLaneIdSetOnPath( const geometry_msgs::msg::Pose & current_pose); bool isOverLine( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, - const double offset = 0.0); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const double offset = 0.0); std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index 30916709c82f7..ba847d8b1f853 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -20,9 +20,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_perception_msgs + autoware_planning_msgs autoware_velocity_smoother diagnostic_msgs eigen diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index eb43e45d55711..3092d33418c8b 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -38,7 +38,7 @@ SceneModuleInterface::SceneModuleInterface( } size_t SceneModuleInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -57,7 +57,7 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); } if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( + pub_debug_path_ = node.create_publisher( std::string("~/debug/path_with_lane_id/") + module_name, 1); } pub_virtual_wall_ = node.create_publisher( @@ -74,7 +74,7 @@ SceneModuleManagerInterface::SceneModuleManagerInterface( } size_t SceneModuleManagerInterface::findEgoSegmentIndex( - const std::vector & points) const + const std::vector & points) const { const auto & p = planner_data_; return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( @@ -83,7 +83,7 @@ size_t SceneModuleManagerInterface::findEgoSegmentIndex( void SceneModuleManagerInterface::updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { planner_data_ = planner_data; @@ -92,7 +92,7 @@ void SceneModuleManagerInterface::updateSceneModuleInstances( } void SceneModuleManagerInterface::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) + tier4_planning_msgs::msg::PathWithLaneId * path) { StopWatch stop_watch; stop_watch.tic("Total"); @@ -145,7 +145,7 @@ void SceneModuleManagerInterface::modifyPathVelocity( pub_infrastructure_commands_->publish(infrastructure_command_array); pub_debug_->publish(debug_marker_array); if (is_publish_debug_path_) { - autoware_auto_planning_msgs::msg::PathWithLaneId debug_path; + tier4_planning_msgs::msg::PathWithLaneId debug_path; debug_path.header = path->header; debug_path.points = path->points; pub_debug_path_->publish(debug_path); @@ -156,7 +156,7 @@ void SceneModuleManagerInterface::modifyPathVelocity( } void SceneModuleManagerInterface::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); @@ -198,8 +198,7 @@ SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( { } -void SceneModuleManagerInterfaceWithRTC::plan( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) +void SceneModuleManagerInterfaceWithRTC::plan(tier4_planning_msgs::msg::PathWithLaneId * path) { setActivation(); modifyPathVelocity(path); @@ -262,7 +261,7 @@ void SceneModuleManagerInterfaceWithRTC::publishObjectsOfInterestMarker() } void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto isModuleExpired = getModuleExpiredFunction(path); diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index 9409dddb678db..e5705b1367e0e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -98,8 +98,7 @@ std::optional checkCollision( } std::optional findOffsetSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, - const double offset) + const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset) { if (offset >= 0) { return findForwardOffsetSegment(path, index, offset); @@ -109,7 +108,7 @@ std::optional findOffsetSegment( } std::optional createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const size_t lane_id, const double margin, const double vehicle_offset) { // Find collision segment diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp index 6ff2ef53176c4..00d746c56db85 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -54,7 +54,7 @@ visualization_msgs::msg::MarkerArray createPolygonMarkerArray( } visualization_msgs::msg::MarkerArray createPathMarkerArray( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, + const tier4_planning_msgs::msg::PathWithLaneId & path, const std::string & ns, const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b) { @@ -83,7 +83,7 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp index 59c9e4861fbfd..fe956e9be9512 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -26,8 +26,8 @@ constexpr double DOUBLE_EPSILON = 1e-6; namespace behavior_velocity_planner { bool splineInterpolate( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval, - autoware_auto_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) + const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, + tier4_planning_msgs::msg::PathWithLaneId & output, const rclcpp::Logger logger) { if (input.points.size() < 2) { RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1."); @@ -47,8 +47,8 @@ bool splineInterpolate( * is the velocity of the closest point for the input "sub-path" which consists of the points before * the interpolated point. */ -autoware_auto_planning_msgs::msg::Path interpolatePath( - const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval) +autoware_planning_msgs::msg::Path interpolatePath( + const autoware_planning_msgs::msg::Path & path, const double length, const double interval) { const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")}; @@ -125,10 +125,10 @@ autoware_auto_planning_msgs::msg::Path interpolatePath( return motion_utils::resamplePath(path, s_out); } -autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( - const autoware_auto_planning_msgs::msg::Path & path) +autoware_planning_msgs::msg::Path filterLitterPathPoint( + const autoware_planning_msgs::msg::Path & path) { - autoware_auto_planning_msgs::msg::Path filtered_path; + autoware_planning_msgs::msg::Path filtered_path; const double epsilon = 0.01; size_t latest_id = 0; @@ -153,10 +153,10 @@ autoware_auto_planning_msgs::msg::Path filterLitterPathPoint( return filtered_path; } -autoware_auto_planning_msgs::msg::Path filterStopPathPoint( - const autoware_auto_planning_msgs::msg::Path & path) +autoware_planning_msgs::msg::Path filterStopPathPoint( + const autoware_planning_msgs::msg::Path & path) { - autoware_auto_planning_msgs::msg::Path filtered_path = path; + autoware_planning_msgs::msg::Path filtered_path = path; bool found_stop = false; for (size_t i = 0; i < filtered_path.points.size(); ++i) { if (std::fabs(filtered_path.points.at(i).longitudinal_velocity_mps) < 0.01) { diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index 725c717082620..ecc314bca2009 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -20,8 +20,8 @@ #include #include -#include #include +#include #include @@ -35,10 +35,10 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; @@ -55,8 +55,7 @@ bool smoothPath( const auto & smoother = planner_data->velocity_smoother_; auto trajectory = - motion_utils::convertToTrajectoryPoints( - in_path); + motion_utils::convertToTrajectoryPoints(in_path); const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); const auto traj_steering_rate_limited = diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index a3d1aab4f9b9f..b724d01346f1e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include @@ -39,7 +39,7 @@ namespace { size_t calcPointIndexFromSegmentIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) { const size_t prev_point_idx = seg_idx; @@ -54,7 +54,7 @@ size_t calcPointIndexFromSegmentIndex( return next_point_idx; } -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) { @@ -93,7 +93,7 @@ namespace behavior_velocity_planner { namespace planning_utils { -using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::PathPoint; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; using motion_utils::validateNonEmpty; @@ -105,7 +105,7 @@ using tier4_autoware_utils::createQuaternionFromYaw; using tier4_autoware_utils::getPoint; size_t calcSegmentIndexFromPointIndex( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) { if (idx == 0) { @@ -304,7 +304,7 @@ bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.size() == 0) { return geometry_msgs::msg::Pose{}; @@ -605,9 +605,8 @@ std::vector getSubsequentLaneIdsSetOnPath( // TODO(murooka) remove calcSignedArcLength using findNearestSegmentIndex bool isOverLine( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, - const double offset) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, + const geometry_msgs::msg::Pose & line_pose, const double offset) { return motion_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) + offset < diff --git a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp index f43450800b46e..fbc5f5d709c5c 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -82,8 +82,8 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity) TEST(specialInterpolation, specialInterpolation) { - using autoware_auto_planning_msgs::msg::Path; - using autoware_auto_planning_msgs::msg::PathPoint; + using autoware_planning_msgs::msg::Path; + using autoware_planning_msgs::msg::PathPoint; using behavior_velocity_planner::interpolatePath; using motion_utils::calcSignedArcLength; using motion_utils::searchZeroVelocityIndex; diff --git a/planning/behavior_velocity_planner_common/test/src/utils.hpp b/planning/behavior_velocity_planner_common/test/src/utils.hpp index 1328edb6f6027..4dbf4c73fcd82 100644 --- a/planning/behavior_velocity_planner_common/test/src/utils.hpp +++ b/planning/behavior_velocity_planner_common/test/src/utils.hpp @@ -15,23 +15,23 @@ #ifndef UTILS_HPP_ #define UTILS_HPP_ -#include -#include #include +#include +#include #include namespace test { -inline autoware_auto_planning_msgs::msg::PathWithLaneId generatePath( +inline tier4_planning_msgs::msg::PathWithLaneId generatePath( double x0, double y0, double x, double y, int nb_points) { - autoware_auto_planning_msgs::msg::PathWithLaneId path{}; + tier4_planning_msgs::msg::PathWithLaneId path{}; double x_step = (x - x0) / (nb_points - 1); double y_step = (y - y0) / (nb_points - 1); for (int i = 0; i < nb_points; ++i) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId point{}; + tier4_planning_msgs::msg::PathPointWithLaneId point{}; point.point.pose.position.x = x0 + x_step * i; point.point.pose.position.y = y0 + y_step * i; point.point.pose.position.z = 0.0; diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index 7c79d1c288a0d..d3e2d813c3799 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -71,14 +71,14 @@ This module can handle multiple types of obstacles by creating abstracted dynami Abstracted obstacle data has following information. -| Name | Type | Description | -| ---------------- | ----------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------- | -| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | -| classifications | `std::vector` | classifications with probability | -| shape | `autoware_auto_perception_msgs::msg::Shape` | shape of the obstacle | -| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | -| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | -| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | +| Name | Type | Description | +| ---------------- | ------------------------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------- | +| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | +| classifications | `std::vector` | classifications with probability | +| shape | `autoware_perception_msgs::msg::Shape` | shape of the obstacle | +| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | +| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | +| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | Enter the maximum/minimum velocity of the object as a parameter, adding enough margin to the expected velocity. This parameter is used to create polygons for [collision detection](.#Collision-detection). diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index a0b15570f48f2..40e55b3de1a17 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -19,8 +19,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs behavior_velocity_crosswalk_module behavior_velocity_planner_common eigen diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 9b94c8b681942..edf1d8cf48b8e 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -321,7 +321,7 @@ double convertDurationToDouble(const builtin_interfaces::msg::Duration & duratio // Create a path leading up to a specified prediction time std::vector createPathToPredictionTime( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) + const autoware_perception_msgs::msg::PredictedPath & predicted_path, double prediction_time) { // Calculate the number of poses to include based on the prediction time and the time step between // poses diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index f020da2abd045..c7d8ef674aafd 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -44,17 +44,17 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using run_out_utils::DynamicObstacle; using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; using run_out_utils::PlannerParam; using run_out_utils::PredictedPath; -using PathPointsWithLaneId = std::vector; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; +using PathPointsWithLaneId = std::vector; /** * @brief base class for creating dynamic obstacles from multiple types of input diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/behavior_velocity_run_out_module/src/manager.cpp index 1c85a22f57bf6..fb6a70070749a 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/behavior_velocity_run_out_module/src/manager.cpp @@ -146,8 +146,7 @@ RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) setDynamicObstacleCreator(node, debug_ptr_); } -void RunOutModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void RunOutModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { if (path.points.empty()) { return; @@ -162,8 +161,7 @@ void RunOutModuleManager::launchNewModules( } std::function &)> -RunOutModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +RunOutModuleManager::getModuleExpiredFunction(const tier4_planning_msgs::msg::PathWithLaneId & path) { return [&path]([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { diff --git a/planning/behavior_velocity_run_out_module/src/manager.hpp b/planning/behavior_velocity_run_out_module/src/manager.hpp index 4dd66ad45898b..ad35caf98149a 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/behavior_velocity_run_out_module/src/manager.hpp @@ -37,10 +37,10 @@ class RunOutModuleManager : public SceneModuleManagerInterface std::shared_ptr debug_ptr_; std::unique_ptr dynamic_obstacle_creator_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; void setDynamicObstacleCreator(rclcpp::Node & node, std::shared_ptr & debug_ptr); }; diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_run_out_module/src/path_utils.cpp index 1cec20297683c..be7db13cd3ca3 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.cpp @@ -18,7 +18,7 @@ namespace behavior_velocity_planner::run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points) { diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp index 92aca946c13ef..8867778520bc7 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -15,8 +15,8 @@ #ifndef PATH_UTILS_HPP_ #define PATH_UTILS_HPP_ -#include #include +#include #include #include @@ -31,7 +31,7 @@ namespace run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( - const std::vector & points, + const std::vector & points, const geometry_msgs::msg::Point & src_point, const std::vector & target_points); diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index da826e1cf0cf6..de50b3161ea6f 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -743,7 +743,7 @@ std::optional RunOutModule::calcStopPoint( bool RunOutModule::insertStopPoint( const std::optional stop_point, - autoware_auto_planning_msgs::msg::PathWithLaneId & path) + tier4_planning_msgs::msg::PathWithLaneId & path) { // no stop point if (!stop_point) { @@ -764,7 +764,7 @@ bool RunOutModule::insertStopPoint( } // to PathPointWithLaneId - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = path.points.at(nearest_seg_idx); stop_point_with_lane_id.point.pose = *stop_point; planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); @@ -880,7 +880,7 @@ void RunOutModule::insertApproachingVelocity( // to PathPointWithLaneId // use lane id of point behind inserted point - autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + tier4_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); stop_point_with_lane_id.point.pose = *stop_point; diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 3db6051ab36e7..2f797a44d06f7 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -32,12 +32,12 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; class RunOutModule : public SceneModuleInterface @@ -124,7 +124,7 @@ class RunOutModule : public SceneModuleInterface bool insertStopPoint( const std::optional stop_point, - autoware_auto_planning_msgs::msg::PathWithLaneId & path); + tier4_planning_msgs::msg::PathWithLaneId & path); void insertVelocityForState( const std::optional & dynamic_obstacle, const PlannerData planner_data, diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index f17f4c7251ea4..c6eda901a1424 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -54,7 +54,7 @@ Polygon2d createBoostPolyFromMsg(const std::vector & std::uint8_t getHighestProbLabel(const std::vector & classification) { - std::uint8_t label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + std::uint8_t label = autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; float highest_prob = 0.0; for (const auto & _class : classification) { if (highest_prob < _class.probability) { diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 51b460482458f..9208e57dcdc7f 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -21,9 +21,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -34,18 +34,18 @@ namespace behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::PathPoint; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::PathWithLaneId; using vehicle_info_util::VehicleInfo; -using PathPointsWithLaneId = std::vector; +using PathPointsWithLaneId = std::vector; struct CommonParam { double normal_min_jerk; // min jerk limit for mild stop [m/sss] diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml index 3dd16f2fd792a..9b1de66c95c92 100644 --- a/planning/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -15,7 +15,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index 5949357c90ff3..fcd000699b7ff 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -53,8 +53,7 @@ SpeedBumpModuleManager::SpeedBumpModuleManager(rclcpp::Node & node) static_cast(getOrDeclareParameter(node, ns + ".max_speed")); } -void SpeedBumpModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void SpeedBumpModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & speed_bump_with_lane_id : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -71,7 +70,7 @@ void SpeedBumpModuleManager::launchNewModules( std::function &)> SpeedBumpModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto speed_bump_id_set = planning_utils::getRegElemIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp index 480208067d87e..40fcfdd081c2e 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -39,10 +39,10 @@ class SpeedBumpModuleManager : public SceneModuleManagerInterface private: SpeedBumpModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class SpeedBumpModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp index 34de722055d3d..f227366127046 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -26,7 +26,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class SpeedBumpModule : public SceneModuleInterface { diff --git a/planning/behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_speed_bump_module/src/util.hpp index fd114772bda60..2cd408735fd61 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.hpp @@ -30,8 +30,8 @@ #include #include -#include #include +#include #include @@ -40,9 +40,9 @@ namespace behavior_velocity_planner namespace bg = boost::geometry; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Point32; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; // the status of intersection between path and speed bump struct PathPolygonIntersectionStatus diff --git a/planning/behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_stop_line_module/package.xml index ee79d64312161..c71ecb83098fb 100644 --- a/planning/behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -18,7 +18,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index 5c9f0fd39c644..b37b3b119178c 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -43,8 +43,7 @@ StopLineModuleManager::StopLineModuleManager(rclcpp::Node & node) } std::vector StopLineModuleManager::getStopLinesWithLaneIdOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::vector stop_lines_with_lane_id; @@ -66,8 +65,7 @@ std::vector StopLineModuleManager::getStopLinesWithLaneIdOnP } std::set StopLineModuleManager::getStopLineIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const lanelet::LaneletMapPtr lanelet_map) + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { std::set stop_line_id_set; @@ -78,8 +76,7 @@ std::set StopLineModuleManager::getStopLineIdSetOnPath( return stop_line_id_set; } -void StopLineModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +void StopLineModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & stop_line_with_lane_id : getStopLinesWithLaneIdOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { @@ -95,7 +92,7 @@ void StopLineModuleManager::launchNewModules( std::function &)> StopLineModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto stop_line_id_set = getStopLineIdSetOnPath(path, planner_data_->route_handler_->getLaneletMapPtr()); diff --git a/planning/behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp index 4b665a3a536f7..af9dbaa083c33 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -45,17 +45,17 @@ class StopLineModuleManager : public SceneModuleManagerInterface StopLineModule::PlannerParam planner_param_; std::vector getStopLinesWithLaneIdOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); std::set getStopLineIdSetOnPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map); - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; }; class StopLineModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index 544e14f130a7e..ff7b5a269db32 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -19,8 +19,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_planning_msgs autoware_perception_msgs + autoware_planning_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index edddef5cef4d8..01c3acf84cd63 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -41,17 +41,16 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node) planner_param_.enable_pass_judge = getOrDeclareParameter(node, ns + ".enable_pass_judge"); planner_param_.yellow_lamp_period = getOrDeclareParameter(node, ns + ".yellow_lamp_period"); - pub_tl_state_ = node.create_publisher( + pub_tl_state_ = node.create_publisher( "~/output/traffic_signal", 1); } -void TrafficLightModuleManager::modifyPathVelocity( - autoware_auto_planning_msgs::msg::PathWithLaneId * path) +void TrafficLightModuleManager::modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) { visualization_msgs::msg::MarkerArray debug_marker_array; visualization_msgs::msg::MarkerArray virtual_wall_marker_array; - autoware_perception_msgs::msg::TrafficSignal tl_state; + autoware_perception_msgs::msg::TrafficLightGroup tl_state; autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; velocity_factor_array.header.frame_id = "map"; @@ -110,7 +109,7 @@ void TrafficLightModuleManager::modifyPathVelocity( } void TrafficLightModuleManager::launchNewModules( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { for (const auto & traffic_light_reg_elem : planning_utils::getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), @@ -140,7 +139,7 @@ void TrafficLightModuleManager::launchNewModules( std::function &)> TrafficLightModuleManager::getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) + const tier4_planning_msgs::msg::PathWithLaneId & path) { const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 959ef2f91d36c..97340f8592a7d 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -40,12 +40,12 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC private: TrafficLightModule::PlannerParam planner_param_; - void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; std::function &)> getModuleExpiredFunction( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + const tier4_planning_msgs::msg::PathWithLaneId & path) override; - void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; + void modifyPathVelocity(tier4_planning_msgs::msg::PathWithLaneId * path) override; bool isModuleRegisteredFromRegElement(const lanelet::Id & id, const size_t module_id) const; @@ -56,7 +56,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC const lanelet::TrafficLightConstPtr registered_element) const; // Debug - rclcpp::Publisher::SharedPtr pub_tl_state_; + rclcpp::Publisher::SharedPtr pub_tl_state_; std::optional nearest_ref_stop_path_point_index_; }; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 0158251bb42b4..c886578dc65e4 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -75,7 +75,7 @@ std::optional findNearestCollisionPoint( } bool createTargetPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, + const tier4_planning_msgs::msg::PathWithLaneId & input, const LineString2d & stop_line, const double offset, size_t & target_point_idx, Eigen::Vector2d & target_point) { if (input.points.size() < 2) { @@ -140,7 +140,7 @@ bool createTargetPoint( } bool calcStopPointAndInsertIndex( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const lanelet::ConstLineString3d & lanelet_stop_lines, const double & offset, const double & stop_line_extend_length, Eigen::Vector2d & stop_line_point, size_t & stop_line_point_idx) @@ -394,12 +394,11 @@ bool TrafficLightModule::isTrafficSignalTimedOut() const return false; } -autoware_auto_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, - const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, - tier4_planning_msgs::msg::StopReason * stop_reason) +tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, + const Eigen::Vector2d & target_point, tier4_planning_msgs::msg::StopReason * stop_reason) { - autoware_auto_planning_msgs::msg::PathWithLaneId modified_path; + tier4_planning_msgs::msg::PathWithLaneId modified_path; modified_path = input; // Create stop pose diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index d6a409ca7034a..8385a1210d421 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -36,8 +36,8 @@ namespace behavior_velocity_planner class TrafficLightModule : public SceneModuleInterface { public: - using TrafficSignal = autoware_perception_msgs::msg::TrafficSignal; - using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; + using TrafficSignal = autoware_perception_msgs::msg::TrafficLightGroup; + using TrafficSignalElement = autoware_perception_msgs::msg::TrafficLightElement; using Time = rclcpp::Time; enum class State { APPROACH, GO_OUT }; @@ -45,7 +45,8 @@ class TrafficLightModule : public SceneModuleInterface { double base_link2front; std::vector, autoware_perception_msgs::msg::TrafficSignal>> + std::shared_ptr, + autoware_perception_msgs::msg::TrafficLightGroup>> tl_state; std::vector stop_poses; geometry_msgs::msg::Pose first_stop_pose; @@ -87,10 +88,9 @@ class TrafficLightModule : public SceneModuleInterface private: bool isStopSignal(); - autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPose( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input, - const size_t & insert_target_point_idx, const Eigen::Vector2d & target_point, - tier4_planning_msgs::msg::StopReason * stop_reason); + tier4_planning_msgs::msg::PathWithLaneId insertStopPose( + const tier4_planning_msgs::msg::PathWithLaneId & input, const size_t & insert_target_point_idx, + const Eigen::Vector2d & target_point, tier4_planning_msgs::msg::StopReason * stop_reason); bool isPassthrough(const double & signed_arc_length) const; diff --git a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml deleted file mode 100644 index 943ef175aa8f8..0000000000000 --- a/planning/behavior_velocity_virtual_traffic_light_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/behavior_velocity_walkway_module/package.xml b/planning/behavior_velocity_walkway_module/package.xml index 2c72959d6b1d4..ea6c7803717f9 100644 --- a/planning/behavior_velocity_walkway_module/package.xml +++ b/planning/behavior_velocity_walkway_module/package.xml @@ -17,7 +17,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs behavior_velocity_crosswalk_module behavior_velocity_planner_common geometry_msgs diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp index 453fcdb40f0db..5228445bb4e03 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface { diff --git a/planning/costmap_generator/README.md b/planning/costmap_generator/README.md index d30d8b5b58706..cb298195a82a1 100644 --- a/planning/costmap_generator/README.md +++ b/planning/costmap_generator/README.md @@ -6,12 +6,12 @@ This node reads `PointCloud` and/or `DynamicObjectArray` and creates an `Occupan ### Input topics -| Name | Type | Description | -| ------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------- | -| `~input/objects` | autoware_auto_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | -| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | -| `~input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map, for drivable areas | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| Name | Type | Description | +| ------------------------- | ------------------------------------------ | ---------------------------------------------------------------------------- | +| `~input/objects` | autoware_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | +| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | +| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map, for drivable areas | +| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp index 87fded0c01891..31c8a05cc3c60 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp @@ -53,8 +53,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -84,7 +84,7 @@ class CostmapGenerator : public rclcpp::Node bool use_parkinglot_; lanelet::LaneletMapPtr lanelet_map_; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; sensor_msgs::msg::PointCloud2::ConstSharedPtr points_; std::string costmap_frame_; @@ -114,9 +114,8 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_points_; - rclcpp::Subscription::SharedPtr - sub_objects_; - rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; + rclcpp::Subscription::SharedPtr sub_objects_; + rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; rclcpp::Subscription::SharedPtr sub_scenario_; rclcpp::TimerBase::SharedPtr timer_; @@ -143,12 +142,12 @@ class CostmapGenerator : public rclcpp::Node void initLaneletMap(); /// \brief callback for loading lanelet2 map - void onLaneletMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onLaneletMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); /// \brief callback for DynamicObjectArray /// \param[in] in_objects input DynamicObjectArray usually from prediction or perception /// component - void onObjects(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void onObjects(const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); /// \brief callback for sensor_msgs::PointCloud2 /// \param[in] in_points input sensor_msgs::PointCloud2. Assuming ground-filtered pointcloud @@ -190,7 +189,7 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost from DynamicObjectArray /// \param[in] in_objects: subscribed DynamicObjectArray grid_map::Matrix generateObjectsCostmap( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); /// \brief calculate cost from lanelet2 map grid_map::Matrix generatePrimitivesCostmap(); diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp index 84a7807e432cd..a88bb97a623e2 100644 --- a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp +++ b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp @@ -53,7 +53,7 @@ #include #endif -#include +#include #include @@ -71,7 +71,7 @@ class ObjectsToCostmap grid_map::Matrix makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, const double size_of_expansion_kernel, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); private: const int NUMBER_OF_POINTS; @@ -84,7 +84,7 @@ class ObjectsToCostmap /// \param[in] expand_rectangle_size: expanding 4 points /// \param[out] 4 rectangle points Eigen::MatrixXd makeRectanglePoints( - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size); /// \brief make polygon(grid_map::Polygon) from 4 rectangle's points @@ -93,7 +93,7 @@ class ObjectsToCostmap /// \param[out] polygon with 4 rectangle points grid_map::Polygon makePolygonFromObjectBox( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size); /// \brief make expanded point from convex hull's point @@ -111,7 +111,7 @@ class ObjectsToCostmap /// \param[out] polygon object with convex hull points grid_map::Polygon makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size); /// \brief set cost in polygon by using DynamicObject's score diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index 8200db93ae2fb..777dfc7a50a6e 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -196,12 +196,12 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) // Subscribers using std::placeholders::_1; - sub_objects_ = this->create_subscription( + sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1)); sub_points_ = this->create_subscription( "~/input/points_no_ground", rclcpp::SensorDataQoS(), std::bind(&CostmapGenerator::onPoints, this, _1)); - sub_lanelet_bin_map_ = this->create_subscription( + sub_lanelet_bin_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&CostmapGenerator::onLaneletMapBin, this, _1)); sub_scenario_ = this->create_subscription( @@ -263,7 +263,7 @@ void CostmapGenerator::loadParkingAreasFromLaneletMap( } void CostmapGenerator::onLaneletMapBin( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_); @@ -278,7 +278,7 @@ void CostmapGenerator::onLaneletMapBin( } void CostmapGenerator::onObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { objects_ = msg; } @@ -388,12 +388,12 @@ grid_map::Matrix CostmapGenerator::generatePointsCostmap( return points_costmap; } -autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( +autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( const tf2_ros::Buffer & tf_buffer, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, const std::string & target_frame_id, const std::string & src_frame_id) { - auto objects = new autoware_auto_perception_msgs::msg::PredictedObjects(); + auto objects = new autoware_perception_msgs::msg::PredictedObjects(); *objects = *in_objects; objects->header.frame_id = target_frame_id; @@ -411,11 +411,11 @@ autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformOb object.kinematics.initial_pose_with_covariance.pose = output_stamped.pose; } - return autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); + return autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); } grid_map::Matrix CostmapGenerator::generateObjectsCostmap( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { const auto object_frame = in_objects->header.frame_id; const auto transformed_objects = diff --git a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp index 79ab4a3bf66c1..f6f024fb92a4e 100644 --- a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp @@ -59,7 +59,7 @@ ObjectsToCostmap::ObjectsToCostmap() } Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size) { double length = in_object.shape.dimensions.x + expand_rectangle_size; @@ -85,7 +85,7 @@ Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectBox( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size) { grid_map::Polygon polygon; @@ -122,7 +122,7 @@ geometry_msgs::msg::Point ObjectsToCostmap::makeExpandedPoint( grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size) { grid_map::Polygon polygon; @@ -157,7 +157,7 @@ void ObjectsToCostmap::setCostInPolygon( grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, const double size_of_expansion_kernel, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { grid_map::GridMap objects_costmap = costmap; objects_costmap.add(OBJECTS_COSTMAP_LAYER_, 0); @@ -165,11 +165,11 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( for (const auto & object : in_objects->objects) { grid_map::Polygon polygon; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { polygon = makePolygonFromObjectConvexHull(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); } diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index ae124a1774881..dc94d74dce8ae 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -15,8 +15,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_map_msgs + autoware_perception_msgs grid_map_ros lanelet2_extension libpcl-all-dev diff --git a/planning/costmap_generator/test/test_objects_to_costmap.cpp b/planning/costmap_generator/test/test_objects_to_costmap.cpp index 667ee70ef53ff..15b853d0782aa 100644 --- a/planning/costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/costmap_generator/test/test_objects_to_costmap.cpp @@ -17,7 +17,7 @@ #include -using LABEL = autoware_auto_perception_msgs::msg::ObjectClassification; +using LABEL = autoware_perception_msgs::msg::ObjectClassification; class ObjectsToCostMapTest : public ::testing::Test { @@ -69,10 +69,10 @@ grid_y */ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) { - auto objs = std::make_shared(); - autoware_auto_perception_msgs::msg::PredictedObject object; + auto objs = std::make_shared(); + autoware_perception_msgs::msg::PredictedObject object; - object.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); + object.classification.push_back(autoware_perception_msgs::msg::ObjectClassification{}); object.classification.at(0).label = LABEL::CAR; object.classification.at(0).probability = 0.8; @@ -85,7 +85,7 @@ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0; object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1; - object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; object.shape.dimensions.x = 5; object.shape.dimensions.y = 3; object.shape.dimensions.z = 2; diff --git a/planning/freespace_planner/README.md b/planning/freespace_planner/README.md index b40f2681dce51..6ed0ab605ea05 100644 --- a/planning/freespace_planner/README.md +++ b/planning/freespace_planner/README.md @@ -13,19 +13,19 @@ In other words, the output trajectory doesn't include both forward and backward ### Input topics -| Name | Type | Description | -| ----------------------- | ---------------------------------- | --------------------------------------------------------- | -| `~input/route` | autoware_auto_planning_msgs::Route | route and goal pose | -| `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas | -| `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| Name | Type | Description | +| ----------------------- | ----------------------------- | --------------------------------------------------------- | +| `~input/route` | autoware_planning_msgs::Route | route and goal pose | +| `~input/occupancy_grid` | nav_msgs::OccupancyGrid | costmap, for drivable areas | +| `~input/odometry` | nav_msgs::Odometry | vehicle velocity, for checking whether vehicle is stopped | +| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics -| Name | Type | Description | -| -------------------- | --------------------------------------- | ------------------------------------------ | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | -| `is_completed` | bool (implemented as rosparam) | whether all split trajectory are published | +| Name | Type | Description | +| -------------------- | ---------------------------------- | ------------------------------------------ | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | +| `is_completed` | bool (implemented as rosparam) | whether all split trajectory are published | ### Output TFs diff --git a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp index 2c10b1b491ace..25f3bca215063 100644 --- a/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp +++ b/planning/freespace_planner/include/freespace_planner/freespace_planner_node.hpp @@ -38,8 +38,8 @@ #include #include -#include #include +#include #include #include #include @@ -65,8 +65,8 @@ namespace freespace_planner { -using autoware_auto_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::LaneletRoute; +using autoware_planning_msgs::msg::Trajectory; using freespace_planning_algorithms::AbstractPlanningAlgorithm; using freespace_planning_algorithms::AstarParam; using freespace_planning_algorithms::AstarSearch; diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index 8ebb22fdb5c2f..8445bd2355b25 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -15,7 +15,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager freespace_planning_algorithms geometry_msgs diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp index 1c2b4513e9c45..b0d9e9cf3edcd 100644 --- a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp +++ b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp @@ -44,9 +44,9 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; using freespace_planning_algorithms::AstarSearch; using freespace_planning_algorithms::PlannerWaypoint; using freespace_planning_algorithms::PlannerWaypoints; diff --git a/planning/mission_planner/README.md b/planning/mission_planner/README.md index a9eb488c71377..2f912e30b8246 100644 --- a/planning/mission_planner/README.md +++ b/planning/mission_planner/README.md @@ -47,10 +47,10 @@ It distributes route requests and planning results according to current MRM oper ### Subscriptions -| Name | Type | Description | -| ----------------------- | ------------------------------------ | ---------------------- | -| `~/input/vector_map` | autoware_auto_mapping_msgs/HADMapBin | vector map of Lanelet2 | -| `~/input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | +| Name | Type | Description | +| --------------------- | ----------------------------------- | ---------------------- | +| `input/vector_map` | autoware_map_msgs/msg/LaneletMapBin | vector map of Lanelet2 | +| `input/modified_goal` | geometry_msgs/PoseWithUuidStamped | modified goal pose | ### Publications @@ -70,7 +70,7 @@ It distributes route requests and planning results according to current MRM oper ![route_sections](./media/route_sections.svg) Route section, whose type is `autoware_planning_msgs/LaneletSegment`, is a "slice" of a road that bundles lane changeable lanes. -Note that the most atomic unit of route is `autoware_auto_mapping_msgs/LaneletPrimitive`, which has the unique id of a lane in a vector map and its type. +Note that the most atomic unit of route is `autoware_planning_msgs/LaneletPrimitive`, which has the unique id of a lane in a vector map and its type. Therefore, route message does not contain geometric information about the lane since we did not want to have planning module’s message to have dependency on map data structure. The ROS message of route section contains following three elements for each route section. diff --git a/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp b/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp index 96de30ddab592..80e197e2a8ef9 100644 --- a/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp +++ b/planning/mission_planner/include/mission_planner/mission_planner_plugin.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -32,12 +32,12 @@ class PlannerPlugin public: using RoutePoints = std::vector; using LaneletRoute = autoware_planning_msgs::msg::LaneletRoute; - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; using MarkerArray = visualization_msgs::msg::MarkerArray; virtual ~PlannerPlugin() = default; virtual void initialize(rclcpp::Node * node) = 0; - virtual void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) = 0; + virtual void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) = 0; virtual bool ready() const = 0; virtual LaneletRoute plan(const RoutePoints & points) = 0; virtual MarkerArray visualize(const LaneletRoute & route) const = 0; diff --git a/planning/mission_planner/package.xml b/planning/mission_planner/package.xml index b8ad634c9a0d4..f878953d50420 100644 --- a/planning/mission_planner/package.xml +++ b/planning/mission_planner/package.xml @@ -18,7 +18,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_mapping_msgs + autoware_map_msgs autoware_planning_msgs geometry_msgs lanelet2_extension diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index d4f12316052d9..5ba152af6611d 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -163,12 +163,12 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) void DefaultPlanner::initialize(rclcpp::Node * node) { initialize_common(node); - map_subscriber_ = node_->create_subscription( + map_subscriber_ = node_->create_subscription( "~/input/vector_map", rclcpp::QoS{10}.transient_local(), std::bind(&DefaultPlanner::map_callback, this, std::placeholders::_1)); } -void DefaultPlanner::initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) +void DefaultPlanner::initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) { initialize_common(node); map_callback(msg); @@ -179,7 +179,7 @@ bool DefaultPlanner::ready() const return is_graph_ready_; } -void DefaultPlanner::map_callback(const HADMapBin::ConstSharedPtr msg) +void DefaultPlanner::map_callback(const LaneletMapBin::ConstSharedPtr msg) { route_handler_.setMap(*msg); is_graph_ready_ = true; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 8c0fbf3b33955..60c232162991d 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -45,7 +45,7 @@ class DefaultPlanner : public mission_planner::PlannerPlugin { public: void initialize(rclcpp::Node * node) override; - void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) override; + void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override; bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; void updateRoute(const PlannerPlugin::LaneletRoute & route) override; @@ -63,11 +63,11 @@ class DefaultPlanner : public mission_planner::PlannerPlugin DefaultPlannerParameters param_; rclcpp::Node * node_; - rclcpp::Subscription::SharedPtr map_subscriber_; + rclcpp::Subscription::SharedPtr map_subscriber_; rclcpp::Publisher::SharedPtr pub_goal_footprint_marker_; void initialize_common(rclcpp::Node * node); - void map_callback(const HADMapBin::ConstSharedPtr msg); + void map_callback(const LaneletMapBin::ConstSharedPtr msg); /** * @brief check if the goal_footprint is within the combined lanelet of route_lanelets plus the diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index af363bcf23da7..d95bcb8caefae 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -58,7 +58,7 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_odometry_ = create_subscription( "~/input/odometry", rclcpp::QoS(1), std::bind(&MissionPlanner::on_odometry, this, _1)); - sub_vector_map_ = create_subscription( + sub_vector_map_ = create_subscription( "~/input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, _1)); sub_reroute_availability_ = create_subscription( "~/input/reroute_availability", rclcpp::QoS(1), @@ -141,7 +141,7 @@ void MissionPlanner::on_reroute_availability(const RerouteAvailability::ConstSha reroute_availability_ = msg; } -void MissionPlanner::on_map(const HADMapBin::ConstSharedPtr msg) +void MissionPlanner::on_map(const LaneletMapBin::ConstSharedPtr msg) { map_ptr_ = msg; lanelet_map_ptr_ = std::make_shared(); diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index 6752ceb4eaa8b..3d712a351d94a 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -44,7 +44,7 @@ namespace mission_planner { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; @@ -84,19 +84,19 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_modified_goal_; rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; rclcpp::Publisher::SharedPtr pub_marker_; Odometry::ConstSharedPtr odometry_; - HADMapBin::ConstSharedPtr map_ptr_; + LaneletMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; RouteState state_; LaneletRoute::ConstSharedPtr current_route_; lanelet::LaneletMapPtr lanelet_map_ptr_{nullptr}; void on_odometry(const Odometry::ConstSharedPtr msg); - void on_map(const HADMapBin::ConstSharedPtr msg); + void on_map(const LaneletMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml index 5ba740773d5a5..051415e9856ed 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/package.xml @@ -14,9 +14,9 @@ autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_motion_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs geometry_msgs lanelet2_extension libboost-dev diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp index 585f4760290ef..efb957ea85691 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/debug.cpp @@ -118,7 +118,7 @@ void add_lanelet_markers( void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, - const std::vector & trajectory_points, + const std::vector & trajectory_points, const size_t first_ego_idx, const double z, const size_t prev_nb) { auto debug_marker = get_base_marker(); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp index 56047e459bf51..81610f750e753 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.cpp @@ -63,7 +63,7 @@ bool object_is_incoming( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger) { @@ -157,7 +157,7 @@ std::optional> object_time_to_range( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger) { const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp index 2aa3a8b490bf6..e691f2893dab9 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/decisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -54,7 +54,7 @@ double time_along_trajectory(const EgoData & ego_data, const size_t target_idx); /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const double dist_buffer, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit @@ -68,7 +68,7 @@ std::optional> object_time_to_range( /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index 31631fb577b09..ce993dc1cedb0 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -27,7 +27,7 @@ namespace autoware::motion_velocity_planner::out_of_lane { void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang) { if (predicted_path.path.empty() || stop_line.size() < 2) return; @@ -59,7 +59,7 @@ void cut_predicted_path_beyond_line( } std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, + const autoware_perception_msgs::msg::PredictedPath & path, const std::shared_ptr planner_data) { lanelet::ConstLanelets lanelets; @@ -84,7 +84,7 @@ std::optional find_next_stop_line( } void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const std::shared_ptr planner_data, const double object_front_overhang) { const auto stop_line = find_next_stop_line(predicted_path, planner_data); @@ -98,16 +98,16 @@ void cut_predicted_path_beyond_red_lights( } } -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const std::shared_ptr planner_data, const EgoData & ego_data, const PlannerParam & params) { - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + autoware_perception_msgs::msg::PredictedObjects filtered_objects; filtered_objects.header = planner_data->predicted_objects->header; for (const auto & object : planner_data->predicted_objects->objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; }) != object.classification.end(); if (is_pedestrian) continue; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 0770fbc0dcff1..3083d85df42ab 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -29,7 +29,7 @@ namespace autoware::motion_velocity_planner::out_of_lane /// @param [in] stop_line stop line used for cutting /// @param [in] object_front_overhang extra distance to cut ahead of the stop line void cut_predicted_path_beyond_line( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const lanelet::BasicLineString2d & stop_line, const double object_front_overhang); /// @brief find the next red light stop line along the given path @@ -37,14 +37,14 @@ void cut_predicted_path_beyond_line( /// @param [in] planner_data planner data with stop line information /// @return the first red light stop line found along the path (if any) std::optional find_next_stop_line( - const autoware_auto_perception_msgs::msg::PredictedPath & path, + const autoware_perception_msgs::msg::PredictedPath & path, const std::shared_ptr planner_data); /// @brief cut predicted path beyond stop lines of red lights /// @param [inout] predicted_path predicted path to cut /// @param [in] planner_data planner data to get the map and traffic light information void cut_predicted_path_beyond_red_lights( - autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + autoware_perception_msgs::msg::PredictedPath & predicted_path, const std::shared_ptr planner_data, const double object_front_overhang); /// @brief filter predicted objects and their predicted paths @@ -52,7 +52,7 @@ void cut_predicted_path_beyond_red_lights( /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const std::shared_ptr planner_data, const EgoData & ego_data, const PlannerParam & params); } // namespace autoware::motion_velocity_planner::out_of_lane diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp index b662186049342..2cd050d15dd9b 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.cpp @@ -147,7 +147,7 @@ void OutOfLaneModule::update_parameters(const std::vector & p } VelocityPlanningResult OutOfLaneModule::plan( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { VelocityPlanningResult result; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp index 8a20d5c850a09..94f1c41fe97a6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/out_of_lane_module.hpp @@ -22,9 +22,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include @@ -40,7 +40,7 @@ class OutOfLaneModule : public PluginModuleInterface void init(rclcpp::Node & node, const std::string & module_name) override; void update_parameters(const std::vector & parameters) override; VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) override; std::string get_module_name() const override { return module_name_; } diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp index 04f9f1ccac4c2..4fce13582aa1e 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/src/types.hpp @@ -17,9 +17,9 @@ #include -#include -#include -#include +#include +#include +#include #include #include @@ -34,7 +34,7 @@ namespace autoware::motion_velocity_planner::out_of_lane { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; /// @brief parameters for the "out of lane" module struct PlannerParam @@ -100,7 +100,7 @@ struct Slowdown struct SlowdownToInsert { Slowdown slowdown{}; - autoware_auto_planning_msgs::msg::TrajectoryPoint point{}; + autoware_planning_msgs::msg::TrajectoryPoint point{}; }; /// @brief bound of an overlap range (either the first, or last bound) @@ -136,7 +136,7 @@ struct OverlapRange std::vector overlaps{}; std::optional decision{}; RangeTimes times{}; - std::optional object{}; + std::optional object{}; } debug; }; using OverlapRanges = std::vector; @@ -173,7 +173,7 @@ struct OtherLane /// @brief data related to the ego vehicle struct EgoData { - std::vector trajectory_points{}; + std::vector trajectory_points{}; size_t first_trajectory_idx{}; double velocity{}; // [m/s] double max_decel{}; // [m/s²] @@ -185,7 +185,7 @@ struct DecisionInputs { OverlapRanges ranges{}; EgoData ego_data{}; - autoware_auto_perception_msgs::msg::PredictedObjects objects{}; + autoware_perception_msgs::msg::PredictedObjects objects{}; std::shared_ptr route_handler{}; lanelet::ConstLanelets lanelets{}; }; @@ -202,7 +202,7 @@ struct DebugData lanelet::ConstLanelets trajectory_lanelets; lanelet::ConstLanelets ignored_lanelets; lanelet::ConstLanelets other_lanelets; - std::vector trajectory_points; + std::vector trajectory_points; size_t first_trajectory_idx; size_t prev_footprints = 0; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp index 4961cd064efaf..6d1ba8084b821 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_out_of_lane_module/test/test_filter_predicted_objects.cpp @@ -14,9 +14,9 @@ #include "../src/filter_predicted_objects.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -24,7 +24,7 @@ TEST(TestCollisionDistance, CutPredictedPathBeyondLine) { using autoware::motion_velocity_planner::out_of_lane::cut_predicted_path_beyond_line; - autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + autoware_perception_msgs::msg::PredictedPath predicted_path; lanelet::BasicLineString2d stop_line; double object_front_overhang = 0.0; const auto eps = 1e-9; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index c8d0ac54ca969..add6b5ef392ea 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -19,10 +19,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -50,7 +50,7 @@ namespace autoware::motion_velocity_planner struct TrafficSignalStamped { builtin_interfaces::msg::Time stamp; - autoware_perception_msgs::msg::TrafficSignal signal; + autoware_perception_msgs::msg::TrafficLightGroup signal; }; struct PlannerData { @@ -63,7 +63,7 @@ struct PlannerData geometry_msgs::msg::PoseStamped::ConstSharedPtr current_odometry; geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity; geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; std::shared_ptr route_handler; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp index 9011d3fb7e079..dd2b89e3261d6 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/plugin_module_interface.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -37,7 +37,7 @@ class PluginModuleInterface virtual void init(rclcpp::Node & node, const std::string & module_name) = 0; virtual void update_parameters(const std::vector & parameters) = 0; virtual VelocityPlanningResult plan( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) = 0; virtual std::string get_module_name() const = 0; motion_utils::VelocityFactorInterface velocity_factor_interface_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index b3eaaccd11d13..2f4e7241d60ee 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -15,8 +15,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_velocity_smoother behavior_velocity_planner_common eigen diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md index 9190adf1ff67e..494446828e134 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/README.md @@ -17,23 +17,23 @@ This means that to stop before a wall, a stop point is inserted in the trajector ## Input topics -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------- | ----------------------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | input trajectory | -| `~/input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | -| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | -| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | -| `~/input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | -| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | -| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | +| Name | Type | Description | +| -------------------------------------- | ----------------------------------------------------- | ----------------------------- | +| `~/input/trajectory` | autoware_planning_msgs::msg::Trajectory | input trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle position and velocity | +| `~/input/accel` | geometry_msgs::msg::AccelWithCovarianceStamped | vehicle acceleration | +| `~/input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~/input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/traffic_signals` | autoware_perception_msgs::msg::TrafficLightGroupArray | traffic light states | +| `~/input/virtual_traffic_light_states` | tier4_v2x_msgs::msg::VirtualTrafficLightStateArray | virtual traffic light states | +| `~/input/occupancy_grid` | nav_msgs::msg::OccupancyGrid | occupancy grid | ## Output topics | Name | Type | Description | | --------------------------- | ------------------------------------------------- | -------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | +| `~/output/trajectory` | autoware_planning_msgs::msg::Trajectory | Ego trajectory with updated velocity profile | | `~/output/velocity_factors` | autoware_adapi_v1_msgs::msg::VelocityFactorsArray | factors causing change in the ego velocity profile | ## Services diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml index 0cbf220cd0955..e45935b36b13a 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/package.xml @@ -17,10 +17,10 @@ rosidl_default_generators - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_motion_velocity_planner_common + autoware_perception_msgs + autoware_planning_msgs autoware_velocity_smoother diagnostic_msgs eigen diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 7c2c56c332865..4d152afa250b7 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -60,11 +60,11 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & using std::placeholders::_2; // Subscribers - sub_trajectory_ = this->create_subscription( + sub_trajectory_ = this->create_subscription( "~/input/trajectory", 1, std::bind(&MotionVelocityPlannerNode::on_trajectory, this, _1), create_subscription_options(this)); sub_predicted_objects_ = - this->create_subscription( + this->create_subscription( "~/input/dynamic_objects", 1, std::bind(&MotionVelocityPlannerNode::on_predicted_objects, this, _1), create_subscription_options(this)); @@ -78,12 +78,12 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & sub_acceleration_ = this->create_subscription( "~/input/accel", 1, std::bind(&MotionVelocityPlannerNode::on_acceleration, this, _1), create_subscription_options(this)); - sub_lanelet_map_ = this->create_subscription( + sub_lanelet_map_ = this->create_subscription( "~/input/vector_map", rclcpp::QoS(10).transient_local(), std::bind(&MotionVelocityPlannerNode::on_lanelet_map, this, _1), create_subscription_options(this)); sub_traffic_signals_ = - this->create_subscription( + this->create_subscription( "~/input/traffic_signals", 1, std::bind(&MotionVelocityPlannerNode::on_traffic_signals, this, _1), create_subscription_options(this)); @@ -104,7 +104,7 @@ MotionVelocityPlannerNode::MotionVelocityPlannerNode(const rclcpp::NodeOptions & // Publishers trajectory_pub_ = - this->create_publisher("~/output/trajectory", 1); + this->create_publisher("~/output/trajectory", 1); velocity_factor_publisher_ = this->create_publisher( "~/output/velocity_factors", 1); @@ -183,7 +183,7 @@ void MotionVelocityPlannerNode::on_occupancy_grid( } void MotionVelocityPlannerNode::on_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.predicted_objects = msg; @@ -245,7 +245,7 @@ void MotionVelocityPlannerNode::set_velocity_smoother_params() } void MotionVelocityPlannerNode::on_lanelet_map( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -254,7 +254,7 @@ void MotionVelocityPlannerNode::on_lanelet_map( } void MotionVelocityPlannerNode::on_traffic_signals( - const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(mutex_); @@ -263,27 +263,29 @@ void MotionVelocityPlannerNode::on_traffic_signals( const auto traffic_light_id_map_last_observed_old = planner_data_.traffic_light_id_map_last_observed_; planner_data_.traffic_light_id_map_last_observed_.clear(); - for (const auto & signal : msg->signals) { + for (const auto & signal : msg->traffic_light_groups) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; traffic_signal.signal = signal; - planner_data_.traffic_light_id_map_raw_[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_raw_[signal.traffic_light_group_id] = traffic_signal; const bool is_unknown_observation = std::any_of(signal.elements.begin(), signal.elements.end(), [](const auto & element) { - return element.color == autoware_perception_msgs::msg::TrafficSignalElement::UNKNOWN; + return element.color == autoware_perception_msgs::msg::TrafficLightElement::UNKNOWN; }); // if the observation is UNKNOWN and past observation is available, only update the timestamp // and keep the body of the info - const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + const auto old_data = + traffic_light_id_map_last_observed_old.find(signal.traffic_light_group_id); if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { // copy last observation - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = old_data->second; // update timestamp - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id].stamp = msg->stamp; } else { - planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_light_group_id] = + traffic_signal; } } } @@ -296,7 +298,7 @@ void MotionVelocityPlannerNode::on_virtual_traffic_light_states( } void MotionVelocityPlannerNode::on_trajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg) { std::unique_lock lk(mutex_); @@ -328,7 +330,7 @@ void MotionVelocityPlannerNode::on_trajectory( } void MotionVelocityPlannerNode::insert_stop( - autoware_auto_planning_msgs::msg::Trajectory & trajectory, + autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const { const auto seg_idx = motion_utils::findNearestSegmentIndex(trajectory.points, stop_point); @@ -342,7 +344,7 @@ void MotionVelocityPlannerNode::insert_stop( } void MotionVelocityPlannerNode::insert_slowdown( - autoware_auto_planning_msgs::msg::Trajectory & trajectory, + autoware_planning_msgs::msg::Trajectory & trajectory, const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const { const auto from_seg_idx = @@ -403,10 +405,10 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s return traj_smoothed; } -autoware_auto_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( +autoware_planning_msgs::msg::Trajectory MotionVelocityPlannerNode::generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points) { - autoware_auto_planning_msgs::msg::Trajectory output_trajectory_msg; + autoware_planning_msgs::msg::Trajectory output_trajectory_msg; output_trajectory_msg.points = {input_trajectory_points.begin(), input_trajectory_points.end()}; if (smooth_velocity_before_planning_) input_trajectory_points = smooth_trajectory(input_trajectory_points, planner_data_); diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp index ecf128bf9a012..4b943dcbb67f2 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.hpp @@ -24,9 +24,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -42,11 +42,11 @@ namespace autoware::motion_velocity_planner { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_motion_velocity_planner_node::srv::LoadPlugin; using autoware_motion_velocity_planner_node::srv::UnloadPlugin; -using TrajectoryPoints = std::vector; +using autoware_planning_msgs::msg::Trajectory; +using TrajectoryPoints = std::vector; class MotionVelocityPlannerNode : public rclcpp::Node { @@ -59,35 +59,35 @@ class MotionVelocityPlannerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; // subscriber - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Subscription::SharedPtr sub_predicted_objects_; rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; rclcpp::Subscription::SharedPtr sub_acceleration_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr sub_traffic_signals_; rclcpp::Subscription::SharedPtr sub_virtual_traffic_light_states_; rclcpp::Subscription::SharedPtr sub_occupancy_grid_; void on_trajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_trajectory_msg); void on_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void on_no_ground_pointcloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void on_odometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void on_acceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); - void on_lanelet_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_lanelet_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); void on_traffic_signals( - const autoware_perception_msgs::msg::TrafficSignalArray::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); void on_virtual_traffic_light_states( const tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr msg); void on_occupancy_grid(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg); // publishers - rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr debug_viz_pub_; rclcpp::Publisher::SharedPtr velocity_factor_publisher_; @@ -101,7 +101,7 @@ class MotionVelocityPlannerNode : public rclcpp::Node // members PlannerData planner_data_; MotionVelocityPlannerManager planner_manager_; - HADMapBin::ConstSharedPtr map_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr_{nullptr}; bool has_received_map_ = false; rclcpp::Service::SharedPtr srv_load_plugin_; @@ -120,15 +120,15 @@ class MotionVelocityPlannerNode : public rclcpp::Node // function bool is_data_ready() const; void insert_stop( - autoware_auto_planning_msgs::msg::Trajectory & trajectory, + autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Point & stop_point) const; void insert_slowdown( - autoware_auto_planning_msgs::msg::Trajectory & trajectory, + autoware_planning_msgs::msg::Trajectory & trajectory, const autoware::motion_velocity_planner::SlowdownInterval & slowdown_interval) const; autoware::motion_velocity_planner::TrajectoryPoints smooth_trajectory( const autoware::motion_velocity_planner::TrajectoryPoints & trajectory_points, const autoware::motion_velocity_planner::PlannerData & planner_data) const; - autoware_auto_planning_msgs::msg::Trajectory generate_trajectory( + autoware_planning_msgs::msg::Trajectory generate_trajectory( autoware::motion_velocity_planner::TrajectoryPoints input_trajectory_points); std::unique_ptr logger_configure_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp index fa3a3b1ae5dcb..66063fcbaebca 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.cpp @@ -72,7 +72,7 @@ void MotionVelocityPlannerManager::update_module_parameters( } std::vector MotionVelocityPlannerManager::plan_velocities( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data) { std::vector results; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp index a5e330535fc73..70af1041ec222 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/planner_manager.hpp @@ -20,9 +20,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -45,7 +45,7 @@ class MotionVelocityPlannerManager void unload_module_plugin(rclcpp::Node & node, const std::string & name); void update_module_parameters(const std::vector & parameters); std::vector plan_velocities( - const std::vector & ego_trajectory_points, + const std::vector & ego_trajectory_points, const std::shared_ptr planner_data); private: diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp index 32262ab7eec35..6a6bd2c0dcf19 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_data.hpp @@ -15,7 +15,7 @@ #ifndef OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ #define OBJECTS_OF_INTEREST_MARKER_INTERFACE__MARKER_DATA_HPP_ -#include +#include #include #include @@ -24,7 +24,7 @@ namespace objects_of_interest_marker_interface struct ObjectMarkerData { geometry_msgs::msg::Pose pose{}; - autoware_auto_perception_msgs::msg::Shape shape{}; + autoware_perception_msgs::msg::Shape shape{}; std_msgs::msg::ColorRGBA color; }; diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp index c1fae5ecc4bec..5a094e349c31a 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/marker_utils.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include diff --git a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp index d5a19fede4fc2..7138ddd49d4c7 100644 --- a/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp +++ b/planning/objects_of_interest_marker_interface/include/objects_of_interest_marker_interface/objects_of_interest_marker_interface.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include #include @@ -47,7 +47,7 @@ class ObjectsOfInterestMarkerInterface * @param color_name Color name */ void insertObjectData( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const ColorName & color_name); /** @@ -57,7 +57,7 @@ class ObjectsOfInterestMarkerInterface * @param color Color data with alpha */ void insertObjectDataWithCustomColor( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape, const std_msgs::msg::ColorRGBA & color); /** diff --git a/planning/objects_of_interest_marker_interface/package.xml b/planning/objects_of_interest_marker_interface/package.xml index 2cdb0fe2c9a46..28d00e2fdcd95 100644 --- a/planning/objects_of_interest_marker_interface/package.xml +++ b/planning/objects_of_interest_marker_interface/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs rclcpp std_msgs diff --git a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp index 03e71193cc5a5..f12731aba5df2 100644 --- a/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp +++ b/planning/objects_of_interest_marker_interface/src/objects_of_interest_marker_interface.cpp @@ -20,7 +20,7 @@ namespace objects_of_interest_marker_interface { -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::Pose; using std_msgs::msg::ColorRGBA; using visualization_msgs::msg::Marker; diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 8016107d96911..23be6908cbcfe 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -15,17 +15,17 @@ The `obstacle_cruise_planner` package has following modules. ### Input topics -| Name | Type | Description | -| -------------------- | ----------------------------------------------- | ---------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | -| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | +| Name | Type | Description | +| -------------------- | ------------------------------------------ | ---------------- | +| `~/input/trajectory` | autoware_planning_msgs::Trajectory | input trajectory | +| `~/input/objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | ### Output topics | Name | Type | Description | | ------------------------------- | ---------------------------------------------- | ------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | +| `~/output/trajectory` | autoware_planning_msgs::Trajectory | output trajectory | | `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | | `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | | `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | @@ -46,7 +46,7 @@ This planner data is created first, and then sent to the planning algorithm. struct PlannerData { rclcpp::Time current_time; - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; geometry_msgs::msg::Pose current_pose; double ego_vel; double current_acc; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index d46062dd8f85c..979cef8610cbd 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -19,9 +19,9 @@ #include "autoware_adapi_v1_msgs/msg/planning_behavior.hpp" #include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -38,13 +38,13 @@ using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::AccelStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index eaba45a31869a..e394add65b05d 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -18,8 +18,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py index 6a11c916124e9..e3ede845a842d 100755 --- a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py +++ b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py @@ -15,7 +15,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory from geometry_msgs.msg import Pose from geometry_msgs.msg import Twist from geometry_msgs.msg import TwistStamped diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index ef5b0facd236e..c922874594f54 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -13,21 +13,21 @@ ### Input topics -| Name | Type | Description | -| --------------------------- | ----------------------------------------------- | ------------------- | -| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | -| `~/input/vector_map` | autoware_auto_mapping_msgs::HADMapBin | vector map | -| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | -| `~/input/dynamic_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | +| Name | Type | Description | +| --------------------------- | ------------------------------------------ | ------------------- | +| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | +| `~/input/trajectory` | autoware_planning_msgs::Trajectory | trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | +| `~/input/dynamic_objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | ### Output topics -| Name | Type | Description | -| ---------------------- | --------------------------------------- | -------------------------------------- | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | -| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | +| Name | Type | Description | +| ---------------------- | ------------------------------------ | -------------------------------------- | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | +| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that cause the vehicle to stop | ### Common Parameter diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 2c7308e576129..55501cf596b07 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include #include @@ -31,9 +31,9 @@ namespace motion_planning { -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; class AdaptiveCruiseController { public: @@ -45,7 +45,7 @@ class AdaptiveCruiseController const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); @@ -192,7 +192,7 @@ class AdaptiveCruiseController const std_msgs::msg::Header & trajectory_header); double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx); std::optional estimatePointVelocityFromObject( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const double traj_yaw, const pcl::PointXYZ & nearest_collision_point); std::optional estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index d894544a67fe1..723d7e8d12e56 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -29,8 +29,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -77,9 +77,9 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; @@ -94,7 +94,7 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; struct ObstacleWithDetectionTime { explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 6b8e2b8b3ec9d..1d1e8a3a9844d 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include #include @@ -33,7 +33,7 @@ namespace motion_planning using diagnostic_msgs::msg::DiagnosticStatus; using geometry_msgs::msg::Pose; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; struct StopPoint { @@ -256,7 +256,7 @@ struct PlannerData pcl::PointXYZ lateral_nearest_slow_down_point; - autoware_auto_perception_msgs::msg::Shape slow_down_object_shape{}; + autoware_perception_msgs::msg::Shape slow_down_object_shape{}; rclcpp::Time nearest_collision_point_time{}; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index d4a7d92d8f161..b43e51873c09e 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -45,15 +45,15 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseArray; using std_msgs::msg::Header; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using PointVariant = std::variant; std::optional> calcFeasibleMarginAndVelocity( @@ -111,10 +111,10 @@ void getLateralNearestPointForPredictedObject( Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertBoundingBoxObjectToGeometryPolygon( const Pose & current_pose, const double & base_to_front, const double & base_to_rear, @@ -145,7 +145,7 @@ std::optional getObstacleFromUuid( bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr); diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 1a6f8433875ce..fcf09b96c1345 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -21,8 +21,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager diagnostic_msgs motion_utils diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 965d52a882568..2931da5830a60 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -198,7 +198,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header) { @@ -306,7 +306,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( calcUpperVelocity(col_point_distance, point_velocity, current_velocity); pub_debug_->publish(debug_values_); - if (target_object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (target_object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // if the target object is obstacle return stop true RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), @@ -399,7 +399,7 @@ double AdaptiveCruiseController::calcTrajYaw( } std::optional AdaptiveCruiseController::estimatePointVelocityFromObject( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const double traj_yaw, const pcl::PointXYZ & nearest_collision_point) { geometry_msgs::msg::Point nearest_collision_p_ros; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 8f57b3e0f051a..31c044c93af22 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -40,7 +40,7 @@ namespace motion_planning { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; @@ -628,7 +628,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_slow_down_range; bool found_slow_down_object = false; Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -636,7 +636,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( slow_down_param_.pedestrian_lateral_margin); found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.vehicle_lateral_margin); @@ -647,7 +647,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.unknown_lateral_margin); @@ -722,15 +722,15 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_slow_down_vehicle_polygon; const auto & obj = filtered_objects.objects.at(nearest_slow_down_object_index); - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.pedestrian_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.vehicle_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.unknown_lateral_margin); @@ -763,7 +763,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_collision_polygon; bool found_collision_object = false; Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); @@ -772,7 +772,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.pedestrian_lateral_margin); found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = convertBoundingBoxObjectToGeometryPolygon( @@ -782,7 +782,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.vehicle_lateral_margin); found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -846,7 +846,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d object_polygon{}; Polygon2d one_step_move_vehicle_polygon; bool found_collision_object = false; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); @@ -855,7 +855,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.pedestrian_lateral_margin); found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = convertBoundingBoxObjectToGeometryPolygon( @@ -865,7 +865,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.vehicle_lateral_margin); found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -1128,7 +1128,7 @@ void ObstacleStopPlannerNode::insertVelocity( if (node_param_.use_predicted_objects) { if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + autoware_perception_msgs::msg::Shape::CYLINDER) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * @@ -1136,7 +1136,7 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_param_.pedestrian_lateral_margin; } else if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * @@ -1144,7 +1144,7 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_param_.vehicle_lateral_margin; } else if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::POLYGON) { + autoware_perception_msgs::msg::Shape::POLYGON) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 31fb843c234d9..3e2ecca8ec4d7 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -32,8 +32,8 @@ namespace motion_planning { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -632,7 +632,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( } Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; @@ -655,7 +655,7 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; tf2::Transform tf_map2obj; @@ -698,13 +698,13 @@ bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & ob return base_pose_vec.dot(obstacle_vec) >= 0; } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index 36aa35bb31156..9afe338cc17e0 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -146,22 +146,22 @@ For example a value of `1` means all trajectory points will be evaluated while a ### Inputs -| Name | Type | Description | -| ----------------------------- | ------------------------------------------------ | -------------------------------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | -| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | -| `~/input/dynamic_obstacles` | `autoware_auto_perception_msgs/PredictedObjects` | Dynamic objects | -| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | -| `~/input/map` | `autoware_auto_mapping_msgs/HADMapBin` | Vector map used to retrieve static obstacles | +| Name | Type | Description | +| ----------------------------- | ------------------------------------------- | -------------------------------------------------- | +| `~/input/trajectory` | `autoware_planning_msgs/Trajectory` | Reference trajectory | +| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | +| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | +| `~/input/dynamic_obstacles` | `autoware_perception_msgs/PredictedObjects` | Dynamic objects | +| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | +| `~/input/map` | `autoware_map_msgs/LaneletMapBin` | Vector map used to retrieve static obstacles | ### Outputs -| Name | Type | Description | -| ------------------------------- | ---------------------------------------- | -------------------------------------------------------- | -| `~/output/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Trajectory with adjusted velocities | -| `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | -| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | +| Name | Type | Description | +| ------------------------------- | ----------------------------------- | -------------------------------------------------------- | +| `~/output/trajectory` | `autoware_planning_msgs/Trajectory` | Trajectory with adjusted velocities | +| `~/output/debug_markers` | `visualization_msgs/MarkerArray` | Debug markers (envelopes, obstacle polygons) | +| `~/output/runtime_microseconds` | `tier4_debug_msgs/Float64` | Time taken to calculate the trajectory (in microseconds) | ## Parameters diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp index 3f5f8cd814ea3..d3074e3fb1b45 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/forward_projection.hpp @@ -18,7 +18,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -#include +#include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp index 4d89d4b440c21..0beced62bd7d6 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -19,7 +19,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -#include +#include #include #include @@ -63,8 +63,8 @@ void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, - const Float buffer, const Float min_vel); + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, + const Float min_vel); /// @brief create footprint polygons from projection lines /// @details A footprint is create for each group of lines. Each group of lines is assumed to share diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 63eec0f2cf378..f0b9d34413c9a 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -69,7 +69,7 @@ class ObstacleVelocityLimiterNode : public rclcpp::Node sub_pointcloud_; //!< @brief subscriber for obstacle pointcloud rclcpp::Subscription::SharedPtr sub_odom_; //!< @brief subscriber for the current velocity - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; // cached inputs PredictedObjects::ConstSharedPtr dynamic_obstacles_ptr_; diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp index 8620c254968fe..618437edc981a 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include @@ -163,7 +163,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp index e44011e1f0ee3..6bd1b39b02f73 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -17,17 +17,17 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include +#include #include #include namespace obstacle_velocity_limiter { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using nav_msgs::msg::OccupancyGrid; using PointCloud = sensor_msgs::msg::PointCloud2; using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml index 8c9c89094f5f0..49e424bfbc9ef 100644 --- a/planning/obstacle_velocity_limiter/package.xml +++ b/planning/obstacle_velocity_limiter/package.xml @@ -11,8 +11,8 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs autoware_planning_test_manager eigen grid_map_msgs diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index a54b238ad56e1..2e56e8034555a 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -14,7 +14,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_planning_msgs.msg import Trajectory +from autoware_planning_msgs.msg import Trajectory import matplotlib.pyplot as plt from nav_msgs.msg import Odometry import numpy as np diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp index ac1b4bde65211..7c050b078c3aa 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp @@ -35,8 +35,8 @@ Float calculateSafeVelocity( } multi_polygon_t createPolygonMasks( - const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, - const Float buffer, const Float min_vel) + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, + const Float min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); } diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index ea08c2f7cf552..18cf534da8c7e 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -57,9 +57,9 @@ ObstacleVelocityLimiterNode::ObstacleVelocityLimiterNode(const rclcpp::NodeOptio sub_odom_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, [this](const nav_msgs::msg::Odometry::ConstSharedPtr msg) { current_odometry_ptr_ = msg; }); - map_sub_ = create_subscription( + map_sub_ = create_subscription( "~/input/map", rclcpp::QoS{1}.transient_local(), - [this](const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) { + [this](const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); static_map_obstacles_ = extractStaticObstacles(*lanelet_map_ptr_, obstacle_params_.static_map_tags); diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp index 603e573e68f17..5d4c1d29665df 100644 --- a/planning/obstacle_velocity_limiter/src/obstacles.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -59,7 +59,7 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp index c5e69e5fd8fb9..d4f44e7a6a775 100644 --- a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -17,8 +17,8 @@ #include "obstacle_velocity_limiter/types.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -384,8 +384,8 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { - using autoware_auto_perception_msgs::msg::PredictedObject; - using autoware_auto_perception_msgs::msg::PredictedObjects; + using autoware_perception_msgs::msg::PredictedObject; + using autoware_perception_msgs::msg::PredictedObjects; using obstacle_velocity_limiter::createObjectPolygons; PredictedObjects objects; diff --git a/planning/path_smoother/include/path_smoother/type_alias.hpp b/planning/path_smoother/include/path_smoother/type_alias.hpp index c8f6d6da98dcd..21cbcf8e7eefc 100644 --- a/planning/path_smoother/include/path_smoother/type_alias.hpp +++ b/planning/path_smoother/include/path_smoother/type_alias.hpp @@ -15,10 +15,10 @@ #ifndef PATH_SMOOTHER__TYPE_ALIAS_HPP_ #define PATH_SMOOTHER__TYPE_ALIAS_HPP_ -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "nav_msgs/msg/odometry.hpp" #include "std_msgs/msg/header.hpp" @@ -30,10 +30,10 @@ namespace path_smoother // std_msgs using std_msgs::msg::Header; // planning -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; // navigation using nav_msgs::msg::Odometry; // debug diff --git a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp index 067f5d2c57cc4..55a7829a5bea3 100644 --- a/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp +++ b/planning/path_smoother/include/path_smoother/utils/trajectory_utils.hpp @@ -24,8 +24,8 @@ #include -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/path_smoother/package.xml b/planning/path_smoother/package.xml index 0cba14254e492..a0e83fdf8a091 100644 --- a/planning/path_smoother/package.xml +++ b/planning/path_smoother/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager geometry_msgs interpolation diff --git a/planning/path_smoother/src/utils/trajectory_utils.cpp b/planning/path_smoother/src/utils/trajectory_utils.cpp index 6409c61700f34..b4d0950a593d5 100644 --- a/planning/path_smoother/src/utils/trajectory_utils.cpp +++ b/planning/path_smoother/src/utils/trajectory_utils.cpp @@ -18,8 +18,8 @@ #include "motion_utils/trajectory/conversion.hpp" #include "path_smoother/utils/geometry_utils.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index 43c3e029d4437..9d787070a9dc5 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -26,19 +26,19 @@ #include #include -#include -#include -#include -#include +#include #include #include #include +#include #include #include #include #include #include #include +#include +#include #include #include @@ -57,14 +57,14 @@ namespace test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletPrimitive; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; using RouteSections = std::vector; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -137,18 +137,18 @@ LaneletSegment createLaneletSegment(int id); lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename); /** - * @brief Converts a Lanelet map to a HADMapBin message. + * @brief Converts a Lanelet map to a LaneletMapBin message. * - * This function converts a given Lanelet map to a HADMapBin message. It also + * This function converts a given Lanelet map to a LaneletMapBin message. It also * parses the format and map versions from the specified filename and includes * them in the message. The timestamp for the message is set to the provided time. * * @param map The Lanelet map to convert. * @param lanelet2_filename The filename of the Lanelet2 map, used to parse format and map versions. * @param now The current time to set in the message header. - * @return A HADMapBin message containing the converted map data. + * @return A LaneletMapBin message containing the converted map data. */ -HADMapBin convertToMapBinMsg( +LaneletMapBin convertToMapBinMsg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -175,28 +175,28 @@ LaneletRoute makeNormalRoute(); OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2); /** - * @brief Creates a HADMapBin message from a Lanelet map file. + * @brief Creates a LaneletMapBin message from a Lanelet map file. * * This function loads a Lanelet map from the given file, overwrites the - * centerline with the specified resolution, and converts the map to a HADMapBin message. + * centerline with the specified resolution, and converts the map to a LaneletMapBin message. * * @param absolute_path The absolute path to the Lanelet2 map file. * @param center_line_resolution The resolution for the centerline. - * @return A HADMapBin message containing the map data. + * @return A LaneletMapBin message containing the map data. */ -HADMapBin make_map_bin_msg( +LaneletMapBin make_map_bin_msg( const std::string & absolute_path, const double center_line_resolution = 5.0); /** - * @brief Creates a HADMapBin message using a predefined Lanelet2 map file. + * @brief Creates a LaneletMapBin message using a predefined Lanelet2 map file. * * This function loads a lanelet2_map.osm from the test_map folder in the * planning_test_utils package, overwrites the centerline with a resolution of 5.0, - * and converts the map to a HADMapBin message. + * and converts the map to a LaneletMapBin message. * - * @return A HADMapBin message containing the map data. + * @return A LaneletMapBin message containing the map data. */ -HADMapBin makeMapBinMsg(); +LaneletMapBin makeMapBinMsg(); /** * @brief Creates an Odometry message with a specified shift. @@ -366,7 +366,7 @@ void createPublisherWithQoS( std::shared_ptr> & publisher) { if constexpr ( - std::is_same_v || std::is_same_v || + std::is_same_v || std::is_same_v || std::is_same_v) { rclcpp::QoS qos(rclcpp::KeepLast(1)); qos.reliable(); diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index b236a64f48da0..8f3c3259c33d0 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -15,12 +15,11 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_mapping_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_map_msgs autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils lanelet2_extension diff --git a/planning/planning_test_utils/src/planning_test_utils.cpp b/planning/planning_test_utils/src/planning_test_utils.cpp index efed22855cf0f..3ba97552ef250 100644 --- a/planning/planning_test_utils/src/planning_test_utils.cpp +++ b/planning/planning_test_utils/src/planning_test_utils.cpp @@ -56,7 +56,7 @@ lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) return nullptr; } -HADMapBin convertToMapBinMsg( +LaneletMapBin convertToMapBinMsg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { std::string format_version{}; @@ -64,11 +64,11 @@ HADMapBin convertToMapBinMsg( lanelet::io_handlers::AutowareOsmParser::parseVersions( lanelet2_filename, &format_version, &map_version); - HADMapBin map_bin_msg; + LaneletMapBin map_bin_msg; map_bin_msg.header.stamp = now; map_bin_msg.header.frame_id = "map"; - map_bin_msg.format_version = format_version; - map_bin_msg.map_version = map_version; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); return map_bin_msg; @@ -103,11 +103,12 @@ OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution) return costmap_msg; } -HADMapBin make_map_bin_msg(const std::string & absolute_path, const double center_line_resolution) +LaneletMapBin make_map_bin_msg( + const std::string & absolute_path, const double center_line_resolution) { const auto map = loadMap(absolute_path); if (!map) { - return autoware_auto_mapping_msgs::msg::HADMapBin_>{}; + return autoware_map_msgs::msg::LaneletMapBin_>{}; } // overwrite centerline @@ -119,7 +120,7 @@ HADMapBin make_map_bin_msg(const std::string & absolute_path, const double cente return map_bin_msg; } -HADMapBin makeMapBinMsg() +LaneletMapBin makeMapBinMsg() { const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); diff --git a/planning/planning_topic_converter/README.md b/planning/planning_topic_converter/README.md index 7be6979e62281..74137cda0197d 100644 --- a/planning/planning_topic_converter/README.md +++ b/planning/planning_topic_converter/README.md @@ -2,7 +2,7 @@ ## Purpose -This package provides tools that convert topic type among types are defined in . +This package provides tools that convert topic type among types are defined in . ## Inner-workings / Algorithms diff --git a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp b/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp index f214f2a4d5e2a..708f624e97898 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp +++ b/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp @@ -18,18 +18,18 @@ #include "planning_topic_converter/converter_base.hpp" #include "rclcpp/rclcpp.hpp" -#include -#include +#include +#include #include namespace planning_topic_converter { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; class PathToTrajectory : public ConverterBase { diff --git a/planning/planning_topic_converter/package.xml b/planning/planning_topic_converter/package.xml index d07d59f3d2c89..3699f3e53900c 100644 --- a/planning/planning_topic_converter/package.xml +++ b/planning/planning_topic_converter/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto - autoware_auto_planning_msgs + autoware_planning_msgs motion_utils rclcpp rclcpp_components diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt index 709e00c5a9a25..455dded2d7e32 100644 --- a/planning/planning_validator/CMakeLists.txt +++ b/planning/planning_validator/CMakeLists.txt @@ -56,7 +56,7 @@ if(BUILD_TESTING) ) ament_target_dependencies(test_planning_validator rclcpp - autoware_auto_planning_msgs + autoware_planning_msgs ) target_link_libraries(test_planning_validator planning_validator_component diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index a260254159947..9d70e7f78a5bb 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -31,10 +31,10 @@ The following features are to be implemented. The `planning_validator` takes in the following inputs: -| Name | Type | Description | -| -------------------- | -------------------------------------- | ---------------------------------------------- | -| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | -| `~/input/trajectory` | autoware_auto_planning_msgs/Trajectory | target trajectory to be validated in this node | +| Name | Type | Description | +| -------------------- | --------------------------------- | ---------------------------------------------- | +| `~/input/kinematics` | nav_msgs/Odometry | ego pose and twist | +| `~/input/trajectory` | autoware_planning_msgs/Trajectory | target trajectory to be validated in this node | ### Outputs @@ -42,7 +42,7 @@ It outputs the following: | Name | Type | Description | | ---------------------------- | ------------------------------------------ | ------------------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/Trajectory | validated trajectory | +| `~/output/trajectory` | autoware_planning_msgs/Trajectory | validated trajectory | | `~/output/validation_status` | planning_validator/PlanningValidatorStatus | validator status to inform the reason why the trajectory is valid/invalid | | `/diagnostics` | diagnostic_msgs/DiagnosticStatus | diagnostics to report errors | diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/planning_validator/debug_marker.hpp index 4c7f6907a97b2..4671a0b0d7297 100644 --- a/planning/planning_validator/include/planning_validator/debug_marker.hpp +++ b/planning/planning_validator/include/planning_validator/debug_marker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -32,8 +32,7 @@ class PlanningValidatorDebugMarkerPublisher explicit PlanningValidatorDebugMarkerPublisher(rclcpp::Node * node); void pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, - int id = 0); + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0); void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0); void pushVirtualWall(const geometry_msgs::msg::Pose & pose); void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg); diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index 36954c267b2a9..fed6e161c4d90 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -35,8 +35,8 @@ namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 5831993d748b6..11eaf4f37f400 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -25,8 +25,8 @@ namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; std::pair getMaxValAndIdx(const std::vector & v); diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 9ecc760efd7e3..5a413f27dc7a3 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -15,7 +15,7 @@ autoware_cmake rosidl_default_generators - autoware_auto_planning_msgs + autoware_planning_msgs autoware_planning_test_manager diagnostic_updater geometry_msgs diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 1a0793957402a..82117d199c36e 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -40,7 +40,7 @@ void PlanningValidatorDebugMarkerPublisher::clearMarkers() } void PlanningValidatorDebugMarkerPublisher::pushPoseMarker( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) + const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id) { pushPoseMarker(p.pose, ns, id); } diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp index 421b60d0ab756..1d1e47ad10407 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp @@ -17,14 +17,14 @@ #include -#include +#include #include namespace planning_validator { -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; class InvalidTrajectoryPublisherNode : public rclcpp::Node { diff --git a/planning/planning_validator/test/src/test_planning_validator_functions.cpp b/planning/planning_validator/test/src/test_planning_validator_functions.cpp index 482996c4c040d..e406b6c5d7da2 100644 --- a/planning/planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -21,7 +21,7 @@ #include -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using planning_validator::PlanningValidator; TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.cpp b/planning/planning_validator/test/src/test_planning_validator_helper.cpp index 19e0d5a7f73ee..de7d8293286d7 100644 --- a/planning/planning_validator/test/src/test_planning_validator_helper.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_helper.cpp @@ -19,8 +19,8 @@ #include -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createQuaternionFromYaw; Trajectory generateTrajectoryWithConstantAcceleration( diff --git a/planning/planning_validator/test/src/test_planning_validator_helper.hpp b/planning/planning_validator/test/src/test_planning_validator_helper.hpp index d2e6472fb04bc..23b634c3f56ed 100644 --- a/planning/planning_validator/test/src/test_planning_validator_helper.hpp +++ b/planning/planning_validator/test/src/test_planning_validator_helper.hpp @@ -17,11 +17,11 @@ #include -#include -#include +#include +#include #include -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using nav_msgs::msg::Odometry; Trajectory generateTrajectoryWithConstantAcceleration( diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index a7b90be4471cd..2329a3b9bb6c8 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -29,7 +29,7 @@ * This test checks the diagnostics message published from the planning_validator node */ -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 73bf99962ea82..24a20f86ed718 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -17,11 +17,11 @@ #include -#include -#include +#include #include #include #include +#include #include #include @@ -37,13 +37,13 @@ namespace route_handler { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using std_msgs::msg::Header; +using tier4_planning_msgs::msg::PathWithLaneId; using unique_identifier_msgs::msg::UUID; using RouteSections = std::vector; @@ -55,10 +55,10 @@ class RouteHandler { public: RouteHandler() = default; - explicit RouteHandler(const HADMapBin & map_msg); + explicit RouteHandler(const LaneletMapBin & map_msg); // non-const methods - void setMap(const HADMapBin & map_msg); + void setMap(const LaneletMapBin & map_msg); void setRoute(const LaneletRoute & route_msg); void setRouteLanelets(const lanelet::ConstLanelets & path_lanelets); void clearRoute(); diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 097bc62f8bf39..8be6707e933d7 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -22,8 +22,7 @@ ament_lint_auto autoware_lint_common - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs autoware_planning_msgs geometry_msgs lanelet2_extension diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 4dd559b9bbc98..d410cb2668175 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -22,9 +22,9 @@ #include #include -#include -#include #include +#include +#include #include #include @@ -43,12 +43,12 @@ namespace { -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletPrimitive; +using autoware_planning_msgs::msg::Path; using geometry_msgs::msg::Pose; using lanelet::utils::to2D; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; bool exists(const std::vector & primitives, const int64_t & id) { @@ -133,13 +133,13 @@ std::string toString(const geometry_msgs::msg::Pose & pose) namespace route_handler { -RouteHandler::RouteHandler(const HADMapBin & map_msg) +RouteHandler::RouteHandler(const LaneletMapBin & map_msg) { setMap(map_msg); route_ptr_ = nullptr; } -void RouteHandler::setMap(const HADMapBin & map_msg) +void RouteHandler::setMap(const LaneletMapBin & map_msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( diff --git a/planning/route_handler/test/test_route_handler.hpp b/planning/route_handler/test/test_route_handler.hpp index 3c6893da3f9d1..9f9fba717a4aa 100644 --- a/planning/route_handler/test/test_route_handler.hpp +++ b/planning/route_handler/test/test_route_handler.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include @@ -37,7 +37,7 @@ namespace route_handler::test { -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; class TestRouteHandler : public ::testing::Test { diff --git a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp index 5b395883dd65d..6f0bef882743c 100644 --- a/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp +++ b/planning/sampling_based_planner/frenet_planner/src/frenet_planner/frenet_planner.cpp @@ -22,7 +22,7 @@ #include #include -#include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path.hpp" #include #include diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index b82896f4fd93f..27009719c230c 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -19,17 +19,17 @@ Note that the velocity is just taken over from the input path. ### input -| Name | Type | Description | -| ------------------ | -------------------------------------------------- | -------------------------------------------------- | -| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | -| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | -| `~/input/objects` | autoware_auto_perception_msgs/msg/PredictedObjects | objects to avoid | +| Name | Type | Description | +| ------------------ | --------------------------------------------- | -------------------------------------------------- | +| `~/input/path` | autoware_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | +| `~/input/objects` | autoware_perception_msgs/msg/PredictedObjects | objects to avoid | ### output -| Name | Type | Description | -| --------------------- | ------------------------------------------ | ----------------------------------------------------------------- | -| `~/output/trajectory` | autoware_auto_planning_msgs/msg/Trajectory | generated trajectory that is feasible to drive and collision-free | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ----------------------------------------------------------------- | +| `~/output/trajectory` | autoware_planning_msgs/msg/Trajectory | generated trajectory that is feasible to drive and collision-free | ## Algorithm 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 index 41493b74659fc..448400236c9f8 100644 --- 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 @@ -22,7 +22,7 @@ #include "sampler_common/structures.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include +#include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp index 29cfddfbf8632..a9a2728b27c49 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp @@ -20,11 +20,11 @@ #include "path_sampler/type_alias.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp index 8a792eba2f237..90e655c086444 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp @@ -15,11 +15,11 @@ #ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ #define PATH_SAMPLER__TYPE_ALIAS_HPP_ -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -33,12 +33,12 @@ namespace path_sampler // std_msgs using std_msgs::msg::Header; // planning -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::Path; +using autoware_planning_msgs::msg::PathPoint; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; // perception -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; // navigation using nav_msgs::msg::Odometry; // visualization diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp index 3e5afdbdad696..875b81b9cd49a 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/geometry_utils.hpp @@ -24,8 +24,8 @@ #include "path_sampler/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include "boost/optional/optional_fwd.hpp" diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp index d848e9b209d28..a036dcfe638c2 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/utils/trajectory_utils.hpp @@ -24,8 +24,8 @@ #include "path_sampler/type_alias.hpp" #include "sampler_common/structures.hpp" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" #include #include diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/path_sampler/package.xml index 22303a021ff63..a57ff6e6033f6 100644 --- a/planning/sampling_based_planner/path_sampler/package.xml +++ b/planning/sampling_based_planner/path_sampler/package.xml @@ -13,7 +13,8 @@ autoware_cmake - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs bezier_sampler frenet_planner geometry_msgs diff --git a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp index 4ae6382b82383..2d1587c7b45a3 100644 --- a/planning/sampling_based_planner/path_sampler/src/path_generation.cpp +++ b/planning/sampling_based_planner/path_sampler/src/path_generation.cpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include diff --git a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp index ad516faa717ff..07bd4f32787de 100644 --- a/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp +++ b/planning/sampling_based_planner/path_sampler/src/utils/trajectory_utils.cpp @@ -19,8 +19,8 @@ #include "path_sampler/utils/geometry_utils.hpp" #include "tf2/utils.h" -#include "autoware_auto_planning_msgs/msg/path_point.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/path_point.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/point32.hpp" #include "geometry_msgs/msg/pose.hpp" diff --git a/planning/scenario_selector/README.md b/planning/scenario_selector/README.md index 0aea2c876cf35..373ac164e5057 100644 --- a/planning/scenario_selector/README.md +++ b/planning/scenario_selector/README.md @@ -6,21 +6,21 @@ ### Input topics -| Name | Type | Description | -| -------------------------------- | --------------------------------------- | ----------------------------------------------------- | -| `~input/lane_driving/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of LaneDriving scenario | -| `~input/parking/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of Parking scenario | -| `~input/lanelet_map` | autoware_auto_mapping_msgs::HADMapBin | | -| `~input/route` | autoware_planning_msgs::LaneletRoute | route and goal pose | -| `~input/odometry` | nav_msgs::Odometry | for checking whether vehicle is stopped | -| `is_parking_completed` | bool (implemented as rosparam) | whether all split trajectory of Parking are published | +| Name | Type | Description | +| -------------------------------- | ------------------------------------- | ----------------------------------------------------- | +| `~input/lane_driving/trajectory` | autoware_planning_msgs::Trajectory | trajectory of LaneDriving scenario | +| `~input/parking/trajectory` | autoware_planning_msgs::Trajectory | trajectory of Parking scenario | +| `~input/lanelet_map` | autoware_map_msgs::msg::LaneletMapBin | | +| `~input/route` | autoware_planning_msgs::LaneletRoute | route and goal pose | +| `~input/odometry` | nav_msgs::Odometry | for checking whether vehicle is stopped | +| `is_parking_completed` | bool (implemented as rosparam) | whether all split trajectory of Parking are published | ### Output topics -| Name | Type | Description | -| -------------------- | --------------------------------------- | ---------------------------------------------- | -| `~output/scenario` | tier4_planning_msgs::Scenario | current scenario and scenarios to be activated | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed | +| Name | Type | Description | +| -------------------- | ---------------------------------- | ---------------------------------------------- | +| `~output/scenario` | tier4_planning_msgs::Scenario | current scenario and scenarios to be activated | +| `~output/trajectory` | autoware_planning_msgs::Trajectory | trajectory to be followed | ### Output TFs diff --git a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp index 3651f6c76b8bc..7162a435d8852 100644 --- a/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp +++ b/planning/scenario_selector/include/scenario_selector/scenario_selector_node.hpp @@ -18,9 +18,9 @@ #include #include -#include -#include +#include #include +#include #include #include #include @@ -48,39 +48,37 @@ class ScenarioSelectorNode : public rclcpp::Node public: explicit ScenarioSelectorNode(const rclcpp::NodeOptions & node_options); - void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr msg); void onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onParkingState(const std_msgs::msg::Bool::ConstSharedPtr msg); bool isDataReady(); void onTimer(); - void onLaneDrivingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void onParkingTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - void publishTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onLaneDrivingTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void onParkingTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void publishTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); void updateCurrentScenario(); std::string selectScenarioByPosition(); - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory( + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory( const std::string & scenario); private: rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Subscription::SharedPtr sub_lanelet_map_; + rclcpp::Subscription::SharedPtr sub_lanelet_map_; rclcpp::Subscription::SharedPtr sub_route_; rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_lane_driving_trajectory_; - rclcpp::Subscription::SharedPtr - sub_parking_trajectory_; + rclcpp::Subscription::SharedPtr sub_parking_trajectory_; rclcpp::Subscription::SharedPtr sub_parking_state_; - rclcpp::Publisher::SharedPtr pub_trajectory_; + rclcpp::Publisher::SharedPtr pub_trajectory_; rclcpp::Publisher::SharedPtr pub_scenario_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_; - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_; autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route_; nav_msgs::msg::Odometry::ConstSharedPtr current_pose_; geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; diff --git a/planning/scenario_selector/package.xml b/planning/scenario_selector/package.xml index 341076505d5b8..f910c09dbb070 100644 --- a/planning/scenario_selector/package.xml +++ b/planning/scenario_selector/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_planning_msgs + autoware_map_msgs + autoware_planning_msgs autoware_planning_test_manager lanelet2_extension nav_msgs diff --git a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp index 7cc251a03296c..2ba56a911bbeb 100644 --- a/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp +++ b/planning/scenario_selector/src/scenario_selector_node/scenario_selector_node.cpp @@ -91,7 +91,7 @@ bool isInParkingLot( } bool isNearTrajectoryEnd( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory, + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory, const geometry_msgs::msg::Pose & current_pose, const double th_dist) { if (!trajectory || trajectory->points.empty()) { @@ -120,8 +120,8 @@ bool isStopped( } // namespace -autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr -ScenarioSelectorNode::getScenarioTrajectory(const std::string & scenario) +autoware_planning_msgs::msg::Trajectory::ConstSharedPtr ScenarioSelectorNode::getScenarioTrajectory( + const std::string & scenario) { if (scenario == tier4_planning_msgs::msg::Scenario::LANEDRIVING) { return lane_driving_trajectory_; @@ -185,8 +185,7 @@ void ScenarioSelectorNode::updateCurrentScenario() } } -void ScenarioSelectorNode::onMap( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) +void ScenarioSelectorNode::onMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg( @@ -290,7 +289,7 @@ void ScenarioSelectorNode::onTimer() } void ScenarioSelectorNode::onLaneDrivingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { lane_driving_trajectory_ = msg; @@ -302,7 +301,7 @@ void ScenarioSelectorNode::onLaneDrivingTrajectory( } void ScenarioSelectorNode::onParkingTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { parking_trajectory_ = msg; @@ -314,7 +313,7 @@ void ScenarioSelectorNode::onParkingTrajectory( } void ScenarioSelectorNode::publishTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { const auto now = this->now(); const auto delay_sec = (now - msg->header.stamp).seconds(); @@ -340,16 +339,15 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti is_parking_completed_(false) { // Input - sub_lane_driving_trajectory_ = - this->create_subscription( - "input/lane_driving/trajectory", rclcpp::QoS{1}, - std::bind(&ScenarioSelectorNode::onLaneDrivingTrajectory, this, std::placeholders::_1)); + sub_lane_driving_trajectory_ = this->create_subscription( + "input/lane_driving/trajectory", rclcpp::QoS{1}, + std::bind(&ScenarioSelectorNode::onLaneDrivingTrajectory, this, std::placeholders::_1)); - sub_parking_trajectory_ = this->create_subscription( + sub_parking_trajectory_ = this->create_subscription( "input/parking/trajectory", rclcpp::QoS{1}, std::bind(&ScenarioSelectorNode::onParkingTrajectory, this, std::placeholders::_1)); - sub_lanelet_map_ = this->create_subscription( + sub_lanelet_map_ = this->create_subscription( "input/lanelet_map", rclcpp::QoS{1}.transient_local(), std::bind(&ScenarioSelectorNode::onMap, this, std::placeholders::_1)); sub_route_ = this->create_subscription( @@ -365,7 +363,7 @@ ScenarioSelectorNode::ScenarioSelectorNode(const rclcpp::NodeOptions & node_opti // Output pub_scenario_ = this->create_publisher("output/scenario", rclcpp::QoS{1}); - pub_trajectory_ = this->create_publisher( + pub_trajectory_ = this->create_publisher( "output/trajectory", rclcpp::QoS{1}); // Timer Callback diff --git a/planning/surround_obstacle_checker/README.md b/planning/surround_obstacle_checker/README.md index 071701c024685..5cf7c8a419b61 100644 --- a/planning/surround_obstacle_checker/README.md +++ b/planning/surround_obstacle_checker/README.md @@ -78,13 +78,13 @@ As mentioned in stop condition section, it prevents chattering by changing thres ### Input -| Name | Type | Description | -| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index a720de68acbcd..d5b2c49df51cb 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -17,12 +17,13 @@ #include "surround_obstacle_checker/debug_marker.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include #include #include -#include +#include #include #include #include @@ -45,8 +46,8 @@ namespace surround_obstacle_checker { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using motion_utils::VehicleStopChecker; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; @@ -110,9 +111,12 @@ class SurroundObstacleCheckerNode : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_; // publisher and subscriber - rclcpp::Subscription::SharedPtr sub_pointcloud_; - rclcpp::Subscription::SharedPtr sub_dynamic_objects_; - rclcpp::Subscription::SharedPtr sub_odometry_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_pointcloud_{this, "~/input/pointcloud"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_dynamic_objects_{ + this, "~/input/objects"}; rclcpp::Publisher::SharedPtr pub_stop_reason_; rclcpp::Publisher::SharedPtr pub_clear_velocity_limit_; rclcpp::Publisher::SharedPtr pub_velocity_limit_; diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 49165f5475ad2..d9b5d4615e148 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,8 +14,8 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs + autoware_perception_msgs + autoware_planning_msgs diagnostic_msgs eigen libpcl-all-dev diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index e499d6b43dea3..af6dba8fcc0c9 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -51,7 +51,7 @@ namespace surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::pose2transform; @@ -206,17 +206,6 @@ SurroundObstacleCheckerNode::SurroundObstacleCheckerNode(const rclcpp::NodeOptio pub_velocity_limit_ = this->create_publisher( "~/output/max_velocity", rclcpp::QoS{1}.transient_local()); - // Subscribers - sub_pointcloud_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&SurroundObstacleCheckerNode::onPointCloud, this, std::placeholders::_1)); - sub_dynamic_objects_ = this->create_subscription( - "~/input/objects", 1, - std::bind(&SurroundObstacleCheckerNode::onDynamicObjects, this, std::placeholders::_1)); - sub_odometry_ = this->create_subscription( - "~/input/odometry", 1, - std::bind(&SurroundObstacleCheckerNode::onOdometry, this, std::placeholders::_1)); - // Parameter callback set_param_res_ = this->add_on_set_parameters_callback( std::bind(&SurroundObstacleCheckerNode::onParam, this, std::placeholders::_1)); @@ -282,6 +271,10 @@ rcl_interfaces::msg::SetParametersResult SurroundObstacleCheckerNode::onParam( void SurroundObstacleCheckerNode::onTimer() { + odometry_ptr_ = sub_odometry_.takeData(); + pointcloud_ptr_ = sub_pointcloud_.takeData(); + object_ptr_ = sub_dynamic_objects_.takeData(); + if (!odometry_ptr_) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), 5000 /* ms */, "waiting for current velocity..."); @@ -377,22 +370,6 @@ void SurroundObstacleCheckerNode::onTimer() debug_ptr_->publish(); } -void SurroundObstacleCheckerNode::onPointCloud( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) -{ - pointcloud_ptr_ = msg; -} - -void SurroundObstacleCheckerNode::onDynamicObjects(const PredictedObjects::ConstSharedPtr msg) -{ - object_ptr_ = msg; -} - -void SurroundObstacleCheckerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - odometry_ptr_ = msg; -} - std::optional SurroundObstacleCheckerNode::getNearestObstacle() const { const auto nearest_pointcloud = getNearestObstacleByPointCloud(); diff --git a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md index 8a72c976621f8..f8a5f29e064f7 100644 --- a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md +++ b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -78,13 +78,13 @@ Stop condition の項で述べたように、状態によって障害物判定 ### Input -| Name | Type | Description | -| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| Name | Type | Description | +| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 0070646a20d49..a8eb88f55df19 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -14,7 +14,6 @@ import launch from launch.actions import DeclareLaunchArgument -from launch.actions import GroupAction from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -78,22 +77,7 @@ def launch_setup(context, *args, **kwargs): ], ) - map_to_odom_tf_publisher = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_map_to_odom_tf_publisher", - output="screen", - arguments=[ - "--frame-id", - "map", - "--child-frame-id", - "odom", - ], - ) - - group = GroupAction([simple_planning_simulator_node, map_to_odom_tf_publisher]) - - return [group] + return [simple_planning_simulator_node] def generate_launch_description(): diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index 098aa0e56b1ae..3845a96108d2b 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -4,7 +4,7 @@ args: node_name_suffix: vector_map topic: /map/vector_map - topic_type: autoware_auto_mapping_msgs/msg/HADMapBin + topic_type: autoware_map_msgs/msg/LaneletMapBin best_effort: false transient_local: true warn_rate: 0.0 @@ -69,7 +69,7 @@ args: node_name_suffix: object_recognition_objects topic: /perception/object_recognition/objects - topic_type: autoware_auto_perception_msgs/msg/PredictedObjects + topic_type: autoware_perception_msgs/msg/PredictedObjects best_effort: false transient_local: false warn_rate: 5.0 @@ -82,7 +82,7 @@ args: node_name_suffix: traffic_light_recognition_traffic_signals topic: /perception/traffic_light_recognition/traffic_signals - topic_type: autoware_perception_msgs/msg/TrafficSignalArray + topic_type: autoware_perception_msgs/msg/TrafficLightGroupArray best_effort: false transient_local: false warn_rate: 5.0 @@ -108,7 +108,7 @@ args: node_name_suffix: scenario_planning_trajectory topic: /planning/scenario_planning/trajectory - topic_type: autoware_auto_planning_msgs/msg/Trajectory + topic_type: autoware_planning_msgs/msg/Trajectory best_effort: false transient_local: false warn_rate: 5.0 @@ -121,7 +121,7 @@ args: node_name_suffix: trajectory_follower_control_cmd topic: /control/trajectory_follower/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 @@ -134,7 +134,7 @@ args: node_name_suffix: control_command_control_cmd topic: /control/command/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 @@ -147,7 +147,7 @@ args: node_name_suffix: vehicle_status_velocity_status topic: /vehicle/status/velocity_status - topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport + topic_type: autoware_vehicle_msgs/msg/VelocityReport best_effort: false transient_local: false warn_rate: 5.0 @@ -160,7 +160,7 @@ args: node_name_suffix: vehicle_status_steering_status topic: /vehicle/status/steering_status - topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport + topic_type: autoware_vehicle_msgs/msg/SteeringReport best_effort: false transient_local: false warn_rate: 5.0 @@ -173,7 +173,7 @@ args: node_name_suffix: system_emergency_control_cmd topic: /system/emergency/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand + topic_type: autoware_control_msgs/msg/Control best_effort: false transient_local: false warn_rate: 5.0 diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index f9b5a167bdab1..77276c7acb0e3 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -15,10 +15,9 @@ autoware_ad_api_specs autoware_adapi_v1_msgs autoware_adapi_version_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs component_interface_specs component_interface_utils diagnostic_graph_utils diff --git a/system/default_ad_api/src/compatibility/autoware_state.hpp b/system/default_ad_api/src/compatibility/autoware_state.hpp index 4cd6233f5df0a..eff74728efcb0 100644 --- a/system/default_ad_api/src/compatibility/autoware_state.hpp +++ b/system/default_ad_api/src/compatibility/autoware_state.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -44,7 +44,7 @@ class AutowareStateNode : public rclcpp::Node Sub sub_routing_; Sub sub_operation_mode_; - using AutowareState = autoware_auto_system_msgs::msg::AutowareState; + using AutowareState = autoware_system_msgs::msg::AutowareState; using LocalizationState = autoware_ad_api::localization::InitializationState::Message; using RoutingState = autoware_ad_api::routing::RouteState::Message; using OperationModeState = autoware_ad_api::operation_mode::OperationModeState::Message; diff --git a/system/default_ad_api/src/perception.cpp b/system/default_ad_api/src/perception.cpp index 61d09444c70a4..794bbf141ca8b 100644 --- a/system/default_ad_api/src/perception.cpp +++ b/system/default_ad_api/src/perception.cpp @@ -24,7 +24,7 @@ using ObjectClassification = autoware_adapi_v1_msgs::msg::ObjectClassification; using DynamicObject = autoware_adapi_v1_msgs::msg::DynamicObject; using DynamicObjectPath = autoware_adapi_v1_msgs::msg::DynamicObjectPath; using API_Shape = shape_msgs::msg::SolidPrimitive; -using Shape = autoware_auto_perception_msgs::msg::Shape; +using Shape = autoware_perception_msgs::msg::Shape; std::unordered_map shape_type_ = { {Shape::BOUNDING_BOX, API_Shape::BOX}, diff --git a/system/default_ad_api/src/perception.hpp b/system/default_ad_api/src/perception.hpp index d1c71fdb08025..ff3e4801b07ac 100644 --- a/system/default_ad_api/src/perception.hpp +++ b/system/default_ad_api/src/perception.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include diff --git a/system/emergency_handler/README.md b/system/emergency_handler/README.md index 3d658dbb4586f..f34ebcf8d9b9a 100644 --- a/system/emergency_handler/README.md +++ b/system/emergency_handler/README.md @@ -14,24 +14,24 @@ Emergency Handler is a node to select proper MRM from from system failure state ### Input -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | -| `/control/vehicle_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used as reference when generate Emergency Control Command | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------------ | ----------------------------------------------------------------------------- | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus | +| `/control/vehicle_cmd` | `autoware_control_msgs::msg::Control` | Used as reference when generate Emergency Control Command | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/api/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/api/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | ### Output -| Name | Type | Description | -| ------------------------------------------ | ------------------------------------------------------ | ----------------------------------------------------- | -| `/system/emergency/shift_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | -| `/system/emergency/hazard_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | -| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | -| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | -| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | +| Name | Type | Description | +| ------------------------------------------ | ------------------------------------------------- | ----------------------------------------------------- | +| `/system/emergency/shift_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/api/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | +| `/system/api/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | +| `/system/api/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | ## Parameters 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 a78b35be26daf..5ff269705456d 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -21,11 +21,11 @@ // Autoware #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include @@ -57,48 +57,43 @@ class EmergencyHandler : public rclcpp::Node private: // Subscribers - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_hazard_status_stamped_; - rclcpp::Subscription::SharedPtr - sub_prev_control_command_; + rclcpp::Subscription::SharedPtr sub_prev_control_command_; rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_mrm_comfortable_stop_status_; rclcpp::Subscription::SharedPtr sub_mrm_emergency_stop_status_; - autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr prev_control_command_; + 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_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + 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_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); - void onPrevControlCommand( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + 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_auto_vehicle_msgs::msg::ControlModeReport::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_; + rclcpp::Publisher::SharedPtr pub_control_command_; // rclcpp::Publisher::SharedPtr pub_shift_; // rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr - pub_hazard_cmd_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; + rclcpp::Publisher::SharedPtr pub_hazard_cmd_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; - autoware_auto_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); - autoware_auto_vehicle_msgs::msg::GearCommand createGearCmdMsg(); + autoware_vehicle_msgs::msg::HazardLightsCommand createHazardCmdMsg(); + autoware_vehicle_msgs::msg::GearCommand createGearCmdMsg(); void publishControlCommands(); rclcpp::Publisher::SharedPtr pub_mrm_state_; diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index 97344f9b8c594..aa2090c86edb8 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -13,9 +13,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components 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 2770a14691d95..b96cb0d0549f9 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -31,18 +31,16 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) using std::placeholders::_1; // Subscriber - 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_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( + 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}, @@ -52,12 +50,12 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); // Publisher - pub_control_command_ = create_publisher( + pub_control_command_ = create_publisher( "~/output/control_command", rclcpp::QoS{1}); - pub_hazard_cmd_ = create_publisher( + pub_hazard_cmd_ = create_publisher( "~/output/hazard", rclcpp::QoS{1}); pub_gear_cmd_ = - create_publisher("~/output/gear", rclcpp::QoS{1}); + create_publisher("~/output/gear", rclcpp::QoS{1}); pub_mrm_state_ = create_publisher("~/output/mrm/state", rclcpp::QoS{1}); @@ -75,9 +73,9 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) // Initialize odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - prev_control_command_ = autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr( - new autoware_auto_control_msgs::msg::AckermannControlCommand); + 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(); @@ -93,19 +91,18 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) } void EmergencyHandler::onHazardStatusStamped( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg) { hazard_status_stamped_ = msg; stamp_hazard_status_ = this->now(); } void EmergencyHandler::onPrevControlCommand( - const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) + const autoware_control_msgs::msg::Control::ConstSharedPtr msg) { - auto control_command = new autoware_auto_control_msgs::msg::AckermannControlCommand(*msg); + auto control_command = new autoware_control_msgs::msg::Control(*msg); control_command->stamp = msg->stamp; - prev_control_command_ = - autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr(control_command); + prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); } void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) @@ -114,7 +111,7 @@ void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr } void EmergencyHandler::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; } @@ -131,9 +128,9 @@ void EmergencyHandler::onMrmEmergencyStopStatus( mrm_emergency_stop_status_ = msg; } -autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() +autoware_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() { - using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using autoware_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; // Check emergency @@ -155,7 +152,7 @@ autoware_auto_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHaz void EmergencyHandler::publishControlCommands() { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; // Create timestamp const auto stamp = this->now(); @@ -373,7 +370,7 @@ void EmergencyHandler::transitionTo(const int new_state) void EmergencyHandler::updateMrmState() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_vehicle_msgs::msg::ControlModeReport; + using autoware_vehicle_msgs::msg::ControlModeReport; // Check emergency const bool is_emergency = isEmergency(); @@ -416,7 +413,7 @@ void EmergencyHandler::updateMrmState() autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurrentMrmBehavior() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; // Get hazard level auto level = hazard_status_stamped_->status.level; diff --git a/system/hazard_status_converter/package.xml b/system/hazard_status_converter/package.xml index 35ff28998ee80..addf51edbb8e9 100644 --- a/system/hazard_status_converter/package.xml +++ b/system/hazard_status_converter/package.xml @@ -11,7 +11,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_system_msgs + autoware_system_msgs diagnostic_graph_utils diagnostic_msgs rclcpp diff --git a/system/hazard_status_converter/src/converter.cpp b/system/hazard_status_converter/src/converter.cpp index 9aa354c124bba..e8213b441bb33 100644 --- a/system/hazard_status_converter/src/converter.cpp +++ b/system/hazard_status_converter/src/converter.cpp @@ -61,7 +61,7 @@ void Converter::on_update(DiagGraph::ConstSharedPtr graph) { using DiagnosticStatus = diagnostic_msgs::msg::DiagnosticStatus; using DiagnosticLevel = DiagnosticStatus::_level_type; - using HazardStatus = autoware_auto_system_msgs::msg::HazardStatus; + using HazardStatus = autoware_system_msgs::msg::HazardStatus; using HazardLevel = HazardStatus::_level_type; const auto get_hazard_level = [](DiagnosticLevel unit_level, DiagnosticLevel root_level) { diff --git a/system/hazard_status_converter/src/converter.hpp b/system/hazard_status_converter/src/converter.hpp index ad14ddde2bf5c..442eedf588429 100644 --- a/system/hazard_status_converter/src/converter.hpp +++ b/system/hazard_status_converter/src/converter.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include @@ -31,7 +31,7 @@ class Converter : public rclcpp::Node explicit Converter(const rclcpp::NodeOptions & options); private: - using HazardStatusStamped = autoware_auto_system_msgs::msg::HazardStatusStamped; + using HazardStatusStamped = autoware_system_msgs::msg::HazardStatusStamped; using DiagGraph = diagnostic_graph_utils::DiagGraph; using DiagUnit = diagnostic_graph_utils::DiagUnit; void on_create(DiagGraph::ConstSharedPtr graph); diff --git a/system/mrm_emergency_stop_operator/README.md b/system/mrm_emergency_stop_operator/README.md index 0866202f4352a..203303c0ea748 100644 --- a/system/mrm_emergency_stop_operator/README.md +++ b/system/mrm_emergency_stop_operator/README.md @@ -10,18 +10,18 @@ MRM emergency stop operator is a node that generates emergency stop commands acc ### Input -| Name | Type | Description | -| ------------------------------------ | ---------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | -| `~/input/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | -| `~/input/control/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Control command output from the last node of the control component. Used for the initial value of the emergency stop command. | -| | | | +| Name | Type | Description | +| ------------------------------------ | ------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------- | +| `~/input/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | MRM execution order | +| `~/input/control/control_cmd` | `autoware_control_msgs::msg::Control` | Control command output from the last node of the control component. Used for the initial value of the emergency stop command. | +| | | | ### Output -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------------- | ---------------------- | -| `~/output/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | -| `~/output/mrm/emergency_stop/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Emergency stop command | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------- | ---------------------- | +| `~/output/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | MRM execution status | +| `~/output/mrm/emergency_stop/control_cmd` | `autoware_control_msgs::msg::Control` | Emergency stop command | ## Parameters diff --git a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp index 5b91f56973ae9..d7995a51ac8fb 100644 --- a/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp +++ b/system/mrm_emergency_stop_operator/include/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.hpp @@ -20,7 +20,7 @@ #include // Autoware -#include +#include #include #include @@ -30,7 +30,7 @@ #include namespace mrm_emergency_stop_operator { -using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_control_msgs::msg::Control; using tier4_system_msgs::msg::MrmBehaviorStatus; using tier4_system_msgs::srv::OperateMrm; @@ -55,9 +55,9 @@ class MrmEmergencyStopOperator : public rclcpp::Node const std::vector & parameters); // Subscriber - rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; - void onControlCommand(AckermannControlCommand::ConstSharedPtr msg); + void onControlCommand(Control::ConstSharedPtr msg); // Server rclcpp::Service::SharedPtr service_operation_; @@ -67,10 +67,10 @@ class MrmEmergencyStopOperator : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_status_; - rclcpp::Publisher::SharedPtr pub_control_cmd_; + rclcpp::Publisher::SharedPtr pub_control_cmd_; void publishStatus() const; - void publishControlCommand(const AckermannControlCommand & command) const; + void publishControlCommand(const Control & command) const; // Timer rclcpp::TimerBase::SharedPtr timer_; @@ -79,12 +79,11 @@ class MrmEmergencyStopOperator : public rclcpp::Node // States MrmBehaviorStatus status_; - AckermannControlCommand prev_control_cmd_; + Control prev_control_cmd_; bool is_prev_control_cmd_subscribed_; // Algorithm - AckermannControlCommand calcTargetAcceleration( - const AckermannControlCommand & prev_control_cmd) const; + Control calcTargetAcceleration(const Control & prev_control_cmd) const; }; } // namespace mrm_emergency_stop_operator diff --git a/system/mrm_emergency_stop_operator/package.xml b/system/mrm_emergency_stop_operator/package.xml index 6ca4e3a815f72..74ee8c7183741 100644 --- a/system/mrm_emergency_stop_operator/package.xml +++ b/system/mrm_emergency_stop_operator/package.xml @@ -12,7 +12,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs + autoware_control_msgs rclcpp rclcpp_components std_msgs diff --git a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp index 9ee9d7a685df3..589a2fd14d990 100644 --- a/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp +++ b/system/mrm_emergency_stop_operator/src/mrm_emergency_stop_operator/mrm_emergency_stop_operator_core.cpp @@ -28,7 +28,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n params_.target_jerk = declare_parameter("target_jerk", -1.5); // Subscriber - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control/control_cmd", 1, std::bind(&MrmEmergencyStopOperator::onControlCommand, this, std::placeholders::_1)); @@ -40,8 +40,7 @@ MrmEmergencyStopOperator::MrmEmergencyStopOperator(const rclcpp::NodeOptions & n // Publisher pub_status_ = create_publisher("~/output/mrm/emergency_stop/status", 1); - pub_control_cmd_ = - create_publisher("~/output/mrm/emergency_stop/control_cmd", 1); + pub_control_cmd_ = create_publisher("~/output/mrm/emergency_stop/control_cmd", 1); // Timer const auto update_period_ns = rclcpp::Rate(params_.update_rate).period(); @@ -70,7 +69,7 @@ rcl_interfaces::msg::SetParametersResult MrmEmergencyStopOperator::onParameter( return result; } -void MrmEmergencyStopOperator::onControlCommand(AckermannControlCommand::ConstSharedPtr msg) +void MrmEmergencyStopOperator::onControlCommand(Control::ConstSharedPtr msg) { if (status_.state != MrmBehaviorStatus::OPERATING) { prev_control_cmd_ = *msg; @@ -97,7 +96,7 @@ void MrmEmergencyStopOperator::publishStatus() const pub_status_->publish(status); } -void MrmEmergencyStopOperator::publishControlCommand(const AckermannControlCommand & command) const +void MrmEmergencyStopOperator::publishControlCommand(const Control & command) const { pub_control_cmd_->publish(command); } @@ -114,15 +113,14 @@ void MrmEmergencyStopOperator::onTimer() publishStatus(); } -AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( - const AckermannControlCommand & prev_control_cmd) const +Control MrmEmergencyStopOperator::calcTargetAcceleration(const Control & prev_control_cmd) const { - auto control_cmd = AckermannControlCommand(); + auto control_cmd = Control(); if (!is_prev_control_cmd_subscribed_) { control_cmd.stamp = this->now(); control_cmd.longitudinal.stamp = this->now(); - control_cmd.longitudinal.speed = 0.0; + control_cmd.longitudinal.velocity = 0.0; control_cmd.longitudinal.acceleration = static_cast(params_.target_acceleration); control_cmd.longitudinal.jerk = 0.0; control_cmd.lateral.stamp = this->now(); @@ -136,8 +134,8 @@ AckermannControlCommand MrmEmergencyStopOperator::calcTargetAcceleration( control_cmd.stamp = this->now(); control_cmd.longitudinal.stamp = this->now(); - control_cmd.longitudinal.speed = static_cast(std::max( - prev_control_cmd.longitudinal.speed + prev_control_cmd.longitudinal.acceleration * dt, 0.0)); + control_cmd.longitudinal.velocity = static_cast(std::max( + prev_control_cmd.longitudinal.velocity + prev_control_cmd.longitudinal.acceleration * dt, 0.0)); control_cmd.longitudinal.acceleration = static_cast(std::max( prev_control_cmd.longitudinal.acceleration + params_.target_jerk * dt, params_.target_acceleration)); diff --git a/system/mrm_handler/README.md b/system/mrm_handler/README.md index 4944d8a833643..8ccb95e6ca8d3 100644 --- a/system/mrm_handler/README.md +++ b/system/mrm_handler/README.md @@ -14,26 +14,26 @@ MRM Handler is a node to select a proper MRM from a system failure state contain ### Input -| Name | Type | Description | -| -------------------------------------- | ---------------------------------------------------- | --------------------------------------------------------------------------------------------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | -| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | -| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | -| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | -| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | -| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | -| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | +| Name | Type | Description | +| -------------------------------------- | --------------------------------------------------- | --------------------------------------------------------------------------------------------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not | +| `/system/operation_mode/availability` | `tier4_system_msgs::msg::OperationModeAvailability` | Used to select proper MRM from system available mrm behavior contained in operationModeAvailability | +| `/vehicle/status/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual | +| `/system/mrm/emergency_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM emergency stop operation is available | +| `/system/mrm/comfortable_stop/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM comfortable stop operation is available | +| `/system/mrm/pull_over_manager/status` | `tier4_system_msgs::msg::MrmBehaviorStatus` | Used to check if MRM pull over operation is available | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | Used to check whether the current operation mode is AUTO or STOP. | ### Output -| Name | Type | Description | -| --------------------------------------- | ------------------------------------------------------ | ----------------------------------------------------- | -| `/system/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | -| `/system/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | -| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | -| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | -| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | -| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over | +| Name | Type | Description | +| --------------------------------------- | ------------------------------------------------- | ----------------------------------------------------- | +| `/system/emergency/gear_cmd` | `autoware_vehicle_msgs::msg::GearCommand` | Required to execute proper MRM (send gear cmd) | +| `/system/emergency/hazard_lights_cmd` | `autoware_vehicle_msgs::msg::HazardLightsCommand` | Required to execute proper MRM (send turn signal cmd) | +| `/system/fail_safe/mrm_state` | `autoware_adapi_v1_msgs::msg::MrmState` | Inform MRM execution state and selected MRM behavior | +| `/system/mrm/emergency_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM emergency stop | +| `/system/mrm/comfortable_stop/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM comfortable stop | +| `/system/mrm/pull_over_manager/operate` | `tier4_system_msgs::srv::OperateMrm` | Execution order for MRM pull over | ## Parameters 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 c7aaca96aae49..8c58d961953ce 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -24,9 +24,9 @@ // Autoware #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -68,8 +68,7 @@ class MrmHandler : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; + rclcpp::Subscription::SharedPtr sub_control_mode_; rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; rclcpp::Subscription::SharedPtr @@ -82,7 +81,7 @@ class MrmHandler : public rclcpp::Node sub_operation_mode_state_; nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; 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_; @@ -90,7 +89,7 @@ class MrmHandler : public rclcpp::Node autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::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); @@ -105,9 +104,8 @@ class MrmHandler : public rclcpp::Node // rclcpp::Publisher::SharedPtr pub_shift_; // rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr - pub_hazard_cmd_; - rclcpp::Publisher::SharedPtr pub_gear_cmd_; + rclcpp::Publisher::SharedPtr pub_hazard_cmd_; + rclcpp::Publisher::SharedPtr pub_gear_cmd_; void publishHazardCmd(); void publishGearCmd(); diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index c15bc98a6c190..2db22cecaa82d 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -13,9 +13,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_system_msgs + autoware_vehicle_msgs nav_msgs rclcpp rclcpp_components 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 5e709946c2f46..afcc7ebd208ab 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -40,7 +40,7 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" sub_odom_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmHandler::onOdometry, this, _1)); // subscribe control mode - sub_control_mode_ = create_subscription( + sub_control_mode_ = create_subscription( "~/input/control_mode", rclcpp::QoS{1}, std::bind(&MrmHandler::onControlMode, this, _1)); sub_operation_mode_availability_ = create_subscription( @@ -60,10 +60,10 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" std::bind(&MrmHandler::onOperationModeState, this, _1)); // Publisher - pub_hazard_cmd_ = create_publisher( + pub_hazard_cmd_ = create_publisher( "~/output/hazard", rclcpp::QoS{1}); pub_gear_cmd_ = - create_publisher("~/output/gear", rclcpp::QoS{1}); + create_publisher("~/output/gear", rclcpp::QoS{1}); pub_mrm_state_ = create_publisher("~/output/mrm/state", rclcpp::QoS{1}); @@ -85,7 +85,7 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" // Initialize odom_ = std::make_shared(); - control_mode_ = std::make_shared(); + control_mode_ = std::make_shared(); mrm_pull_over_status_ = std::make_shared(); mrm_comfortable_stop_status_ = std::make_shared(); @@ -108,7 +108,7 @@ void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) } void MrmHandler::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; } @@ -167,7 +167,7 @@ void MrmHandler::onOperationModeState( void MrmHandler::publishHazardCmd() { - using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using autoware_vehicle_msgs::msg::HazardLightsCommand; HazardLightsCommand msg; msg.stamp = this->now(); @@ -187,7 +187,7 @@ void MrmHandler::publishHazardCmd() void MrmHandler::publishGearCmd() { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; GearCommand msg; msg.stamp = this->now(); @@ -444,7 +444,7 @@ void MrmHandler::transitionTo(const int new_state) void MrmHandler::updateMrmState() { using autoware_adapi_v1_msgs::msg::MrmState; - using autoware_auto_vehicle_msgs::msg::ControlModeReport; + using autoware_vehicle_msgs::msg::ControlModeReport; // Check emergency const bool is_emergency = isEmergency(); diff --git a/system/system_error_monitor/README.md b/system/system_error_monitor/README.md index a62d4b0f00576..e765239f30f4d 100644 --- a/system/system_error_monitor/README.md +++ b/system/system_error_monitor/README.md @@ -90,19 +90,19 @@ endif ### Input -| Name | Type | Description | -| ---------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | -| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](./config/diagnostic_aggregator) is used to | -| `/autoware/state` | `autoware_auto_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | -| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | -| `/vehicle/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | +| Name | Type | Description | +| ---------------------------- | ----------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| `/diagnostics_agg` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostic information aggregated based [diagnostic_aggregator setting](./config/diagnostic_aggregator) is used to | +| `/autoware/state` | `autoware_system_msgs::msg::AutowareState` | Required to ignore error during Route, Planning and Finalizing. | +| `/control/current_gate_mode` | `tier4_control_msgs::msg::GateMode` | Required to select the appropriate module from `autonomous_driving` or `external_control` | +| `/vehicle/control_mode` | `autoware_vehicle_msgs::msg::ControlModeReport` | Required to not hold emergency during manual driving | ### Output -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------------------------------ | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | HazardStatus contains system hazard level, emergency hold status and failure details | -| `/diagnostics_err` | `diagnostic_msgs::msg::DiagnosticArray` | This has the same contents as HazardStatus. This is used for visualization | +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ------------------------------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | HazardStatus contains system hazard level, emergency hold status and failure details | +| `/diagnostics_err` | `diagnostic_msgs::msg::DiagnosticArray` | This has the same contents as HazardStatus. This is used for visualization | ## Parameters diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index 8829bcc1edde2..22f1f4e9012c7 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -20,9 +20,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -86,7 +86,7 @@ class AutowareErrorMonitor : public rclcpp::Node rclcpp::Time emergency_state_switch_time_; rclcpp::Time initialized_time_; - autoware_auto_system_msgs::msg::HazardStatus hazard_status_{}; + autoware_system_msgs::msg::HazardStatus hazard_status_{}; std::unordered_map required_modules_map_; std::string current_mode_; @@ -101,28 +101,25 @@ class AutowareErrorMonitor : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_diag_array_; - rclcpp::Subscription::SharedPtr - sub_autoware_state_; + rclcpp::Subscription::SharedPtr sub_autoware_state_; rclcpp::Subscription::SharedPtr sub_current_gate_mode_; - rclcpp::Subscription::SharedPtr - sub_control_mode_; - void onAutowareState(const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg); + rclcpp::Subscription::SharedPtr sub_control_mode_; + void onAutowareState(const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg); void onCurrentGateMode(const tier4_control_msgs::msg::GateMode::ConstSharedPtr msg); - void onControlMode(const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); + void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onDiagArray(const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr msg); const size_t diag_buffer_size_ = 100; std::unordered_map diag_buffer_map_; diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr diag_array_; - autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; + autoware_system_msgs::msg::AutowareState::ConstSharedPtr autoware_state_; tier4_control_msgs::msg::GateMode::ConstSharedPtr current_gate_mode_; - autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; // Publisher - rclcpp::Publisher::SharedPtr - pub_hazard_status_; + rclcpp::Publisher::SharedPtr pub_hazard_status_; rclcpp::Publisher::SharedPtr pub_diagnostics_err_; - void publishHazardStatus(const autoware_auto_system_msgs::msg::HazardStatus & hazard_status); + void publishHazardStatus(const autoware_system_msgs::msg::HazardStatus & hazard_status); // Service rclcpp::Service::SharedPtr srv_clear_emergency_; @@ -141,14 +138,14 @@ class AutowareErrorMonitor : public rclcpp::Node uint8_t getHazardLevel(const DiagConfig & required_module, const int diag_level) const; void appendHazardDiag( const DiagConfig & required_module, const diagnostic_msgs::msg::DiagnosticStatus & diag, - autoware_auto_system_msgs::msg::HazardStatus * hazard_status) const; - autoware_auto_system_msgs::msg::HazardStatus judgeHazardStatus() const; + autoware_system_msgs::msg::HazardStatus * hazard_status) const; + autoware_system_msgs::msg::HazardStatus judgeHazardStatus() const; void updateHazardStatus(); void updateTimeoutHazardStatus(); bool canAutoRecovery() const; bool isEmergencyHoldingRequired() const; - void loggingErrors(const autoware_auto_system_msgs::msg::HazardStatus & diag_array); + void loggingErrors(const autoware_system_msgs::msg::HazardStatus & diag_array); std::unique_ptr logger_configure_; }; diff --git a/system/system_error_monitor/package.xml b/system/system_error_monitor/package.xml index ed50eda95611a..0c8e2fc0241aa 100644 --- a/system/system_error_monitor/package.xml +++ b/system/system_error_monitor/package.xml @@ -10,8 +10,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_system_msgs + autoware_vehicle_msgs diagnostic_msgs fmt rclcpp diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index e4ca33204f53e..ad67521298643 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -96,9 +96,9 @@ bool isOverLevel(const int & diag_level, const std::string & failure_level_str) } std::vector & getTargetDiagnosticsRef( - const int hazard_level, autoware_auto_system_msgs::msg::HazardStatus * hazard_status) + const int hazard_level, autoware_system_msgs::msg::HazardStatus * hazard_status) { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (hazard_level == HazardStatus::NO_FAULT) { return hazard_status->diag_no_fault; @@ -117,8 +117,7 @@ std::vector & getTargetDiagnosticsRef( } diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( - rclcpp::Clock::SharedPtr clock, - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + rclcpp::Clock::SharedPtr clock, const autoware_system_msgs::msg::HazardStatus & hazard_status) { using diagnostic_msgs::msg::DiagnosticStatus; @@ -150,11 +149,10 @@ diagnostic_msgs::msg::DiagnosticArray convertHazardStatusToDiagnosticArray( } std::set getErrorModules( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status, - const int emergency_hazard_level) + const autoware_system_msgs::msg::HazardStatus & hazard_status, const int emergency_hazard_level) { std::set error_modules; - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (emergency_hazard_level <= HazardStatus::SINGLE_POINT_FAULT) { for (const auto & diag_spf : hazard_status.diag_single_point_fault) { error_modules.insert(diag_spf.name); @@ -175,10 +173,10 @@ std::set getErrorModules( } int isInNoFaultCondition( - const autoware_auto_system_msgs::msg::AutowareState & autoware_state, + const autoware_system_msgs::msg::AutowareState & autoware_state, const tier4_control_msgs::msg::GateMode & current_gate_mode) { - using autoware_auto_system_msgs::msg::AutowareState; + using autoware_system_msgs::msg::AutowareState; using tier4_control_msgs::msg::GateMode; const auto is_in_autonomous_ignore_state = @@ -221,7 +219,7 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) get_parameter_or("hazard_recovery_timeout", params_.hazard_recovery_timeout, 5.0); get_parameter_or( "emergency_hazard_level", params_.emergency_hazard_level, - autoware_auto_system_msgs::msg::HazardStatus::LATENT_FAULT); + autoware_system_msgs::msg::HazardStatus::LATENT_FAULT); get_parameter_or("use_emergency_hold", params_.use_emergency_hold, false); get_parameter_or( "use_emergency_hold_in_manual_driving", params_.use_emergency_hold_in_manual_driving, false); @@ -237,15 +235,15 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) sub_current_gate_mode_ = create_subscription( "~/input/current_gate_mode", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onCurrentGateMode, this, _1)); - sub_autoware_state_ = create_subscription( + sub_autoware_state_ = create_subscription( "~/input/autoware_state", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onAutowareState, this, _1)); - sub_control_mode_ = create_subscription( + sub_control_mode_ = create_subscription( "~/input/control_mode", rclcpp::QoS{1}, std::bind(&AutowareErrorMonitor::onControlMode, this, _1)); // Publisher - pub_hazard_status_ = create_publisher( + pub_hazard_status_ = create_publisher( "~/output/hazard_status", rclcpp::QoS{1}); pub_diagnostics_err_ = create_publisher( "~/output/diagnostics_err", rclcpp::QoS{1}); @@ -256,10 +254,10 @@ AutowareErrorMonitor::AutowareErrorMonitor(const rclcpp::NodeOptions & options) std::bind(&AutowareErrorMonitor::onClearEmergencyService, this, _1, _2)); // Initialize - autoware_auto_vehicle_msgs::msg::ControlModeReport vehicle_state_report; - vehicle_state_report.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL; - control_mode_ = std::make_shared( - vehicle_state_report); + autoware_vehicle_msgs::msg::ControlModeReport vehicle_state_report; + vehicle_state_report.mode = autoware_vehicle_msgs::msg::ControlModeReport::MANUAL; + control_mode_ = + std::make_shared(vehicle_state_report); // Timer initialized_time_ = this->now(); @@ -363,7 +361,7 @@ void AutowareErrorMonitor::onCurrentGateMode( } void AutowareErrorMonitor::onAutowareState( - const autoware_auto_system_msgs::msg::AutowareState::ConstSharedPtr msg) + const autoware_system_msgs::msg::AutowareState::ConstSharedPtr msg) { autoware_state_ = msg; @@ -372,7 +370,7 @@ void AutowareErrorMonitor::onAutowareState( } void AutowareErrorMonitor::onControlMode( - const autoware_auto_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) + const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) { control_mode_ = msg; @@ -486,7 +484,7 @@ boost::optional AutowareErrorMonitor::getLatestDiag( uint8_t AutowareErrorMonitor::getHazardLevel( const DiagConfig & required_module, const int diag_level) const { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; if (isOverLevel(diag_level, required_module.spf_at)) { return HazardStatus::SINGLE_POINT_FAULT; @@ -503,7 +501,7 @@ uint8_t AutowareErrorMonitor::getHazardLevel( void AutowareErrorMonitor::appendHazardDiag( const DiagConfig & required_module, const diagnostic_msgs::msg::DiagnosticStatus & hazard_diag, - autoware_auto_system_msgs::msg::HazardStatus * hazard_status) const + autoware_system_msgs::msg::HazardStatus * hazard_status) const { const auto hazard_level = getHazardLevel(required_module, hazard_diag.level); @@ -520,12 +518,12 @@ void AutowareErrorMonitor::appendHazardDiag( hazard_status->level = std::max(hazard_status->level, hazard_level); } -autoware_auto_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardStatus() const +autoware_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardStatus() const { - using autoware_auto_system_msgs::msg::HazardStatus; + using autoware_system_msgs::msg::HazardStatus; using diagnostic_msgs::msg::DiagnosticStatus; - autoware_auto_system_msgs::msg::HazardStatus hazard_status; + autoware_system_msgs::msg::HazardStatus hazard_status; for (const auto & required_module : required_modules_map_.at(current_mode_)) { const auto & diag_name = required_module.name; const auto latest_diag = getLatestDiag(diag_name); @@ -566,7 +564,7 @@ autoware_auto_system_msgs::msg::HazardStatus AutowareErrorMonitor::judgeHazardSt // Ignore error when vehicle is not ready to start if (isInNoFaultCondition(*autoware_state_, *current_gate_mode_)) { - hazard_status.level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; + hazard_status.level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; } return hazard_status; @@ -604,7 +602,7 @@ void AutowareErrorMonitor::updateTimeoutHazardStatus() { const bool prev_emergency_status = hazard_status_.emergency; - hazard_status_.level = autoware_auto_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; + hazard_status_.level = autoware_system_msgs::msg::HazardStatus::SINGLE_POINT_FAULT; hazard_status_.emergency = true; if (hazard_status_.emergency != prev_emergency_status) { @@ -663,7 +661,7 @@ bool AutowareErrorMonitor::isEmergencyHoldingRequired() const // Don't hold status during manual driving const bool is_manual_driving = - (control_mode_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL); + (control_mode_->mode == autoware_vehicle_msgs::msg::ControlModeReport::MANUAL); const auto no_hold_condition = (!params_.use_emergency_hold_in_manual_driving && is_manual_driving); if (no_hold_condition) { @@ -674,9 +672,9 @@ bool AutowareErrorMonitor::isEmergencyHoldingRequired() const } void AutowareErrorMonitor::publishHazardStatus( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + const autoware_system_msgs::msg::HazardStatus & hazard_status) { - autoware_auto_system_msgs::msg::HazardStatusStamped hazard_status_stamped; + autoware_system_msgs::msg::HazardStatusStamped hazard_status_stamped; hazard_status_stamped.stamp = this->now(); hazard_status_stamped.status = hazard_status; pub_hazard_status_->publish(hazard_status_stamped); @@ -700,7 +698,7 @@ bool AutowareErrorMonitor::onClearEmergencyService( } void AutowareErrorMonitor::loggingErrors( - const autoware_auto_system_msgs::msg::HazardStatus & hazard_status) + const autoware_system_msgs::msg::HazardStatus & hazard_status) { if ( autoware_state_ && current_gate_mode_ && diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md index f5a22aaf20176..ba77cc4c8f5bb 100644 --- a/tools/reaction_analyzer/README.md +++ b/tools/reaction_analyzer/README.md @@ -201,21 +201,21 @@ for each of the nodes. - `sensor_msgs/msg/Image` - `geometry_msgs/msg/PoseWithCovarianceStamped` - `sensor_msgs/msg/Imu` - - `autoware_auto_vehicle_msgs/msg/ControlModeReport` - - `autoware_auto_vehicle_msgs/msg/GearReport` - - `autoware_auto_vehicle_msgs/msg/HazardLightsReport` - - `autoware_auto_vehicle_msgs/msg/SteeringReport` - - `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport` - - `autoware_auto_vehicle_msgs/msg/VelocityReport` + - `autoware_vehicle_msgs/msg/ControlModeReport` + - `autoware_vehicle_msgs/msg/GearReport` + - `autoware_vehicle_msgs/msg/HazardLightsReport` + - `autoware_vehicle_msgs/msg/SteeringReport` + - `autoware_vehicle_msgs/msg/TurnIndicatorsReport` + - `autoware_vehicle_msgs/msg/VelocityReport` - **Subscriber Message Types:** - `sensor_msgs/msg/PointCloud2` - - `autoware_auto_perception_msgs/msg/DetectedObjects` - - `autoware_auto_perception_msgs/msg/TrackedObjects` - - `autoware_auto_msgs/msg/PredictedObject` - - `autoware_auto_planning_msgs/msg/Trajectory` - - `autoware_auto_control_msgs/msg/AckermannControlCommand` + - `autoware_perception_msgs/msg/DetectedObjects` + - `autoware_perception_msgs/msg/TrackedObjects` + - `autoware_perception_msgs/msg/PredictedObject` + - `autoware_planning_msgs/msg/Trajectory` + - `autoware_control_msgs/msg/Control` - **Reaction Types:** - `FIRST_BRAKE` diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp index 6dc3dd896dc92..c5a8f6a2e663a 100644 --- a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -42,9 +42,9 @@ using autoware_adapi_v1_msgs::msg::LocalizationInitializationState; using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_adapi_v1_msgs::msg::RouteState; using autoware_adapi_v1_msgs::srv::ChangeOperationMode; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_internal_msgs::msg::PublishedTime; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using nav_msgs::msg::Odometry; diff --git a/tools/reaction_analyzer/include/subscriber.hpp b/tools/reaction_analyzer/include/subscriber.hpp index d14d41da545f7..21d370a68b556 100644 --- a/tools/reaction_analyzer/include/subscriber.hpp +++ b/tools/reaction_analyzer/include/subscriber.hpp @@ -39,23 +39,23 @@ namespace reaction_analyzer::subscriber { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_control_msgs::msg::Control; using autoware_internal_msgs::msg::PublishedTime; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; // Buffers to be used to store subscribed messages' data, pipeline Header, and PublishedTime using MessageBuffer = std::optional; -// We need to store the past AckermannControlCommands to analyze the first brake -using ControlCommandBuffer = std::pair, MessageBuffer>; +// We need to store the past Control to analyze the first brake +using ControlCommandBuffer = std::pair, MessageBuffer>; // Variant to store different types of buffers using MessageBufferVariant = std::variant; @@ -67,7 +67,7 @@ struct SubscriberVariables std::unique_ptr> sub1_; std::unique_ptr> sub2_; std::unique_ptr> synchronizer_; - // tmp: only for the messages who don't have header e.g. AckermannControlCommand + // tmp: only for the messages who don't have header e.g. Control std::unique_ptr> cache_; }; @@ -75,7 +75,7 @@ struct SubscriberVariables using SubscriberVariablesVariant = std::variant< SubscriberVariables, SubscriberVariables, SubscriberVariables, SubscriberVariables, - SubscriberVariables, SubscriberVariables>; + SubscriberVariables, SubscriberVariables>; // The configuration of the topic to be subscribed which are defined in reaction_chain struct TopicConfig @@ -153,14 +153,11 @@ class SubscriberBase bool init_subscribers(); std::optional get_subscriber_variable( const TopicConfig & topic_config); - std::optional find_first_brake_idx( - const std::vector & cmd_array); - void set_control_command_to_buffer( - std::vector & buffer, const AckermannControlCommand & cmd) const; + std::optional find_first_brake_idx(const std::vector & cmd_array); + void set_control_command_to_buffer(std::vector & buffer, const Control & cmd) const; // Callbacks for modules are subscribed - void on_control_command( - const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr); + void on_control_command(const std::string & node_name, const Control::ConstSharedPtr & msg_ptr); void on_trajectory(const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr); void on_trajectory( const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr, diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp index c6d3a90650884..0c01561fec508 100644 --- a/tools/reaction_analyzer/include/topic_publisher.hpp +++ b/tools/reaction_analyzer/include/topic_publisher.hpp @@ -20,12 +20,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include @@ -45,12 +45,12 @@ namespace reaction_analyzer::topic_publisher { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_control_msgs::msg::Control; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; @@ -164,12 +164,12 @@ using PublisherVariablesVariant = std::variant< PublisherVariables, PublisherVariables, PublisherVariables, PublisherVariables, - PublisherVariables, - PublisherVariables, - PublisherVariables, - PublisherVariables, - PublisherVariables, - PublisherVariables>; + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables>; using LidarOutputPair = std::pair< std::shared_ptr>, diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp index da1defc03f34c..4a31da363deff 100644 --- a/tools/reaction_analyzer/include/utils.hpp +++ b/tools/reaction_analyzer/include/utils.hpp @@ -22,12 +22,12 @@ #include #include -#include -#include -#include -#include -#include +#include #include +#include +#include +#include +#include #include #include #include @@ -52,16 +52,16 @@ // The reaction_analyzer namespace contains utility functions for the Reaction Analyzer tool. namespace reaction_analyzer { -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_control_msgs::msg::Control; using autoware_internal_msgs::msg::PublishedTime; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; using sensor_msgs::msg::PointCloud2; /** @@ -111,7 +111,7 @@ enum class PublisherMessageType { */ enum class SubscriberMessageType { UNKNOWN = 0, - ACKERMANN_CONTROL_COMMAND = 1, + CONTROL = 1, TRAJECTORY = 2, POINTCLOUD2 = 3, DETECTED_OBJECTS = 4, diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml index 05081eac01751..a5199aa78c9f8 100644 --- a/tools/reaction_analyzer/package.xml +++ b/tools/reaction_analyzer/package.xml @@ -15,12 +15,12 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_control_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs autoware_internal_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_system_msgs + autoware_vehicle_msgs eigen libpcl-all-dev message_filters diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml index 62c1191bd345a..b40a67ecc333c 100644 --- a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -54,27 +54,27 @@ obstacle_cruise_planner: topic_name: /planning/scenario_planning/lane_driving/trajectory time_debug_topic_name: /planning/scenario_planning/lane_driving/trajectory/debug/published_time - message_type: autoware_auto_planning_msgs/msg/Trajectory + message_type: autoware_planning_msgs/msg/Trajectory scenario_selector: topic_name: /planning/scenario_planning/scenario_selector/trajectory time_debug_topic_name: /planning/scenario_planning/scenario_selector/trajectory/debug/published_time - message_type: autoware_auto_planning_msgs/msg/Trajectory + message_type: autoware_planning_msgs/msg/Trajectory motion_velocity_smoother: topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory time_debug_topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory/debug/published_time - message_type: autoware_auto_planning_msgs/msg/Trajectory + message_type: autoware_planning_msgs/msg/Trajectory planning_validator: topic_name: /planning/scenario_planning/trajectory time_debug_topic_name: /planning/scenario_planning/trajectory/debug/published_time - message_type: autoware_auto_planning_msgs/msg/Trajectory + message_type: autoware_planning_msgs/msg/Trajectory trajectory_follower: topic_name: /control/trajectory_follower/control_cmd time_debug_topic_name: /control/trajectory_follower/control_cmd/debug/published_time - message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + message_type: autoware_control_msgs/msg/Control vehicle_cmd_gate: topic_name: /control/command/control_cmd time_debug_topic_name: /control/command/control_cmd/debug/published_time - message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + message_type: autoware_control_msgs/msg/Control common_ground_filter: topic_name: /perception/obstacle_segmentation/single_frame/pointcloud time_debug_topic_name: /perception/obstacle_segmentation/single_frame/pointcloud/debug/published_time @@ -86,32 +86,32 @@ multi_object_tracker: topic_name: /perception/object_recognition/tracking/near_objects time_debug_topic_name: /perception/object_recognition/tracking/near_objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/TrackedObjects + message_type: autoware_perception_msgs/msg/TrackedObjects lidar_centerpoint: topic_name: /perception/object_recognition/detection/centerpoint/objects time_debug_topic_name: /perception/object_recognition/detection/centerpoint/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects obstacle_pointcloud_based_validator: topic_name: /perception/object_recognition/detection/centerpoint/validation/objects time_debug_topic_name: /perception/object_recognition/detection/centerpoint/validation/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects decorative_tracker_merger: topic_name: /perception/object_recognition/tracking/objects time_debug_topic_name: /perception/object_recognition/tracking/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/TrackedObjects + message_type: autoware_perception_msgs/msg/TrackedObjects detected_object_feature_remover: topic_name: /perception/object_recognition/detection/clustering/objects time_debug_topic_name: /perception/object_recognition/detection/clustering/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects detection_by_tracker: topic_name: /perception/object_recognition/detection/detection_by_tracker/objects time_debug_topic_name: /perception/object_recognition/detection/detection_by_tracker/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects object_lanelet_filter: topic_name: /perception/object_recognition/detection/objects time_debug_topic_name: /perception/object_recognition/detection/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/DetectedObjects + message_type: autoware_perception_msgs/msg/DetectedObjects map_based_prediction: topic_name: /perception/object_recognition/objects time_debug_topic_name: /perception/object_recognition/objects/debug/published_time - message_type: autoware_auto_perception_msgs/msg/PredictedObjects + message_type: autoware_perception_msgs/msg/PredictedObjects diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp index 8c735c42b9cd5..9064f42377d10 100644 --- a/tools/reaction_analyzer/src/subscriber.cpp +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -189,12 +189,12 @@ void SubscriberBase::reset() // Callbacks void SubscriberBase::on_control_command( - const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr) + const std::string & node_name, const Control::ConstSharedPtr & msg_ptr) { std::lock_guard lock(mutex_); auto & variant = message_buffers_[node_name]; if (!std::holds_alternative(variant)) { - ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); + ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); variant = buffer; } auto & cmd_buffer = std::get(variant); @@ -212,7 +212,7 @@ void SubscriberBase::on_control_command( // TODO(brkay54): update here if message_filters package add support for the messages which // does not have header const auto & subscriber_variant = - std::get>(subscriber_variables_map_[node_name]); + std::get>(subscriber_variables_map_[node_name]); // check if the cache was initialized or not, if there is, use it to set the published time if (subscriber_variant.cache_) { @@ -636,24 +636,23 @@ std::optional SubscriberBase::get_subscriber_variabl const TopicConfig & topic_config) { switch (topic_config.message_type) { - case SubscriberMessageType::ACKERMANN_CONTROL_COMMAND: { - SubscriberVariables subscriber_variable; + case SubscriberMessageType::CONTROL: { + SubscriberVariables subscriber_variable; if (!topic_config.time_debug_topic_address.empty()) { // If not empty, user should define a time debug topic // NOTE: Because message_filters package does not support the messages without headers, we // can not use the synchronizer. After we reacted, we are going to use the cache - // of the both PublishedTime and AckermannControlCommand subscribers to find the messages + // of the both PublishedTime and Control subscribers to find the messages // which have same header time. - std::function callback = - [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + std::function callback = + [this, topic_config](const Control::ConstSharedPtr & ptr) { this->on_control_command(topic_config.node_name, ptr); }; - subscriber_variable.sub1_ = - std::make_unique>( - node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), - create_subscription_options(node_)); + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); @@ -665,15 +664,14 @@ std::optional SubscriberBase::get_subscriber_variabl *subscriber_variable.sub2_, cache_size); } else { - std::function callback = - [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + std::function callback = + [this, topic_config](const Control::ConstSharedPtr & ptr) { this->on_control_command(topic_config.node_name, ptr); }; - subscriber_variable.sub1_ = - std::make_unique>( - node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), - create_subscription_options(node_)); + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); RCLCPP_WARN( @@ -915,8 +913,7 @@ std::optional SubscriberBase::get_subscriber_variabl } } -std::optional SubscriberBase::find_first_brake_idx( - const std::vector & cmd_array) +std::optional SubscriberBase::find_first_brake_idx(const std::vector & cmd_array) { if ( cmd_array.size() < @@ -975,7 +972,7 @@ std::optional SubscriberBase::find_first_brake_idx( } void SubscriberBase::set_control_command_to_buffer( - std::vector & buffer, const AckermannControlCommand & cmd) const + std::vector & buffer, const Control & cmd) const { const auto last_cmd_time = cmd.stamp; if (!buffer.empty()) { diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp index 8a66bf9e33911..f720786d422bc 100644 --- a/tools/reaction_analyzer/src/topic_publisher.cpp +++ b/tools/reaction_analyzer/src/topic_publisher.cpp @@ -512,22 +512,22 @@ void TopicPublisher::init_rosbag_publisher_buffer( } else if (message_type == PublisherMessageType::IMU) { set_message(current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::CONTROL_MODE_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::GEAR_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::HAZARD_LIGHTS_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::STEERING_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::TURN_INDICATORS_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } else if (message_type == PublisherMessageType::VELOCITY_REPORT) { - set_message( + set_message( current_topic, *bag_message, is_empty_area_message); } } diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp index 4a6110322440e..9f9198af7dc48 100644 --- a/tools/reaction_analyzer/src/utils.cpp +++ b/tools/reaction_analyzer/src/utils.cpp @@ -18,22 +18,22 @@ namespace reaction_analyzer { SubscriberMessageType get_subscriber_message_type(const std::string & message_type) { - if (message_type == "autoware_auto_control_msgs/msg/AckermannControlCommand") { - return SubscriberMessageType::ACKERMANN_CONTROL_COMMAND; + if (message_type == "autoware_control_msgs/msg/Control") { + return SubscriberMessageType::CONTROL; } - if (message_type == "autoware_auto_planning_msgs/msg/Trajectory") { + if (message_type == "autoware_planning_msgs/msg/Trajectory") { return SubscriberMessageType::TRAJECTORY; } if (message_type == "sensor_msgs/msg/PointCloud2") { return SubscriberMessageType::POINTCLOUD2; } - if (message_type == "autoware_auto_perception_msgs/msg/PredictedObjects") { + if (message_type == "autoware_perception_msgs/msg/PredictedObjects") { return SubscriberMessageType::PREDICTED_OBJECTS; } - if (message_type == "autoware_auto_perception_msgs/msg/DetectedObjects") { + if (message_type == "autoware_perception_msgs/msg/DetectedObjects") { return SubscriberMessageType::DETECTED_OBJECTS; } - if (message_type == "autoware_auto_perception_msgs/msg/TrackedObjects") { + if (message_type == "autoware_perception_msgs/msg/TrackedObjects") { return SubscriberMessageType::TRACKED_OBJECTS; } return SubscriberMessageType::UNKNOWN; @@ -62,22 +62,22 @@ PublisherMessageType get_publisher_message_type(const std::string & message_type if (message_type == "sensor_msgs/msg/Imu") { return PublisherMessageType::IMU; } - if (message_type == "autoware_auto_vehicle_msgs/msg/ControlModeReport") { + if (message_type == "autoware_vehicle_msgs/msg/ControlModeReport") { return PublisherMessageType::CONTROL_MODE_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/GearReport") { + if (message_type == "autoware_vehicle_msgs/msg/GearReport") { return PublisherMessageType::GEAR_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/HazardLightsReport") { + if (message_type == "autoware_vehicle_msgs/msg/HazardLightsReport") { return PublisherMessageType::HAZARD_LIGHTS_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/SteeringReport") { + if (message_type == "autoware_vehicle_msgs/msg/SteeringReport") { return PublisherMessageType::STEERING_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport") { + if (message_type == "autoware_vehicle_msgs/msg/TurnIndicatorsReport") { return PublisherMessageType::TURN_INDICATORS_REPORT; } - if (message_type == "autoware_auto_vehicle_msgs/msg/VelocityReport") { + if (message_type == "autoware_vehicle_msgs/msg/VelocityReport") { return PublisherMessageType::VELOCITY_REPORT; } return PublisherMessageType::UNKNOWN; @@ -360,17 +360,17 @@ PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityPara dimension.set__z(entity_params.z_l); obj.shape.set__dimensions(dimension); - obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; obj.existence_probability = 1.0; obj.kinematics.initial_pose_with_covariance.pose = entity_pose; - autoware_auto_perception_msgs::msg::PredictedPath path; + autoware_perception_msgs::msg::PredictedPath path; path.confidence = 1.0; path.path.emplace_back(entity_pose); obj.kinematics.predicted_paths.emplace_back(path); - autoware_auto_perception_msgs::msg::ObjectClassification classification; - classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + autoware_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_perception_msgs::msg::ObjectClassification::CAR; classification.probability = 1.0; obj.classification.emplace_back(classification); obj.set__object_id(generate_uuid_msg("test_obstacle")); diff --git a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index 3580f0546bbfb..6b94b7301540c 100644 --- a/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -27,8 +27,8 @@ #include -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" #include "std_msgs/msg/bool.hpp" @@ -56,8 +56,8 @@ namespace accel_brake_map_calibrator { -using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_vehicle_msgs::msg::SteeringReport; +using autoware_vehicle_msgs::msg::VelocityReport; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::OccupancyGrid; using raw_vehicle_cmd_converter::AccelMap; diff --git a/vehicle/accel_brake_map_calibrator/package.xml b/vehicle/accel_brake_map_calibrator/package.xml index 64e7d1fb105cf..4c4eadea5e1ce 100644 --- a/vehicle/accel_brake_map_calibrator/package.xml +++ b/vehicle/accel_brake_map_calibrator/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs motion_utils diff --git a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py b/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py index 749c9e429bf06..3a029e193f41f 100755 --- a/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py +++ b/vehicle/accel_brake_map_calibrator/scripts/actuation_cmd_publisher.py @@ -15,7 +15,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -from autoware_auto_vehicle_msgs.msg import GearCommand +from autoware_vehicle_msgs.msg import GearCommand import rclpy from rclpy.node import Node from tier4_debug_msgs.msg import Float32Stamped diff --git a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py b/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py index 9e57813bd4010..cd2205f8b269f 100755 --- a/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py +++ b/vehicle/accel_brake_map_calibrator/test/sim_actuation_status_publisher.py @@ -15,7 +15,7 @@ # limitations under the License. -from autoware_auto_vehicle_msgs.msg import VelocityReport +from autoware_vehicle_msgs.msg import VelocityReport import rclpy from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy diff --git a/vehicle/external_cmd_converter/README.md b/vehicle/external_cmd_converter/README.md index 3c4181a6f093b..ef5865debd4d4 100644 --- a/vehicle/external_cmd_converter/README.md +++ b/vehicle/external_cmd_converter/README.md @@ -7,16 +7,16 @@ | Name | Type | Description | | --------------------------- | -------------------------------------------- | ----------------------------------------------------------------------------------------------------------------- | | `~/in/external_control_cmd` | tier4_external_api_msgs::msg::ControlCommand | target `throttle/brake/steering_angle/steering_angle_velocity` is necessary to calculate desired control command. | -| `~/input/shift_cmd"` | autoware_auto_vehicle_msgs::GearCommand | current command of gear. | +| `~/input/shift_cmd"` | autoware_vehicle_msgs::GearCommand | current command of gear. | | `~/input/emergency_stop` | tier4_external_api_msgs::msg::Heartbeat | emergency heart beat for external command. | | `~/input/current_gate_mode` | tier4_control_msgs::msg::GateMode | topic for gate mode. | | `~/input/odometry` | navigation_msgs::Odometry | twist topic in odometry is used. | ## Output topics -| Name | Type | Description | -| ------------------- | -------------------------------------------------------- | ------------------------------------------------------------------ | -| `~/out/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | ackermann control command converted from selected external command | +| Name | Type | Description | +| ------------------- | ----------------------------------- | ------------------------------------------------------------------ | +| `~/out/control_cmd` | autoware_control_msgs::msg::Control | ackermann control command converted from selected external command | ## Parameters diff --git a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp index f5a52ad4a6032..2488c8ea7f300 100644 --- a/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp +++ b/vehicle/external_cmd_converter/include/external_cmd_converter/node.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -32,13 +32,12 @@ namespace external_cmd_converter { -using GearCommand = autoware_auto_vehicle_msgs::msg::GearCommand; -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using GearCommand = autoware_vehicle_msgs::msg::GearCommand; +using Control = autoware_control_msgs::msg::Control; using ExternalControlCommand = tier4_external_api_msgs::msg::ControlCommandStamped; using Odometry = nav_msgs::msg::Odometry; using raw_vehicle_cmd_converter::AccelMap; using raw_vehicle_cmd_converter::BrakeMap; -using ControlCommandStamped = autoware_auto_control_msgs::msg::AckermannControlCommand; using Odometry = nav_msgs::msg::Odometry; class ExternalCmdConverterNode : public rclcpp::Node @@ -48,7 +47,7 @@ class ExternalCmdConverterNode : public rclcpp::Node private: // Publisher - rclcpp::Publisher::SharedPtr pub_cmd_; + rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::Publisher::SharedPtr pub_current_cmd_; diff --git a/vehicle/external_cmd_converter/package.xml b/vehicle/external_cmd_converter/package.xml index d9301e23e05fe..513d73d2d39ff 100644 --- a/vehicle/external_cmd_converter/package.xml +++ b/vehicle/external_cmd_converter/package.xml @@ -12,8 +12,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs nav_msgs diff --git a/vehicle/external_cmd_converter/src/node.cpp b/vehicle/external_cmd_converter/src/node.cpp index 32592c02c3347..21945bb9078b8 100644 --- a/vehicle/external_cmd_converter/src/node.cpp +++ b/vehicle/external_cmd_converter/src/node.cpp @@ -26,7 +26,7 @@ ExternalCmdConverterNode::ExternalCmdConverterNode(const rclcpp::NodeOptions & n { using std::placeholders::_1; - pub_cmd_ = create_publisher("out/control_cmd", rclcpp::QoS{1}); + pub_cmd_ = create_publisher("out/control_cmd", rclcpp::QoS{1}); pub_current_cmd_ = create_publisher("out/latest_external_control_cmd", rclcpp::QoS{1}); sub_velocity_ = create_subscription( @@ -142,11 +142,11 @@ void ExternalCmdConverterNode::onExternalCmd(const ExternalControlCommand::Const } // Publish ControlCommand - autoware_auto_control_msgs::msg::AckermannControlCommand output; + autoware_control_msgs::msg::Control output; output.stamp = cmd_ptr->stamp; output.lateral.steering_tire_angle = cmd_ptr->control.steering_angle; output.lateral.steering_tire_rotation_rate = cmd_ptr->control.steering_angle_velocity; - output.longitudinal.speed = ref_velocity; + output.longitudinal.velocity = ref_velocity; output.longitudinal.acceleration = ref_acceleration; pub_cmd_->publish(output); diff --git a/vehicle/raw_vehicle_cmd_converter/README.md b/vehicle/raw_vehicle_cmd_converter/README.md index 5767c4fbf672b..6ac1ee4666cae 100644 --- a/vehicle/raw_vehicle_cmd_converter/README.md +++ b/vehicle/raw_vehicle_cmd_converter/README.md @@ -26,11 +26,11 @@ For ease of calibration and adjustments to the lookup table, an auto-calibration ## Input topics -| Name | Type | Description | -| --------------------- | -------------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | -| `~/input/control_cmd` | autoware_auto_control_msgs::msg::AckermannControlCommand | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | -| `~/input/steering"` | autoware_auto_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control | -| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. | +| Name | Type | Description | +| --------------------- | ------------------------------------- | ------------------------------------------------------------------------------------------------------------------ | +| `~/input/control_cmd` | autoware_control_msgs::msg::Control | target `velocity/acceleration/steering_angle/steering_angle_velocity` is necessary to calculate actuation command. | +| `~/input/steering"` | autoware_vehicle_msgs::SteeringReport | current status of steering used for steering feed back control | +| `~/input/twist` | navigation_msgs::Odometry | twist topic in odometry is used. | ## Output topics diff --git a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp index 98693337b5c28..d71ff96abade0 100644 --- a/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp +++ b/vehicle/raw_vehicle_cmd_converter/include/raw_vehicle_cmd_converter/node.hpp @@ -23,8 +23,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -36,12 +36,12 @@ namespace raw_vehicle_cmd_converter { -using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; +using Control = autoware_control_msgs::msg::Control; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_vehicle_msgs::msg::ActuationCommandStamped; using TwistStamped = geometry_msgs::msg::TwistStamped; using Odometry = nav_msgs::msg::Odometry; -using Steering = autoware_auto_vehicle_msgs::msg::SteeringReport; +using Steering = autoware_vehicle_msgs::msg::SteeringReport; class DebugValues { @@ -77,7 +77,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node //!< @brief subscriber for current velocity rclcpp::Subscription::SharedPtr sub_velocity_; //!< @brief subscriber for vehicle command - rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_control_cmd_; //!< @brief subscriber for steering rclcpp::Subscription::SharedPtr sub_steering_; @@ -85,7 +85,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node std::unique_ptr current_twist_ptr_; // [m/s] std::unique_ptr current_steer_ptr_; - AckermannControlCommand::ConstSharedPtr control_cmd_ptr_; + Control::ConstSharedPtr control_cmd_ptr_; AccelMap accel_map_; BrakeMap brake_map_; SteerMap steer_map_; @@ -110,7 +110,7 @@ class RawVehicleCommandConverterNode : public rclcpp::Node double calculateBrakeMap(const double current_velocity, const double desired_acc); double calculateSteer(const double vel, const double steering, const double steer_rate); void onSteering(const Steering::ConstSharedPtr msg); - void onControlCmd(const AckermannControlCommand::ConstSharedPtr msg); + void onControlCmd(const Control::ConstSharedPtr msg); void onVelocity(const Odometry::ConstSharedPtr msg); void publishActuationCmd(); // for debugging diff --git a/vehicle/raw_vehicle_cmd_converter/package.xml b/vehicle/raw_vehicle_cmd_converter/package.xml index d68f4601aab67..376a5c74f1cb6 100644 --- a/vehicle/raw_vehicle_cmd_converter/package.xml +++ b/vehicle/raw_vehicle_cmd_converter/package.xml @@ -17,8 +17,8 @@ ament_cmake_auto autoware_cmake - autoware_auto_control_msgs - autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_vehicle_msgs geometry_msgs interpolation nav_msgs diff --git a/vehicle/raw_vehicle_cmd_converter/src/node.cpp b/vehicle/raw_vehicle_cmd_converter/src/node.cpp index 2506fb1f70b76..91c668f63dfbd 100644 --- a/vehicle/raw_vehicle_cmd_converter/src/node.cpp +++ b/vehicle/raw_vehicle_cmd_converter/src/node.cpp @@ -76,7 +76,7 @@ RawVehicleCommandConverterNode::RawVehicleCommandConverterNode( steer_pid_.setInitialized(); } pub_actuation_cmd_ = create_publisher("~/output/actuation_cmd", 1); - sub_control_cmd_ = create_subscription( + sub_control_cmd_ = create_subscription( "~/input/control_cmd", 1, std::bind(&RawVehicleCommandConverterNode::onControlCmd, this, _1)); sub_velocity_ = create_subscription( "~/input/odometry", 1, std::bind(&RawVehicleCommandConverterNode::onVelocity, this, _1)); @@ -216,7 +216,7 @@ void RawVehicleCommandConverterNode::onVelocity(const Odometry::ConstSharedPtr m current_twist_ptr_->twist = msg->twist.twist; } -void RawVehicleCommandConverterNode::onControlCmd(const AckermannControlCommand::ConstSharedPtr msg) +void RawVehicleCommandConverterNode::onControlCmd(const Control::ConstSharedPtr msg) { control_cmd_ptr_ = msg; publishActuationCmd(); diff --git a/vehicle/steer_offset_estimator/Readme.md b/vehicle/steer_offset_estimator/Readme.md index 3b455a97e4925..164d411e31050 100644 --- a/vehicle/steer_offset_estimator/Readme.md +++ b/vehicle/steer_offset_estimator/Readme.md @@ -17,10 +17,10 @@ Calculate yaw rate error and then calculate steering error recursively by least ### Input -| Name | Type | Description | -| --------------- | ------------------------------------------------- | ------------- | -| `~/input/twist` | `geometry_msgs::msg::TwistStamped` | vehicle twist | -| `~/input/steer` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | steering | +| Name | Type | Description | +| --------------- | -------------------------------------------- | ------------- | +| `~/input/twist` | `geometry_msgs::msg::TwistStamped` | vehicle twist | +| `~/input/steer` | `autoware_vehicle_msgs::msg::SteeringReport` | steering | ### Output diff --git a/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp b/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp index 64d6694d7ce3e..afd1d0520e951 100644 --- a/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp +++ b/vehicle/steer_offset_estimator/include/steer_offset_estimator/steer_offset_estimator_node.hpp @@ -18,7 +18,7 @@ #include "diagnostic_updater/diagnostic_updater.hpp" #include "rclcpp/rclcpp.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "tier4_debug_msgs/msg/float32_stamped.hpp" @@ -28,7 +28,7 @@ namespace steer_offset_estimator { using geometry_msgs::msg::TwistStamped; using tier4_debug_msgs::msg::Float32Stamped; -using Steering = autoware_auto_vehicle_msgs::msg::SteeringReport; +using Steering = autoware_vehicle_msgs::msg::SteeringReport; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; diff --git a/vehicle/steer_offset_estimator/package.xml b/vehicle/steer_offset_estimator/package.xml index 242ec5f2f148a..636d901d71ef5 100644 --- a/vehicle/steer_offset_estimator/package.xml +++ b/vehicle/steer_offset_estimator/package.xml @@ -9,7 +9,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs diagnostic_updater geometry_msgs rclcpp