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..451ade3771f61 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 @@ -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/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt index aafc62fc90d25..95224b9967d4b 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/CMakeLists.txt @@ -68,7 +68,7 @@ pluginlib_export_plugin_description_file(rviz_common plugins_description.xml) ament_target_dependencies(${PROJECT_NAME} PUBLIC rviz_common rviz_rendering - autoware_auto_vehicle_msgs + autoware_vehicle_msgs tier4_planning_msgs autoware_perception_msgs ) diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md index 0d0def1a46997..943d566ad109c 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/README.md @@ -15,13 +15,13 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t | Name | Type | Description | | ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------ | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | -| `/vehicle/status/turn_indicators_status` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | -| `/vehicle/status/hazard_status` | `autoware_auto_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic is status of gear | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | The topic is vehicle velocity | +| `/vehicle/status/turn_indicators_status` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/hazard_status` | `autoware_vehicle_msgs::msg::HazardReport` | The topic is status of hazard | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic is status of gear | | `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs::msg::VelocityLimit` | The topic is velocity limit | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | The topic is status of traffic light | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The topic is status of traffic light | ## Parameter diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp index 3fe473d5d5123..cb7e49685d0b3 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/gear_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_vehicle_msgs/msg/gear_report.hpp" #include #include @@ -37,7 +37,7 @@ class GearDisplay public: GearDisplay(); void drawGearIndicator(QPainter & painter, const QRectF & backgroundRect); - void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); private: int current_gear_; // Internal variable to store current gear diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp index ec1acb9a5dc68..0831ded468c99 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/signal_display.hpp @@ -95,29 +95,29 @@ private Q_SLOTS: std::unique_ptr traffic_display_; std::unique_ptr speed_limit_display_; - rclcpp::Subscription::SharedPtr gear_sub_; - rclcpp::Subscription::SharedPtr steering_sub_; - rclcpp::Subscription::SharedPtr speed_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr gear_sub_; + rclcpp::Subscription::SharedPtr steering_sub_; + rclcpp::Subscription::SharedPtr speed_sub_; + rclcpp::Subscription::SharedPtr turn_signals_sub_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr hazard_lights_sub_; - rclcpp::Subscription::SharedPtr traffic_sub_; + rclcpp::Subscription::SharedPtr + traffic_sub_; rclcpp::Subscription::SharedPtr speed_limit_sub_; std::mutex property_mutex_; - void updateGearData(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); - void updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); void updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg); + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg); void drawWidget(QImage & hud); }; } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp index 0669028dba3b2..2ae8e9a3fe0b9 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -37,7 +37,7 @@ class SpeedDisplay public: SpeedDisplay(); void drawSpeedDisplay(QPainter & painter, const QRectF & backgroundRect); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_speed_; // Internal variable to store current speed diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp index b59f4acf380db..fcdb293fe8c72 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/speed_limit_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -39,7 +39,7 @@ class SpeedLimitDisplay SpeedLimitDisplay(); void drawSpeedLimitIndicator(QPainter & painter, const QRectF & backgroundRect); void updateSpeedLimitData(const tier4_planning_msgs::msg::VelocityLimit::ConstSharedPtr msg); - void updateSpeedData(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); + void updateSpeedData(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg); private: float current_limit; // Internal variable to store current gear diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp index 121ee9a0a4d84..dada3cddab911 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/steering_wheel_display.hpp @@ -23,7 +23,7 @@ #include #include -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_vehicle_msgs/msg/steering_report.hpp" #include #include @@ -37,8 +37,7 @@ class SteeringWheelDisplay public: SteeringWheelDisplay(); void drawSteeringWheel(QPainter & painter, const QRectF & backgroundRect); - void updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); + void updateSteeringData(const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg); private: float steering_angle_ = 0.0f; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp index efa30f37ccb3e..fd15f542021f1 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/traffic_display.hpp @@ -23,8 +23,9 @@ #include #include -#include -#include +#include +#include +#include #include #include @@ -39,8 +40,8 @@ class TrafficDisplay TrafficDisplay(); void drawTrafficLightIndicator(QPainter & painter, const QRectF & backgroundRect); void updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg); - autoware_perception_msgs::msg::TrafficSignal current_traffic_; + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg); + autoware_perception_msgs::msg::TrafficLightGroupArray current_traffic_; private: QImage traffic_light_image_; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp index ca10c92c06a3e..022de6d8c22c9 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/include/turn_signals_display.hpp @@ -23,8 +23,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -41,9 +41,9 @@ class TurnSignalsDisplay TurnSignalsDisplay(); void drawArrows(QPainter & painter, const QRectF & backgroundRect, const QColor & color); void updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg); void updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg); private: QImage arrowImage; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml index 4d2f53e1e27ed..73e0dbea0866e 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/package.xml @@ -11,8 +11,8 @@ BSD-3-Clause - autoware_auto_vehicle_msgs autoware_perception_msgs + autoware_vehicle_msgs boost rviz_common rviz_ogre_vendor diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp index 825c7c1620e2c..956172bef6ed6 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/gear_display.cpp @@ -49,8 +49,7 @@ GearDisplay::GearDisplay() : current_gear_(0) } } -void GearDisplay::updateGearData( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) +void GearDisplay::updateGearData(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) { current_gear_ = msg->report; // Assuming msg->report contains the gear information } @@ -60,19 +59,19 @@ void GearDisplay::drawGearIndicator(QPainter & painter, const QRectF & backgroun // we deal with the different gears here std::string gearString; switch (current_gear_) { - case autoware_auto_vehicle_msgs::msg::GearReport::NEUTRAL: + case autoware_vehicle_msgs::msg::GearReport::NEUTRAL: gearString = "N"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::LOW: - case autoware_auto_vehicle_msgs::msg::GearReport::LOW_2: + case autoware_vehicle_msgs::msg::GearReport::LOW: + case autoware_vehicle_msgs::msg::GearReport::LOW_2: gearString = "L"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::NONE: - case autoware_auto_vehicle_msgs::msg::GearReport::PARK: + case autoware_vehicle_msgs::msg::GearReport::NONE: + case autoware_vehicle_msgs::msg::GearReport::PARK: gearString = "P"; break; - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE: - case autoware_auto_vehicle_msgs::msg::GearReport::REVERSE_2: + case autoware_vehicle_msgs::msg::GearReport::REVERSE: + case autoware_vehicle_msgs::msg::GearReport::REVERSE_2: gearString = "R"; break; // all the drive gears from DRIVE to DRIVE_16 diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp index 9add6336ece46..ccfdaa69f5d2a 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/signal_display.cpp @@ -76,31 +76,29 @@ void SignalDisplay::onInitialize() auto rviz_ros_node = context_->getRosNodeAbstraction(); gear_topic_property_ = std::make_unique( - "Gear Topic Test", "/vehicle/status/gear_status", "autoware_auto_vehicle_msgs/msg/GearReport", + "Gear Topic Test", "/vehicle/status/gear_status", "autoware_vehicle_msgs/msg/GearReport", "Topic for Gear Data", this, SLOT(topic_updated_gear())); gear_topic_property_->initialize(rviz_ros_node); turn_signals_topic_property_ = std::make_unique( "Turn Signals Topic", "/vehicle/status/turn_indicators_status", - "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, + "autoware_vehicle_msgs/msg/TurnIndicatorsReport", "Topic for Turn Signals Data", this, SLOT(topic_updated_turn_signals())); turn_signals_topic_property_->initialize(rviz_ros_node); speed_topic_property_ = std::make_unique( - "Speed Topic", "/vehicle/status/velocity_status", - "autoware_auto_vehicle_msgs/msg/VelocityReport", "Topic for Speed Data", this, - SLOT(topic_updated_speed())); + "Speed Topic", "/vehicle/status/velocity_status", "autoware_vehicle_msgs/msg/VelocityReport", + "Topic for Speed Data", this, SLOT(topic_updated_speed())); speed_topic_property_->initialize(rviz_ros_node); steering_topic_property_ = std::make_unique( - "Steering Topic", "/vehicle/status/steering_status", - "autoware_auto_vehicle_msgs/msg/SteeringReport", "Topic for Steering Data", this, - SLOT(topic_updated_steering())); + "Steering Topic", "/vehicle/status/steering_status", "autoware_vehicle_msgs/msg/SteeringReport", + "Topic for Steering Data", this, SLOT(topic_updated_steering())); steering_topic_property_->initialize(rviz_ros_node); hazard_lights_topic_property_ = std::make_unique( "Hazard Lights Topic", "/vehicle/status/hazard_lights_status", - "autoware_auto_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, + "autoware_vehicle_msgs/msg/HazardLightsReport", "Topic for Hazard Lights Data", this, SLOT(topic_updated_hazard_lights())); hazard_lights_topic_property_->initialize(rviz_ros_node); @@ -111,10 +109,9 @@ void SignalDisplay::onInitialize() speed_limit_topic_property_->initialize(rviz_ros_node); traffic_topic_property_ = std::make_unique( - "Traffic Topic", - "/planning/scenario_planning/lane_driving/behavior_planning/debug/traffic_signal", - "autoware_perception/msgs/msg/TrafficSignal", "Topic for Traffic Light Data", this, - SLOT(topic_updated_traffic())); + "Traffic Topic", "/perception/traffic_light_recognition/traffic_signals", + "autoware_perception_msgs/msgs/msg/TrafficLightGroupArray", "Topic for Traffic Light Data", + this, SLOT(topic_updated_traffic())); traffic_topic_property_->initialize(rviz_ros_node); } @@ -192,7 +189,7 @@ void SignalDisplay::onDisable() } void SignalDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr msg) { std::lock_guard lock(property_mutex_); @@ -213,7 +210,7 @@ void SignalDisplay::updateSpeedLimitData( } void SignalDisplay::updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -224,7 +221,7 @@ void SignalDisplay::updateHazardLightsData( } void SignalDisplay::updateGearData( - const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -235,7 +232,7 @@ void SignalDisplay::updateGearData( } void SignalDisplay::updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -246,7 +243,7 @@ void SignalDisplay::updateSteeringData( } void SignalDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -258,7 +255,7 @@ void SignalDisplay::updateSpeedData( } void SignalDisplay::updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) { std::lock_guard lock(property_mutex_); @@ -372,11 +369,10 @@ void SignalDisplay::topic_updated_gear() gear_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); gear_sub_ = - rviz_ros_node->get_raw_node()->create_subscription( - gear_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::GearReport::SharedPtr msg) { - updateGearData(msg); - }); + rviz_ros_node->get_raw_node()->create_subscription( + gear_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::GearReport::SharedPtr msg) { updateGearData(msg); }); } void SignalDisplay::topic_updated_steering() @@ -384,12 +380,13 @@ void SignalDisplay::topic_updated_steering() // resubscribe to the topic steering_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - steering_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - steering_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { - updateSteeringData(msg); - }); + steering_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + steering_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { + updateSteeringData(msg); + }); } void SignalDisplay::topic_updated_speed() @@ -397,12 +394,13 @@ void SignalDisplay::topic_updated_speed() // resubscribe to the topic speed_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - speed_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - speed_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { - updateSpeedData(msg); - }); + speed_sub_ = + rviz_ros_node->get_raw_node()->create_subscription( + speed_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { + updateSpeedData(msg); + }); } void SignalDisplay::topic_updated_speed_limit() @@ -427,9 +425,10 @@ void SignalDisplay::topic_updated_turn_signals() turn_signals_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - turn_signals_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { + ->create_subscription( + turn_signals_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::TurnIndicatorsReport::SharedPtr msg) { updateTurnSignalsData(msg); }); } @@ -442,9 +441,10 @@ void SignalDisplay::topic_updated_hazard_lights() hazard_lights_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - hazard_lights_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_auto_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { + ->create_subscription( + hazard_lights_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_vehicle_msgs::msg::HazardLightsReport::SharedPtr msg) { updateHazardLightsData(msg); }); } @@ -454,12 +454,14 @@ void SignalDisplay::topic_updated_traffic() // resubscribe to the topic traffic_sub_.reset(); auto rviz_ros_node = context_->getRosNodeAbstraction().lock(); - traffic_sub_ = rviz_ros_node->get_raw_node() - ->create_subscription( - traffic_topic_property_->getTopicStd(), rclcpp::QoS(rclcpp::KeepLast(10)), - [this](const autoware_perception_msgs::msg::TrafficSignal::SharedPtr msg) { - updateTrafficLightData(msg); - }); + traffic_sub_ = + rviz_ros_node->get_raw_node() + ->create_subscription( + traffic_topic_property_->getTopicStd(), + rclcpp::QoS(rclcpp::KeepLast(10)).durability_volatile().reliable(), + [this](const autoware_perception_msgs::msg::TrafficLightGroupArray::SharedPtr msg) { + updateTrafficLightData(msg); + }); } } // namespace autoware_overlay_rviz_plugin diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp index 5c5342259005b..b9c22ec5f53ac 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_display.cpp @@ -50,7 +50,7 @@ SpeedDisplay::SpeedDisplay() : current_speed_(0.0) } void SpeedDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { try { // Assuming msg->state.longitudinal_velocity_mps is the field you're interested in @@ -64,7 +64,7 @@ void SpeedDisplay::updateSpeedData( } // void SpeedDisplay::processMessage(const -// autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) +// autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) // { // try // { diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp index 4c83b4a73c0c2..3dec6a8e7d4a0 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/speed_limit_display.cpp @@ -57,7 +57,7 @@ void SpeedLimitDisplay::updateSpeedLimitData( } void SpeedLimitDisplay::updateSpeedData( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg) { try { float speed = msg->longitudinal_velocity; diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp index 45ebcde086165..d2390b16e5373 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/steering_wheel_display.cpp @@ -56,7 +56,7 @@ SteeringWheelDisplay::SteeringWheelDisplay() } void SteeringWheelDisplay::updateSteeringData( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr & msg) { try { // Assuming msg->steering_angle is the field you're interested in diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp index f6d232709a333..2dc9c23583a52 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/traffic_display.cpp @@ -48,7 +48,7 @@ TrafficDisplay::TrafficDisplay() } void TrafficDisplay::updateTrafficLightData( - const autoware_perception_msgs::msg::TrafficSignal::ConstSharedPtr & msg) + const autoware_perception_msgs::msg::TrafficLightGroupArray::ConstSharedPtr & msg) { current_traffic_ = *msg; } @@ -68,8 +68,8 @@ void TrafficDisplay::drawTrafficLightIndicator(QPainter & painter, const QRectF backgroundRect.top() + circleRect.height() / 2)); painter.drawEllipse(circleRect); - if (!current_traffic_.elements.empty()) { - switch (current_traffic_.elements[0].color) { + if (!current_traffic_.traffic_light_groups.empty()) { + switch (current_traffic_.traffic_light_groups[0].elements[0].color) { case 1: painter.setBrush(QBrush(tl_red_)); painter.drawEllipse(circleRect); diff --git a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp index 1aaa5871015a8..b42b5d953fcc6 100644 --- a/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp +++ b/common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/src/turn_signals_display.cpp @@ -46,7 +46,7 @@ TurnSignalsDisplay::TurnSignalsDisplay() : current_turn_signal_(0) } void TurnSignalsDisplay::updateTurnSignalsData( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr & msg) { try { // Assuming msg->report is the field you're interested in @@ -58,7 +58,7 @@ void TurnSignalsDisplay::updateTurnSignalsData( } void TurnSignalsDisplay::updateHazardLightsData( - const autoware_auto_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) + const autoware_vehicle_msgs::msg::HazardLightsReport::ConstSharedPtr & msg) { try { // Assuming msg->report is the field you're interested in @@ -80,11 +80,11 @@ void TurnSignalsDisplay::drawArrows( int rightArrowXPos = backgroundRect.right() - scaledRightArrow.width() * 3 - 175; bool leftActive = - (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || - current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); bool rightActive = - (current_turn_signal_ == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || - current_hazard_lights_ == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE); + (current_turn_signal_ == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT || + current_hazard_lights_ == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE); // Color the arrows based on the state of the turn signals and hazard lights by having them blink // on and off diff --git a/common/autoware_perception_rviz_plugin/README.md b/common/autoware_perception_rviz_plugin/README.md index cb5deb48f411c..ed6f3e1675ace 100644 --- a/common/autoware_perception_rviz_plugin/README.md +++ b/common/autoware_perception_rviz_plugin/README.md @@ -1,4 +1,4 @@ -# autoware_auto_perception_plugin +# autoware_perception_rviz_plugin ## Purpose @@ -19,9 +19,9 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ----------------------------------------------------- | ---------------------- | -| | `autoware_auto_perception_msgs::msg::DetectedObjects` | detection result array | +| Name | Type | Description | +| ---- | ------------------------------------------------ | ---------------------- | +| | `autoware_perception_msgs::msg::DetectedObjects` | detection result array | #### Visualization Result @@ -31,9 +31,9 @@ Example: #### Input Types -| Name | Type | Description | -| ---- | ---------------------------------------------------- | --------------------- | -| | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking result array | +| Name | Type | Description | +| ---- | ----------------------------------------------- | --------------------- | +| | `autoware_perception_msgs::msg::TrackedObjects` | tracking result array | #### Visualization Result @@ -45,9 +45,9 @@ Overwrite tracking results with detection results. #### Input Types -| Name | Type | Description | -| ---- | ------------------------------------------------------ | ----------------------- | -| | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | +| Name | Type | Description | +| ---- | ------------------------------------------------- | ----------------------- | +| | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array | #### Visualization Result diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp index b5e0f5c548c31..4dcc4e9ea1545 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/detected_objects_display.hpp @@ -16,7 +16,7 @@ #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include namespace autoware { @@ -26,12 +26,12 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize DetectedObjects class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC DetectedObjectsDisplay -: public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using DetectedObjects = autoware_auto_perception_msgs::msg::DetectedObjects; + using DetectedObjects = autoware_perception_msgs::msg::DetectedObjects; DetectedObjectsDisplay(); diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp index e1a911b55f719..8d29900e1da26 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_detail.hpp @@ -22,10 +22,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -62,21 +62,19 @@ enum class ObjectFillType { Skeleton, Fill }; // Map defining colors according to value of label field in ObjectClassification msg const std::map< - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> + autoware_perception_msgs::msg::ObjectClassification::_label_type, ObjectPropertyValues> // Color map is based on cityscapes color kDefaultObjectPropertyValues = { - {autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN, - {"UNKNOWN", {255, 255, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN, + {autoware_perception_msgs::msg::ObjectClassification::UNKNOWN, {"UNKNOWN", {255, 255, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::CAR, {"CAR", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::BUS, {"BUS", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN, {"PEDESTRIAN", {255, 192, 203}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE, + {autoware_perception_msgs::msg::ObjectClassification::BICYCLE, {"CYCLIST", {119, 11, 32}}}, + {autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE, {"MOTORCYCLE", {119, 11, 32}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER, - {"TRAILER", {30, 144, 255}}}, - {autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; + {autoware_perception_msgs::msg::ObjectClassification::TRAILER, {"TRAILER", {30, 144, 255}}}, + {autoware_perception_msgs::msg::ObjectClassification::TRUCK, {"TRUCK", {30, 144, 255}}}}; /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param shape_msg Shape msg to be converted. Corners should be in object-local frame @@ -87,7 +85,7 @@ const std::map< /// \return Marker ptr. Id and header will have to be set by the caller AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available = true, @@ -95,7 +93,7 @@ get_shape_marker_ptr( AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available = true); @@ -162,13 +160,13 @@ get_yaw_rate_covariance_marker_ptr( AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_arc_line_strip( @@ -183,35 +181,35 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_covariance_eigen_vectors( const Eigen::Matrix2d & matrix, double & sigma1, double & sigma2, double & yaw); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( @@ -219,15 +217,15 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( std::vector & points, const int n); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points); AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, + const autoware_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple = false); /// \brief Convert Point32 to Point @@ -276,7 +274,7 @@ inline AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC geometry_msgs::msg::Pose initPose( /// \return Id of the best classification, Unknown if there is no best label template AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC - autoware_auto_perception_msgs::msg::ObjectClassification::_label_type + autoware_perception_msgs::msg::ObjectClassification::_label_type get_best_label(ClassificationContainerT labels, const std::string & logger_name) { const auto best_class_label = std::max_element( @@ -287,7 +285,7 @@ AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC rclcpp::get_logger(logger_name), "Empty classification field. " "Treating as unknown"); - return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + return autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; } return best_class_label->label; } diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp index 2d86d8dd09db8..b05ba5f27f551 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include @@ -53,7 +53,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase using Color = std::array; using Marker = visualization_msgs::msg::Marker; using MarkerCommon = rviz_default_plugins::displays::MarkerCommon; - using ObjectClassificationMsg = autoware_auto_perception_msgs::msg::ObjectClassification; + using ObjectClassificationMsg = autoware_perception_msgs::msg::ObjectClassification; using RosTopicDisplay = rviz_common::RosTopicDisplay; using PolygonPropertyMap = @@ -189,7 +189,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase /// \return Marker ptr. Id and header will have to be set by the caller template std::optional get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const ClassificationContainerT & labels, const double & line_width, const bool & is_orientation_available) const @@ -212,7 +212,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase template visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available); @@ -363,8 +363,8 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_predicted_path_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const { if (m_display_predicted_paths_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); @@ -379,7 +379,7 @@ class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::optional get_path_confidence_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path) const + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const { if (m_display_path_confidence_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp index 0501d2ab3456d..1445f02e34290 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/predicted_objects_display.hpp @@ -16,7 +16,7 @@ #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include #include #include @@ -39,12 +39,12 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize PredictedObjects class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay -: public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; PredictedObjectsDisplay(); ~PredictedObjectsDisplay() diff --git a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp index 180494d742144..9ccaa5cd150f6 100644 --- a/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp +++ b/common/autoware_perception_rviz_plugin/include/autoware_perception_rviz_plugin/object_detection/tracked_objects_display.hpp @@ -16,7 +16,7 @@ #include "autoware_perception_rviz_plugin/object_detection/object_polygon_display_base.hpp" -#include +#include #include #include @@ -34,13 +34,13 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize TrackedObjects class AUTOWARE_PERCEPTION_RVIZ_PLUGIN_PUBLIC TrackedObjectsDisplay -: public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using TrackedObject = autoware_auto_perception_msgs::msg::TrackedObject; - using TrackedObjects = autoware_auto_perception_msgs::msg::TrackedObjects; + using TrackedObject = autoware_perception_msgs::msg::TrackedObject; + using TrackedObjects = autoware_perception_msgs::msg::TrackedObjects; TrackedObjectsDisplay(); diff --git a/common/autoware_perception_rviz_plugin/package.xml b/common/autoware_perception_rviz_plugin/package.xml index 66b7fafc39de5..460186c33b7d8 100644 --- a/common/autoware_perception_rviz_plugin/package.xml +++ b/common/autoware_perception_rviz_plugin/package.xml @@ -18,7 +18,7 @@ libboost-dev qtbase5-dev - autoware_auto_perception_msgs + autoware_perception_msgs rviz_common rviz_default_plugins diff --git a/common/autoware_perception_rviz_plugin/plugins_description.xml b/common/autoware_perception_rviz_plugin/plugins_description.xml index 50a0b5e5aff56..710e3aa2bfa26 100644 --- a/common/autoware_perception_rviz_plugin/plugins_description.xml +++ b/common/autoware_perception_rviz_plugin/plugins_description.xml @@ -6,21 +6,21 @@ Convert a PredictedObjects to markers and display them. - autoware_auto_perception_msgs/msg/PredictedObjects + autoware_perception_msgs/msg/PredictedObjects Convert a TrackedObjects to markers and display them. - autoware_auto_perception_msgs/msg/TrackedObjects + autoware_perception_msgs/msg/TrackedObjects Convert a DetectedObjects to markers and display them. - autoware_auto_perception_msgs/msg/DetectedObjects + autoware_perception_msgs/msg/DetectedObjects diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp index 335a323d4ecd5..9dd0b2923f09f 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/detected_objects_display.cpp @@ -40,7 +40,7 @@ void DetectedObjectsDisplay::processMessage(DetectedObjects::ConstSharedPtr msg) object.kinematics.pose_with_covariance.pose.orientation, object.classification, get_line_width(), object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 009091f33c272..1d06454582a2f 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -50,7 +50,7 @@ namespace detail using Marker = visualization_msgs::msg::Marker; visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color) { auto marker_ptr = std::make_shared(); @@ -70,8 +70,8 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple) { auto marker_ptr = std::make_shared(); @@ -492,7 +492,7 @@ visualization_msgs::msg::Marker::SharedPtr get_existence_probability_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available, const ObjectFillType fill_type) @@ -502,7 +502,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( marker_ptr->color = color_rgba; marker_ptr->scale.x = line_width; - using autoware_auto_perception_msgs::msg::Shape; + using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { if (fill_type == ObjectFillType::Skeleton) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -542,7 +542,7 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( } visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( - const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const autoware_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width, const bool & is_orientation_available) @@ -550,7 +550,7 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( auto marker_ptr = std::make_shared(); marker_ptr->ns = std::string("shape"); - using autoware_auto_perception_msgs::msg::Shape; + using autoware_perception_msgs::msg::Shape; if (shape_msg.type == Shape::BOUNDING_BOX) { marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); @@ -597,7 +597,7 @@ void calc_line_list_from_points( } void calc_bounding_box_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -620,7 +620,7 @@ void calc_bounding_box_line_list( } void calc_bounding_box_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { // direction triangle @@ -646,7 +646,7 @@ void calc_bounding_box_direction_line_list( } void calc_bounding_box_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -671,7 +671,7 @@ void calc_bounding_box_orientation_line_list( } void calc_2d_bounding_box_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -697,7 +697,7 @@ void calc_2d_bounding_box_bottom_line_list( } void calc_2d_bounding_box_bottom_direction_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -721,7 +721,7 @@ void calc_2d_bounding_box_bottom_direction_line_list( } void calc_2d_bounding_box_bottom_orientation_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double length_half = shape.dimensions.x * 0.5; @@ -746,7 +746,7 @@ void calc_2d_bounding_box_bottom_orientation_line_list( } void calc_cylinder_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double radius = shape.dimensions.x * 0.5; @@ -789,7 +789,7 @@ void calc_cylinder_line_list( } void calc_2d_cylinder_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { const double radius = shape.dimensions.x * 0.5; @@ -837,7 +837,7 @@ void calc_circle_line_list( } void calc_polygon_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { if (shape.footprint.points.size() < 2) { @@ -890,7 +890,7 @@ void calc_polygon_line_list( } void calc_2d_polygon_bottom_line_list( - const autoware_auto_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::Shape & shape, std::vector & points) { if (shape.footprint.points.size() < 2) { @@ -917,7 +917,7 @@ void calc_2d_polygon_bottom_line_list( } void calc_path_line_list( - const autoware_auto_perception_msgs::msg::PredictedPath & paths, + const autoware_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple) { const int circle_line_num = is_simple ? 5 : 10; diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 2a81732d93b95..d11aba912854d 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -295,7 +295,7 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) lock.unlock(); - ObjectPolygonDisplayBase::update( + ObjectPolygonDisplayBase::update( wall_dt, ros_dt); } diff --git a/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp index e8891e2bae897..214ed9ce70e63 100644 --- a/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp +++ b/common/autoware_perception_rviz_plugin/src/object_detection/tracked_objects_display.cpp @@ -61,7 +61,7 @@ void TrackedObjectsDisplay::processMessage(TrackedObjects::ConstSharedPtr msg) object.kinematics.pose_with_covariance.pose.orientation, object.classification, get_line_width(), object.kinematics.orientation_availability == - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); + autoware_perception_msgs::msg::DetectedObjectKinematics::AVAILABLE); if (shape_marker) { auto shape_marker_ptr = shape_marker.value(); shape_marker_ptr->header = msg->header; diff --git a/common/component_interface_specs/include/component_interface_specs/perception.hpp b/common/component_interface_specs/include/component_interface_specs/perception.hpp index 8665b9c35157d..0c72dbdb12078 100644 --- a/common/component_interface_specs/include/component_interface_specs/perception.hpp +++ b/common/component_interface_specs/include/component_interface_specs/perception.hpp @@ -17,14 +17,14 @@ #include -#include +#include namespace perception_interface { struct ObjectRecognition { - using Message = autoware_auto_perception_msgs::msg::PredictedObjects; + using Message = autoware_perception_msgs::msg::PredictedObjects; static constexpr char name[] = "/perception/object_recognition/objects"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/planning.hpp b/common/component_interface_specs/include/component_interface_specs/planning.hpp index 9efd8c871f68f..58aba53d8ac8a 100644 --- a/common/component_interface_specs/include/component_interface_specs/planning.hpp +++ b/common/component_interface_specs/include/component_interface_specs/planning.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include #include @@ -66,7 +66,7 @@ struct LaneletRoute struct Trajectory { - using Message = autoware_auto_planning_msgs::msg::Trajectory; + using Message = autoware_planning_msgs::msg::Trajectory; static constexpr char name[] = "/planning/scenario_planning/trajectory"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/include/component_interface_specs/vehicle.hpp b/common/component_interface_specs/include/component_interface_specs/vehicle.hpp index 6358b35c3c674..e7ce2c5212a25 100644 --- a/common/component_interface_specs/include/component_interface_specs/vehicle.hpp +++ b/common/component_interface_specs/include/component_interface_specs/vehicle.hpp @@ -20,10 +20,10 @@ #include #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include namespace vehicle_interface @@ -31,7 +31,7 @@ namespace vehicle_interface struct SteeringStatus { - using Message = autoware_auto_vehicle_msgs::msg::SteeringReport; + using Message = autoware_vehicle_msgs::msg::SteeringReport; static constexpr char name[] = "/vehicle/status/steering_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -40,7 +40,7 @@ struct SteeringStatus struct GearStatus { - using Message = autoware_auto_vehicle_msgs::msg::GearReport; + using Message = autoware_vehicle_msgs::msg::GearReport; static constexpr char name[] = "/vehicle/status/gear_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -49,7 +49,7 @@ struct GearStatus struct TurnIndicatorStatus { - using Message = autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; + using Message = autoware_vehicle_msgs::msg::TurnIndicatorsReport; static constexpr char name[] = "/vehicle/status/turn_indicators_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; @@ -58,7 +58,7 @@ struct TurnIndicatorStatus struct HazardLightStatus { - using Message = autoware_auto_vehicle_msgs::msg::HazardLightsReport; + using Message = autoware_vehicle_msgs::msg::HazardLightsReport; static constexpr char name[] = "/vehicle/status/hazard_lights_status"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 13f135c925258..fa57bb1641eed 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -12,10 +12,9 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs + autoware_vehicle_msgs nav_msgs rcl rclcpp diff --git a/common/fake_test_node/test/test_fake_test_node.cpp b/common/fake_test_node/test/test_fake_test_node.cpp index e065f332b75e4..163bd761407c5 100644 --- a/common/fake_test_node/test/test_fake_test_node.cpp +++ b/common/fake_test_node/test/test_fake_test_node.cpp @@ -17,7 +17,6 @@ /// \copyright Copyright 2021 Apex.AI, Inc. /// All rights reserved. -#include #include #include @@ -29,7 +28,7 @@ #include #include -using autoware::common::types::bool8_t; +using bool8_t = bool; using FakeNodeFixture = autoware::tools::testing::FakeTestNode; using FakeNodeFixtureParametrized = autoware::tools::testing::FakeTestNodeParametrized; diff --git a/common/goal_distance_calculator/Readme.md b/common/goal_distance_calculator/Readme.md index 3a25e147cf8e6..062b23baabe47 100644 --- a/common/goal_distance_calculator/Readme.md +++ b/common/goal_distance_calculator/Readme.md @@ -10,10 +10,10 @@ This node publishes deviation of self-pose from goal pose. ### Input -| Name | Type | Description | -| ---------------------------------- | ----------------------------------------- | --------------------- | -| `/planning/mission_planning/route` | `autoware_auto_planning_msgs::msg::Route` | Used to get goal pose | -| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | +| Name | Type | Description | +| ---------------------------------- | ------------------------------------ | --------------------- | +| `/planning/mission_planning/route` | `autoware_planning_msgs::msg::Route` | Used to get goal pose | +| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | ### Output diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp index b4a98d90c74e4..3329c1349b707 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ struct Param struct Input { geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose; - autoware_auto_planning_msgs::msg::Route::ConstSharedPtr route; + autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr route; }; struct Output diff --git a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp index 50051d928b6c1..680c4a3cdfff1 100644 --- a/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp +++ b/common/goal_distance_calculator/include/goal_distance_calculator/goal_distance_calculator_node.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include @@ -46,14 +46,14 @@ class GoalDistanceCalculatorNode : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_initial_pose_; tier4_autoware_utils::SelfPoseListener self_pose_listener_; - rclcpp::Subscription::SharedPtr sub_route_; + rclcpp::Subscription::SharedPtr sub_route_; // Data Buffer geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; - autoware_auto_planning_msgs::msg::Route::SharedPtr route_; + autoware_planning_msgs::msg::LaneletRoute::SharedPtr route_; // Callback - void onRoute(const autoware_auto_planning_msgs::msg::Route::ConstSharedPtr & msg); + void onRoute(const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & msg); // Publisher tier4_autoware_utils::DebugPublisher debug_publisher_; diff --git a/common/goal_distance_calculator/package.xml b/common/goal_distance_calculator/package.xml index 49cb674f0a256..eb71dc45f31a3 100644 --- a/common/goal_distance_calculator/package.xml +++ b/common/goal_distance_calculator/package.xml @@ -10,7 +10,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs geometry_msgs rclcpp rclcpp_components diff --git a/common/goal_distance_calculator/src/goal_distance_calculator.cpp b/common/goal_distance_calculator/src/goal_distance_calculator.cpp index 1405d5bd62f7d..719baef283791 100755 --- a/common/goal_distance_calculator/src/goal_distance_calculator.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator.cpp @@ -21,7 +21,7 @@ Output GoalDistanceCalculator::update(const Input & input) Output output{}; output.goal_deviation = - tier4_autoware_utils::calcPoseDeviation(input.route->goal_point.pose, input.current_pose->pose); + tier4_autoware_utils::calcPoseDeviation(input.route->goal_pose, input.current_pose->pose); return output; } diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index c6062d2a156ee..ab1c35e246719 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -48,9 +48,9 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions goal_distance_calculator_->setParam(param_); // Subscriber - sub_route_ = create_subscription( + sub_route_ = create_subscription( "/planning/mission_planning/route", queue_size, - [&](const autoware_auto_planning_msgs::msg::Route::SharedPtr msg_ptr) { route_ = msg_ptr; }); + [&](const autoware_planning_msgs::msg::LaneletRoute::SharedPtr msg_ptr) { route_ = msg_ptr; }); // Wait for first self pose self_pose_listener_.waitForFirstPose(); diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp index a7d7012c19e22..09973826387a2 100644 --- a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -199,7 +199,7 @@ TEST(spline_interpolation, SplineInterpolationPoints2d) TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::createPoint; std::vector points; diff --git a/common/motion_utils/README.md b/common/motion_utils/README.md index 925ee5b162db3..fd18540a9d0cf 100644 --- a/common/motion_utils/README.md +++ b/common/motion_utils/README.md @@ -47,7 +47,7 @@ The second function finds the nearest index in the lane whose id is `lane_id`. ```cpp size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, const int64_t lane_id); ``` @@ -112,6 +112,6 @@ const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_ ## For developers -Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. +Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::PathPoint`, `autoware_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. `motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/motion_utils/docs/vehicle/vehicle.md b/common/motion_utils/docs/vehicle/vehicle.md index 9d33b5ed372c4..c768e6115731c 100644 --- a/common/motion_utils/docs/vehicle/vehicle.md +++ b/common/motion_utils/docs/vehicle/vehicle.md @@ -83,10 +83,10 @@ This class check whether the vehicle arrive at stop point based on localization #### Subscribed Topics -| Name | Type | Description | -| ---------------------------------------- | ---------------------------------------------- | ---------------- | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | -| `/planning/scenario_planning/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | trajectory | +| Name | Type | Description | +| ---------------------------------------- | ----------------------------------------- | ---------------- | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | vehicle odometry | +| `/planning/scenario_planning/trajectory` | `autoware_planning_msgs::msg::Trajectory` | trajectory | #### Parameters diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 258386ca236a3..91322360ce5b8 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -15,9 +15,9 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/path.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include @@ -113,8 +113,8 @@ std::vector resamplePoseVector( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -136,11 +136,10 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, - const bool resample_input_path_stop_point = true); +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); /** * @brief A resampling function for a path. Note that in a default setting, position xy are @@ -159,8 +158,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * longitudinal and lateral velocity. Otherwise, it uses linear interpolation * @return resampled path */ -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); @@ -181,8 +180,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * @param resample_input_path_stop_point If true, resample closest stop point in input path * @return resampled path */ -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, const bool resample_input_path_stop_point = true); @@ -205,8 +204,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * longitudinal, lateral velocity and acceleration. Otherwise, it uses linear interpolation * @return resampled trajectory */ -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); @@ -230,10 +229,10 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( * trajectory * @return resampled trajectory */ -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_akima_spline_for_xy = false, - const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, + const bool use_zero_order_hold_for_twist = true, const bool resample_input_trajectory_stop_point = true); } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp index 7d4be216e89fe..8d009730a925d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/conversion.hpp @@ -15,52 +15,52 @@ #ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ #define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp" +#include "autoware_planning_msgs/msg/detail/path__struct.hpp" +#include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp" #include "std_msgs/msg/header.hpp" +#include "tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" #include namespace motion_utils { -using TrajectoryPoints = std::vector; +using TrajectoryPoints = std::vector; /** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @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. + * 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_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, const std_msgs::msg::Header & header = std_msgs::msg::Header{}); /** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory); +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory); template -autoware_auto_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) +autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPath can be used."); throw std::logic_error("Only specializations of convertToPath can be used."); } template <> -inline autoware_auto_planning_msgs::msg::Path convertToPath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input) +inline autoware_planning_msgs::msg::Path convertToPath( + const tier4_planning_msgs::msg::PathWithLaneId & input) { - autoware_auto_planning_msgs::msg::Path output{}; + autoware_planning_msgs::msg::Path output{}; output.header = input.header; output.left_bound = input.left_bound; output.right_bound = input.right_bound; @@ -80,11 +80,11 @@ TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input) template <> inline TrajectoryPoints convertToTrajectoryPoints( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input) + const tier4_planning_msgs::msg::PathWithLaneId & input) { TrajectoryPoints output{}; for (const auto & p : input.points) { - autoware_auto_planning_msgs::msg::TrajectoryPoint tp; + autoware_planning_msgs::msg::TrajectoryPoint tp; tp.pose = p.point.pose; tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; // since path point doesn't have acc for now @@ -95,20 +95,19 @@ inline TrajectoryPoints convertToTrajectoryPoints( } template -autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( - [[maybe_unused]] const T & input) +tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input) { static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used."); throw std::logic_error("Only specializations of convertToPathWithLaneId can be used."); } template <> -inline autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( +inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId( const TrajectoryPoints & input) { - autoware_auto_planning_msgs::msg::PathWithLaneId output{}; + tier4_planning_msgs::msg::PathWithLaneId output{}; for (const auto & p : input) { - autoware_auto_planning_msgs::msg::PathPointWithLaneId pp; + tier4_planning_msgs::msg::PathPointWithLaneId pp; pp.point.pose = p.pose; pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; output.points.emplace_back(pp); diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index aec329f9f540e..0a0b14cb7488d 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -17,8 +17,10 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" -#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" + +#include #include #include @@ -34,8 +36,8 @@ namespace motion_utils * twist information * @return resampled path(poses) */ -autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory, +autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( + const autoware_planning_msgs::msg::Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -49,8 +51,8 @@ autoware_auto_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint( * twist information * @return resampled path(poses) */ -autoware_auto_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, +tier4_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint( + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index d0ed761b76d9a..d6d4fbf559323 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,7 +15,7 @@ #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp" +#include "tier4_planning_msgs/msg/path_with_lane_id.hpp" #include #include @@ -23,23 +23,23 @@ namespace motion_utils { std::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id); size_t findNearestSegmentIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id); + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id); // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle // center follow the input path // @param [in] path with position to be followed by the center of the vehicle // @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle // center follow the input path NOTE: rear_to_cog is supposed to be positive -autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation = true); } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 7e2d92c9434fb..3715f466e7684 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -56,14 +56,12 @@ void validateNonEmpty(const T & points) } } -extern template void validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); -extern template void -validateNonEmpty>( - const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); +extern template void validateNonEmpty>( + const std::vector &); /** * @brief validate a point is in a non-sharp angle between two points or not @@ -114,14 +112,14 @@ std::optional isDrivingForward(const T & points) } extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); extern template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); /** * @brief checks whether a path of trajectory has forward driving direction using its longitudinal @@ -149,14 +147,14 @@ std::optional isDrivingForwardWithTwist(const T & points_with_twist) } extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); extern template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); /** * @brief remove overlapping points through points container. @@ -194,17 +192,16 @@ T removeOverlapPoints(const T & points, const size_t start_idx = 0) return dst; } -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx = 0); -extern template std::vector -removeOverlapPoints>( - const std::vector & points, +extern template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx = 0); /** @@ -237,8 +234,8 @@ std::optional searchZeroVelocityIndex( } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); /** @@ -262,8 +259,8 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx); /** @@ -279,8 +276,8 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist) } extern template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); +searchZeroVelocityIndex>( + const std::vector & points_with_twist); /** * @brief find nearest point index through points container for a given point. @@ -310,16 +307,14 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin return min_idx; } -extern template size_t findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -extern template size_t -findNearestIndex>( - const std::vector & points, +extern template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); /** @@ -378,18 +373,18 @@ std::optional findNearestIndex( } extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -462,19 +457,17 @@ double calcLongitudinalOffsetToSegment( } extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - const bool throw_exception = false); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); /** * @brief find nearest segment index to point. @@ -505,17 +498,16 @@ size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point return nearest_idx; } -extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +extern template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); extern template size_t -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); /** @@ -557,18 +549,18 @@ std::optional findNearestSegmentIndex( } extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -631,18 +623,17 @@ double calcLateralOffset( return cross_vec(2) / segment_vec.norm(); } -extern template double calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception = false); @@ -695,16 +686,15 @@ double calcLateralOffset( return calcLateralOffset(points, p_target, seg_idx, throw_exception); } -extern template double calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); extern template double -calcLateralOffset>( - const std::vector & points, +calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); -extern template double -calcLateralOffset>( - const std::vector & points, +extern template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); /** @@ -738,18 +728,17 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t return dist_sum; } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); /** * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, @@ -789,17 +778,17 @@ std::vector calcSignedArcLengthPartialSum( } extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); extern template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); /** * @brief calculate length of 2D distance between two points, specified by start point and end point @@ -831,17 +820,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector &, +calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); /** @@ -868,18 +856,17 @@ double calcSignedArcLength( return -calcSignedArcLength(points, dst_point, src_idx); } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); /** * @brief calculate length of 2D distance between two points, specified by start point and end @@ -916,17 +903,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); /** @@ -947,14 +933,12 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } -extern template double calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); -extern template double -calcArcLength>( - const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); +extern template double calcArcLength>( + const std::vector & points); /** * @brief calculate curvature through points container. @@ -986,14 +970,14 @@ std::vector calcCurvature(const T & points) } extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); extern template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); /** * @brief calculate curvature through points container and length of 2d distance for segment used @@ -1026,14 +1010,14 @@ std::vector> calcCurvatureAndArcLength(const T & point } extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); extern template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); /** * @brief calculate length of 2D distance between given start point index in points container and @@ -1063,8 +1047,8 @@ std::optional calcDistanceToForwardStopPoint( } extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const size_t src_idx = 0); /** @@ -1139,17 +1123,17 @@ std::optional calcLongitudinalOffsetPoint( } extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception = false); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); /** * @brief calculate the point offset from source point along the trajectory (or path) (points @@ -1184,16 +1168,16 @@ std::optional calcLongitudinalOffsetPoint( } extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); extern template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); /** @@ -1284,20 +1268,20 @@ std::optional calcLongitudinalOffsetPose( } extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, - const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); /** * @brief calculate the point offset from source point along the trajectory (or path) (points @@ -1331,18 +1315,18 @@ std::optional calcLongitudinalOffsetPose( } extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); extern template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); @@ -1433,19 +1417,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1488,19 +1472,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1561,19 +1545,19 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold = 1e-3); /** @@ -1614,21 +1598,21 @@ std::optional insertTargetPoint( } extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, + std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); @@ -1666,19 +1650,19 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1715,19 +1699,19 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1770,21 +1754,21 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); extern template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); @@ -1817,9 +1801,9 @@ std::optional insertStopPoint( } extern template std::optional -insertStopPoint>( +insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); /** @@ -1859,10 +1843,10 @@ std::optional insertDecelPoint( } extern template std::optional -insertDecelPoint>( +insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, - std::vector & points_with_twist); + std::vector & points_with_twist); /** * @brief Insert orientation to each point in points container (trajectory, path, ...) @@ -1901,15 +1885,13 @@ void insertOrientation(T & points, const bool is_driving_forward) } } -extern template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); -extern template void -insertOrientation>( - std::vector & points, +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); /** @@ -1968,19 +1950,18 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); @@ -2009,17 +1990,16 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, +extern template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); extern template double -calcSignedArcLength>( - const std::vector & points, +calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); /** @@ -2047,18 +2027,17 @@ double calcSignedArcLength( return signed_length_on_traj + signed_length_dst_offset; } -extern template double -calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +extern template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); extern template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); /** * @brief find first nearest point index through points container for a given pose with soft @@ -2149,20 +2128,20 @@ size_t findFirstNearestIndexWithSoftConstraints( } extern template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); -extern template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2205,20 +2184,20 @@ size_t findFirstNearestSegmentIndexWithSoftConstraints( } extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold = std::numeric_limits::max(), const double yaw_threshold = std::numeric_limits::max()); @@ -2270,18 +2249,18 @@ std::optional calcDistanceToForwardStopPoint( } extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); extern template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); @@ -2309,19 +2288,19 @@ T cropForwardPoints( return points; } -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -extern template std::vector -cropForwardPoints>( - const std::vector & points, +extern template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); @@ -2349,19 +2328,19 @@ T cropBackwardPoints( return points; } -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -extern template std::vector -cropBackwardPoints>( - const std::vector & points, +extern template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); @@ -2391,19 +2370,19 @@ T cropPoints( return cropped_points; } -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -extern template std::vector -cropPoints>( - const std::vector & points, +extern template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); @@ -2459,16 +2438,14 @@ double calcYawDeviation( return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); } -extern template double calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); -extern template double -calcYawDeviation>( - const std::vector & points, +extern template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); /** @@ -2495,18 +2472,16 @@ bool isTargetPointFront( return s_target - s_base > threshold; } -extern template bool isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); -extern template bool -isTargetPointFront>( - const std::vector & points, +extern template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold = 0.0); diff --git a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp index 82433d8e3c241..859bbdcc851ea 100644 --- a/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp +++ b/common/motion_utils/include/motion_utils/vehicle/vehicle_state_checker.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -26,7 +26,7 @@ namespace motion_utils { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index dec5287b0a520..21471a4a6a75e 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -22,8 +22,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs + autoware_planning_msgs + autoware_vehicle_msgs builtin_interfaces geometry_msgs interpolation @@ -32,6 +32,7 @@ tf2 tf2_geometry_msgs tier4_autoware_utils + tier4_planning_msgs visualization_msgs ament_cmake_ros diff --git a/common/motion_utils/src/factor/velocity_factor_interface.cpp b/common/motion_utils/src/factor/velocity_factor_interface.cpp index 20742af0b6c0c..6878c056b6f7a 100644 --- a/common/motion_utils/src/factor/velocity_factor_interface.cpp +++ b/common/motion_utils/src/factor/velocity_factor_interface.cpp @@ -15,9 +15,9 @@ #include #include -#include -#include -#include +#include +#include +#include namespace motion_utils { @@ -36,14 +36,14 @@ void VelocityFactorInterface::set( velocity_factor_.detail = detail; } -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, - const Pose &, const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, const Pose &, +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, + const VelocityFactorStatus, const std::string &); +template void VelocityFactorInterface::set( + const std::vector &, const Pose &, const Pose &, const VelocityFactorStatus, const std::string &); -template void VelocityFactorInterface::set( - const std::vector &, const Pose &, - const Pose &, const VelocityFactorStatus, const std::string &); } // namespace motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 834d07a87ea09..4a4e8dff1e4ad 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -181,8 +181,8 @@ std::vector resamplePoseVector( return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -333,13 +333,13 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( return input_path; } - autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path; + tier4_planning_msgs::msg::PathWithLaneId resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_auto_planning_msgs::msg::PathPoint path_point; + autoware_planning_msgs::msg::PathPoint path_point; path_point.pose = interpolated_pose.at(i); path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); path_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -352,9 +352,9 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( return resampled_path; } -autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( - const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, +tier4_planning_msgs::msg::PathWithLaneId resamplePath( + const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { // validate arguments @@ -363,7 +363,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( } // transform input_path - std::vector transformed_input_path( + std::vector transformed_input_path( input_path.points.size()); for (size_t i = 0; i < input_path.points.size(); ++i) { transformed_input_path.at(i) = input_path.points.at(i).point; @@ -418,8 +418,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( use_zero_order_hold_for_v); } -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { @@ -483,13 +483,13 @@ autoware_auto_planning_msgs::msg::Path resamplePath( return input_path; } - autoware_auto_planning_msgs::msg::Path resampled_path; + autoware_planning_msgs::msg::Path resampled_path; resampled_path.header = input_path.header; resampled_path.left_bound = input_path.left_bound; resampled_path.right_bound = input_path.right_bound; resampled_path.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_path.points.size(); ++i) { - autoware_auto_planning_msgs::msg::PathPoint path_point; + autoware_planning_msgs::msg::PathPoint path_point; path_point.pose = interpolated_pose.at(i); path_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); path_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -500,8 +500,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( return resampled_path; } -autoware_auto_planning_msgs::msg::Path resamplePath( - const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, +autoware_planning_msgs::msg::Path resamplePath( + const autoware_planning_msgs::msg::Path & input_path, const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) { @@ -559,8 +559,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( use_zero_order_hold_for_twist); } -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) { @@ -646,11 +646,11 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( return input_trajectory; } - autoware_auto_planning_msgs::msg::Trajectory resampled_trajectory; + autoware_planning_msgs::msg::Trajectory resampled_trajectory; resampled_trajectory.header = input_trajectory.header; resampled_trajectory.points.resize(interpolated_pose.size()); for (size_t i = 0; i < resampled_trajectory.points.size(); ++i) { - autoware_auto_planning_msgs::msg::TrajectoryPoint traj_point; + autoware_planning_msgs::msg::TrajectoryPoint traj_point; traj_point.pose = interpolated_pose.at(i); traj_point.longitudinal_velocity_mps = interpolated_v_lon.at(i); traj_point.lateral_velocity_mps = interpolated_v_lat.at(i); @@ -665,9 +665,9 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( return resampled_trajectory; } -autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, +autoware_planning_msgs::msg::Trajectory resampleTrajectory( + const autoware_planning_msgs::msg::Trajectory & input_trajectory, const double resample_interval, + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) { // validate arguments diff --git a/common/motion_utils/src/trajectory/conversion.cpp b/common/motion_utils/src/trajectory/conversion.cpp index f198003d84091..e3a221c61a2a7 100644 --- a/common/motion_utils/src/trajectory/conversion.cpp +++ b/common/motion_utils/src/trajectory/conversion.cpp @@ -19,34 +19,34 @@ namespace motion_utils { /** - * @brief Convert std::vector to - * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to - * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @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. + * 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_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory, +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory, const std_msgs::msg::Header & header) { - autoware_auto_planning_msgs::msg::Trajectory output{}; + autoware_planning_msgs::msg::Trajectory output{}; output.header = header; for (const auto & pt : trajectory) output.points.push_back(pt); return output; } /** - * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to - * std::vector. + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. */ -std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory) { - std::vector output(trajectory.points.size()); + std::vector output(trajectory.points.size()); std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); return output; } diff --git a/common/motion_utils/src/trajectory/interpolation.cpp b/common/motion_utils/src/trajectory/interpolation.cpp index 6ee3e665f746a..13df39daab59d 100644 --- a/common/motion_utils/src/trajectory/interpolation.cpp +++ b/common/motion_utils/src/trajectory/interpolation.cpp @@ -17,10 +17,10 @@ #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" -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; namespace motion_utils { diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/motion_utils/src/trajectory/path_with_lane_id.cpp index c7e85554dbddd..4c86135811003 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/motion_utils/src/trajectory/path_with_lane_id.cpp @@ -25,7 +25,7 @@ namespace motion_utils { std::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error @@ -56,8 +56,8 @@ std::optional> getPathIndexRangeWithLaneId( } size_t findNearestIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id) { const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); if (opt_range) { @@ -66,7 +66,7 @@ size_t findNearestIndexFromLaneId( validateNonEmpty(path.points); - const auto sub_points = std::vector{ + const auto sub_points = std::vector{ path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; validateNonEmpty(sub_points); @@ -77,8 +77,8 @@ size_t findNearestIndexFromLaneId( } size_t findNearestSegmentIndexFromLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) + const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos, + const int64_t lane_id) { const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); @@ -99,8 +99,8 @@ size_t findNearestSegmentIndexFromLaneId( } // NOTE: rear_to_cog is supposed to be positive -autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, +tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( + const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, const bool enable_last_point_compensation) { auto cog_path = path; diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp index 51c07a75076c8..42c750e3c109f 100644 --- a/common/motion_utils/src/trajectory/trajectory.cpp +++ b/common/motion_utils/src/trajectory/trajectory.cpp @@ -18,607 +18,584 @@ namespace motion_utils { // -template void validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); -template void validateNonEmpty>( - const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); // +template std::optional isDrivingForward>( + const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); template std::optional -isDrivingForward>( - const std::vector &); -template std::optional -isDrivingForward>( - const std::vector &); +isDrivingForward>( + const std::vector &); // template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); template std::optional -isDrivingForwardWithTwist>( - const std::vector &); +isDrivingForwardWithTwist>( + const std::vector &); // -template std::vector -removeOverlapPoints>( - const std::vector & points, const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, - const size_t start_idx); -template std::vector -removeOverlapPoints>( - const std::vector & points, +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist, +searchZeroVelocityIndex>( + const std::vector & points_with_twist, const size_t src_idx); // template std::optional -searchZeroVelocityIndex>( - const std::vector & points_with_twist); +searchZeroVelocityIndex>( + const std::vector & points_with_twist); // -template size_t findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t findNearestIndex>( - const std::vector & points, +template size_t findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); // template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestIndex>( - const std::vector & points, +findNearestIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, const size_t seg_idx, +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); template double -calcLongitudinalOffsetToSegment>( - const std::vector & points, - const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); // -template size_t findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); -template size_t -findNearestSegmentIndex>( - const std::vector & points, +template size_t findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Point & point); // template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); template std::optional -findNearestSegmentIndex>( - const std::vector & points, +findNearestSegmentIndex>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); // -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double -calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); -template double calcLateralOffset>( - const std::vector & points, +template double calcLateralOffset>( + const std::vector & points, const geometry_msgs::msg::Point & p_target, const bool throw_exception); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); // template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, const size_t src_idx, +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, const size_t dst_idx); template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); template std::vector -calcSignedArcLengthPartialSum>( - const std::vector & points, - const size_t src_idx, const size_t dst_idx); +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector &, +template double calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector &, +template double calcSignedArcLength>( + const std::vector &, const geometry_msgs::msg::Point & src_point, const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); // -template double calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); -template double calcArcLength>( - const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); // +template std::vector calcCurvature>( + const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); template std::vector -calcCurvature>( - const std::vector & points); -template std::vector -calcCurvature>( - const std::vector & points); +calcCurvature>( + const std::vector & points); // template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); template std::vector> -calcCurvatureAndArcLength>( - const std::vector & points); +calcCurvatureAndArcLength>( + const std::vector & points); // template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const size_t src_idx); // template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, - const size_t src_idx, const double offset, const bool throw_exception); +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); // template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); template std::optional -calcLongitudinalOffsetPoint>( - const std::vector & points, +calcLongitudinalOffsetPoint>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); // template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, const size_t src_idx, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, - const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); // template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); template std::optional -calcLongitudinalOffsetPose>( - const std::vector & points, +calcLongitudinalOffsetPose>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, - const double overlap_threshold); + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, - std::vector & points, + std::vector & points, const double overlap_threshold); // template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, + std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, - const double max_dist, const double max_yaw, const double overlap_threshold); + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); template std::optional -insertTargetPoint>( +insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, - std::vector & points, const double max_dist, + std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); template std::optional -insertStopPoint>( +insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold = 1e-3); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); // -template std::optional -insertStopPoint>( +template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, - const double max_dist, const double max_yaw, const double overlap_threshold); + std::vector & points_with_twist, const double max_dist, + const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); template std::optional -insertStopPoint>( +insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); // template std::optional -insertStopPoint>( +insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, - std::vector & points_with_twist, + std::vector & points_with_twist, const double overlap_threshold); // template std::optional -insertDecelPoint>( +insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, - std::vector & points_with_twist); + std::vector & points_with_twist); // -template void insertOrientation>( - std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); -template void insertOrientation>( - std::vector & points, +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); // -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double -calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); -template double calcSignedArcLength>( - const std::vector & points, +template double calcSignedArcLength>( + const std::vector & points, const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); // -template double calcSignedArcLength>( - const std::vector & points, const size_t src_idx, +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double -calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); -template double calcSignedArcLength>( - const std::vector & points, - const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); // template size_t -findFirstNearestIndexWithSoftConstraints>( - const std::vector & points, +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); -template size_t findFirstNearestIndexWithSoftConstraints< - std::vector>( - const std::vector & points, +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); // template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); template size_t findFirstNearestSegmentIndexWithSoftConstraints< - std::vector>( - const std::vector & points, + std::vector>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); // template std::optional -calcDistanceToForwardStopPoint>( - const std::vector & points_with_twist, +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); // -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); -template std::vector -cropForwardPoints>( - const std::vector & points, +template std::vector +cropForwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length); // -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); -template std::vector -cropBackwardPoints>( - const std::vector & points, +template std::vector +cropBackwardPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double backward_length); // -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); -template std::vector -cropPoints>( - const std::vector & points, +template std::vector +cropPoints>( + const std::vector & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, const double forward_length, const double backward_length); // -template double calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double -calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); -template double calcYawDeviation>( - const std::vector & points, +template double calcYawDeviation>( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const bool throw_exception); // -template bool isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool -isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); -template bool isTargetPointFront>( - const std::vector & points, +template bool isTargetPointFront>( + const std::vector & points, const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, const double threshold); diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp index 373a12c5bbd76..ef374f0b484db 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/motion_utils/test/src/resample/test_resample.cpp @@ -27,15 +27,15 @@ namespace { -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_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; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -297,7 +297,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -403,7 +403,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Duplicated points in the original path { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(11); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -437,7 +437,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -470,7 +470,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -504,7 +504,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -543,7 +543,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) using motion_utils::resamplePath; { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -651,7 +651,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_backward) // change initial orientation { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -775,7 +775,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -852,7 +852,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -931,7 +931,7 @@ TEST(resample_path_with_lane_id, resample_path_by_vector_non_default) // Lerp v { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPointWithLaneId( @@ -1626,7 +1626,7 @@ TEST(resample_path, resample_path_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -1708,7 +1708,7 @@ TEST(resample_path, resample_path_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = @@ -1742,7 +1742,7 @@ TEST(resample_path, resample_path_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -1776,7 +1776,7 @@ TEST(resample_path, resample_path_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -1815,7 +1815,7 @@ TEST(resample_path, resample_path_by_vector_backward) using motion_utils::resamplePath; { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); @@ -1896,7 +1896,7 @@ TEST(resample_path, resample_path_by_vector_backward) // change initial orientation { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, M_PI, i * 1.0, i * 0.5, i * 0.1); @@ -1994,7 +1994,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2053,7 +2053,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2115,7 +2115,7 @@ TEST(resample_path, resample_path_by_vector_non_default) // Lerp v { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2179,7 +2179,7 @@ TEST(resample_path, resample_path_by_same_interval) // Same point resampling { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2241,7 +2241,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2269,7 +2269,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without stop point but with terminal point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2314,7 +2314,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case without stop point but with terminal point (Boundary Condition) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2360,7 +2360,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case with duplicated zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2390,7 +2390,7 @@ TEST(resample_path, resample_path_by_same_interval) // Normal Case with zero point { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -2532,7 +2532,7 @@ TEST(resample_path, resample_path_by_same_interval) { // Input path size is not enough for resample { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(1); for (size_t i = 0; i < 1; ++i) { path.points.at(i) = @@ -2565,7 +2565,7 @@ TEST(resample_path, resample_path_by_same_interval) // Resample interval is invalid { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2598,7 +2598,7 @@ TEST(resample_path, resample_path_by_same_interval) // Resample interval is invalid (Negative value) { - autoware_auto_planning_msgs::msg::Path path; + autoware_planning_msgs::msg::Path path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = @@ -2698,7 +2698,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Output key is not same as input { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2787,7 +2787,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) { // Input path size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = @@ -2821,7 +2821,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Resampled Arclength size is not enough for interpolation { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2855,7 +2855,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector) // Resampled Arclength is longer than input path { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2895,7 +2895,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Lerp x, y { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -2959,7 +2959,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Slerp z { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = generateTestTrajectoryPoint( @@ -3025,7 +3025,7 @@ TEST(resample_trajectory, resample_trajectory_by_vector_non_default) // Lerp twist { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3094,7 +3094,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Same point resampling { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3160,7 +3160,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3190,7 +3190,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without stop point but with terminal point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3238,7 +3238,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case without stop point but with terminal point (Boundary Condition) { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3287,7 +3287,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case with duplicated zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3319,7 +3319,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Normal Case with zero point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3470,7 +3470,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) { // Input path size is not enough for resample { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = @@ -3503,7 +3503,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Resample interval is invalid { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -3536,7 +3536,7 @@ TEST(resample_trajectory, resample_trajectory_by_same_interval) // Resample interval is invalid (Negative value) { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = diff --git a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp index 549ca4c0ae5bb..7aae860b76533 100644 --- a/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp +++ b/common/motion_utils/test/src/trajectory/benchmark_calcLateralOffset.cpp @@ -22,7 +22,7 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::Trajectory; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; diff --git a/common/motion_utils/test/src/trajectory/test_interpolation.cpp b/common/motion_utils/test/src/trajectory/test_interpolation.cpp index 5794ed61e9e44..90b11e535c486 100644 --- a/common/motion_utils/test/src/trajectory/test_interpolation.cpp +++ b/common/motion_utils/test/src/trajectory/test_interpolation.cpp @@ -27,13 +27,13 @@ namespace { -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_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; constexpr double epsilon = 1e-6; @@ -127,7 +127,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) using motion_utils::calcInterpolatedPoint; { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -319,7 +319,7 @@ TEST(Interpolation, interpolate_path_for_trajectory) // Duplicated Points { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -351,7 +351,7 @@ TEST(Interpolation, interpolate_path_for_path) using motion_utils::calcInterpolatedPoint; { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -515,7 +515,7 @@ TEST(Interpolation, interpolate_path_for_path) // Duplicated Points { - autoware_auto_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathWithLaneId path; path.points.resize(10); for (size_t i = 0; i < 10; ++i) { path.points.at(i) = generateTestPathPoint(i * 1.0, 0.0, 0.0, 0.0, i * 1.0, i * 0.5, i * 0.1); @@ -543,7 +543,7 @@ TEST(Interpolation, interpolate_points_with_length) using motion_utils::calcInterpolatedPose; { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(10); for (size_t i = 0; i < 10; ++i) { traj.points.at(i) = @@ -631,7 +631,7 @@ TEST(Interpolation, interpolate_points_with_length) // one point { - autoware_auto_planning_msgs::msg::Trajectory traj; + autoware_planning_msgs::msg::Trajectory traj; traj.points.resize(1); for (size_t i = 0; i < 1; ++i) { traj.points.at(i) = generateTestTrajectoryPoint( diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index a65147a65b12d..6dd60c499d181 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -22,9 +22,9 @@ namespace { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::createPoint; +using tier4_planning_msgs::msg::PathPointWithLaneId; +using tier4_planning_msgs::msg::PathWithLaneId; geometry_msgs::msg::Pose createPose( double x, double y, double z, double roll, double pitch, double yaw) @@ -54,8 +54,8 @@ PathWithLaneId generateTestPathWithLaneId(const size_t num_points, const double TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { - using autoware_auto_planning_msgs::msg::PathWithLaneId; using motion_utils::getPathIndexRangeWithLaneId; + using tier4_planning_msgs::msg::PathWithLaneId; // Usual cases { diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 8e84b111b0688..9ebc712b3bf1a 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -26,8 +26,8 @@ namespace { -using autoware_auto_planning_msgs::msg::Trajectory; -using TrajectoryPointArray = std::vector; +using autoware_planning_msgs::msg::Trajectory; +using TrajectoryPointArray = std::vector; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; @@ -69,7 +69,7 @@ TrajectoryPointArray generateTestTrajectoryPointArray( const size_t num_points, const double point_interval, const double vel = 0.0, const double init_theta = 0.0, const double delta_theta = 0.0) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; TrajectoryPointArray traj; for (size_t i = 0; i < num_points; ++i) { const double theta = init_theta + i * delta_theta; @@ -106,7 +106,7 @@ TEST(trajectory, validateNonEmpty) TEST(trajectory, validateNonSharpAngle_DefaultThreshold) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::validateNonSharpAngle; TrajectoryPoint p1; @@ -135,7 +135,7 @@ TEST(trajectory, validateNonSharpAngle_DefaultThreshold) TEST(trajectory, validateNonSharpAngle_SetThreshold) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::validateNonSharpAngle; using tier4_autoware_utils::pi; @@ -1299,7 +1299,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::deg2rad; @@ -1385,7 +1385,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::deg2rad; @@ -1563,7 +1563,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; @@ -1637,7 +1637,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcArcLength; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; @@ -5393,7 +5393,7 @@ TEST(Trajectory, removeFirstInvalidOrientationPoints) TEST(trajectory, calcYawDeviation) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::calcYawDeviation; constexpr double tolerance = 1e-3; @@ -5420,7 +5420,7 @@ TEST(trajectory, calcYawDeviation) TEST(trajectory, isTargetPointFront) { - using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using autoware_planning_msgs::msg::TrajectoryPoint; using motion_utils::isTargetPointFront; using tier4_autoware_utils::createPoint; diff --git a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp index aa26a64cb87ce..29802e70bfd5f 100644 --- a/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp +++ b/common/motion_utils/test/src/vehicle/test_vehicle_state_checker_helper.hpp @@ -15,11 +15,11 @@ #ifndef VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ #define VEHICLE__TEST_VEHICLE_STATE_CHECKER_HELPER_HPP_ -#include -#include +#include +#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; inline Trajectory generateTrajectoryWithStopPoint(const geometry_msgs::msg::Pose & goal_pose) { diff --git a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp b/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp index c6dd0104b1e11..6d07d161bf069 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/conversion.hpp @@ -15,15 +15,15 @@ #ifndef OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ #define OBJECT_RECOGNITION_UTILS__CONVERSION_HPP_ -#include -#include +#include +#include namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object); DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects); diff --git a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp b/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp index ae3013c728032..28cc9a786ecac 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/geometry.hpp @@ -15,9 +15,9 @@ #ifndef OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ #define OBJECT_RECOGNITION_UTILS__GEOMETRY_HPP_ -#include -#include -#include +#include +#include +#include #include namespace object_recognition_utils @@ -36,22 +36,19 @@ inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::Pose & p) } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::DetectedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::DetectedObject & obj) { return obj.kinematics.pose_with_covariance.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::TrackedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::TrackedObject & obj) { return obj.kinematics.pose_with_covariance.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_perception_msgs::msg::PredictedObject & obj) +inline geometry_msgs::msg::Pose getPose(const autoware_perception_msgs::msg::PredictedObject & obj) { return obj.kinematics.initial_pose_with_covariance.pose; } diff --git a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp b/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp index 9c0745bb28af9..4ffafc22339d6 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/object_classification.hpp @@ -15,14 +15,14 @@ #ifndef OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ #define OBJECT_RECOGNITION_UTILS__OBJECT_CLASSIFICATION_HPP_ -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" #include #include namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; inline ObjectClassification getHighestProbClassification( const std::vector & object_classifications) diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp index 73cdce6c6444e..1cd838e090560 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp @@ -17,7 +17,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ namespace object_recognition_utils * @return interpolated pose */ boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time); + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time); /** * @brief Resampling predicted path by time step vector. Note this function does not substitute @@ -44,8 +44,8 @@ boost::optional calcInterpolatedPose( * time_step*(num_of_path_points)] * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy = true, const bool use_spline_for_z = false); @@ -57,10 +57,10 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( * @param sampling_horizon sampling time horizon * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, - const bool use_spline_for_xy = true, const bool use_spline_for_z = false); +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy = true, + const bool use_spline_for_z = false); } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 796e276c376d6..fdf73e6b26057 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs interpolation libboost-dev diff --git a/common/object_recognition_utils/src/conversion.cpp b/common/object_recognition_utils/src/conversion.cpp index f6a25cee2c7d5..c8b8e6585f5a7 100644 --- a/common/object_recognition_utils/src/conversion.cpp +++ b/common/object_recognition_utils/src/conversion.cpp @@ -16,10 +16,10 @@ namespace object_recognition_utils { -using autoware_auto_perception_msgs::msg::DetectedObject; -using autoware_auto_perception_msgs::msg::DetectedObjects; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::TrackedObject; +using autoware_perception_msgs::msg::TrackedObjects; DetectedObject toDetectedObject(const TrackedObject & tracked_object) { @@ -43,7 +43,7 @@ DetectedObject toDetectedObject(const TrackedObject & tracked_object) DetectedObjects toDetectedObjects(const TrackedObjects & tracked_objects) { - autoware_auto_perception_msgs::msg::DetectedObjects detected_objects; + autoware_perception_msgs::msg::DetectedObjects detected_objects; detected_objects.header = tracked_objects.header; for (auto & tracked_object : tracked_objects.objects) { diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index 81a22c98d88e5..fe9499b4eec33 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -23,7 +23,7 @@ namespace object_recognition_utils { boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time) + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time) { // Check if relative time is in the valid range if (path.path.empty() || relative_time < 0.0) { @@ -45,8 +45,8 @@ boost::optional calcInterpolatedPose( return boost::none; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy, const bool use_spline_for_z) { @@ -83,7 +83,7 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( const auto interpolated_z = use_spline_for_z ? spline(z) : lerp(z); const auto interpolated_quat = slerp(quat); - autoware_auto_perception_msgs::msg::PredictedPath resampled_path; + autoware_perception_msgs::msg::PredictedPath resampled_path; const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size()); resampled_path.confidence = path.confidence; resampled_path.path.resize(resampled_size); @@ -99,10 +99,9 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( return resampled_path; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy, - const bool use_spline_for_z) +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy, const bool use_spline_for_z) { if (path.path.empty()) { throw std::invalid_argument("Predicted Path is empty"); diff --git a/common/object_recognition_utils/test/src/test_conversion.cpp b/common/object_recognition_utils/test/src/test_conversion.cpp index 61fa0efd07d04..378f9a1245746 100644 --- a/common/object_recognition_utils/test/src/test_conversion.cpp +++ b/common/object_recognition_utils/test/src/test_conversion.cpp @@ -28,10 +28,10 @@ geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const return p; } -autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( const std::uint8_t label, const double probability) { - autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_perception_msgs::msg::ObjectClassification classification; classification.label = label; classification.probability = probability; @@ -42,10 +42,10 @@ autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassificat // NOTE: covariance is not checked TEST(conversion, test_toDetectedObject) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toDetectedObject; - autoware_auto_perception_msgs::msg::TrackedObject tracked_obj; + autoware_perception_msgs::msg::TrackedObject tracked_obj; // existence probability tracked_obj.existence_probability = 1.0; // classification @@ -160,10 +160,10 @@ TEST(conversion, test_toDetectedObject) // NOTE: covariance is not checked TEST(conversion, test_toTrackedObject) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toTrackedObject; - autoware_auto_perception_msgs::msg::DetectedObject detected_obj; + autoware_perception_msgs::msg::DetectedObject detected_obj; // existence probability detected_obj.existence_probability = 1.0; // classification diff --git a/common/object_recognition_utils/test/src/test_geometry.cpp b/common/object_recognition_utils/test/src/test_geometry.cpp index fe9f61b2424c6..ab3ba8fcec7d4 100644 --- a/common/object_recognition_utils/test/src/test_geometry.cpp +++ b/common/object_recognition_utils/test/src/test_geometry.cpp @@ -50,7 +50,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::DetectedObject p; + autoware_perception_msgs::msg::DetectedObject p; p.kinematics.pose_with_covariance.pose.position.x = x_ans; p.kinematics.pose_with_covariance.pose.position.y = y_ans; p.kinematics.pose_with_covariance.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::TrackedObject p; + autoware_perception_msgs::msg::TrackedObject p; p.kinematics.pose_with_covariance.pose.position.x = x_ans; p.kinematics.pose_with_covariance.pose.position.y = y_ans; p.kinematics.pose_with_covariance.pose.position.z = z_ans; @@ -90,7 +90,7 @@ TEST(geometry, getPose) } { - autoware_auto_perception_msgs::msg::PredictedObject p; + autoware_perception_msgs::msg::PredictedObject p; p.kinematics.initial_pose_with_covariance.pose.position.x = x_ans; p.kinematics.initial_pose_with_covariance.pose.position.y = y_ans; p.kinematics.initial_pose_with_covariance.pose.position.z = z_ans; diff --git a/common/object_recognition_utils/test/src/test_matching.cpp b/common/object_recognition_utils/test/src/test_matching.cpp index 8603dd54c433c..2ad34c3ae6bbe 100644 --- a/common/object_recognition_utils/test/src/test_matching.cpp +++ b/common/object_recognition_utils/test/src/test_matching.cpp @@ -15,7 +15,7 @@ #include "object_recognition_utils/matching.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include #include @@ -37,7 +37,7 @@ geometry_msgs::msg::Pose createPose(const double x, const double y, const double TEST(matching, test_get2dIoU) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dIoU; const double quart_circle = 0.16237976320958225; @@ -45,13 +45,13 @@ TEST(matching, test_get2dIoU) { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -61,13 +61,13 @@ TEST(matching, test_get2dIoU) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -77,13 +77,13 @@ TEST(matching, test_get2dIoU) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double iou = get2dIoU(source_obj, target_obj); @@ -103,15 +103,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) const double quart_circle = 0.16237976320958225; { // non overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -119,15 +119,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) } { // partially overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -135,15 +135,15 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) } { // fully overlapped - autoware_auto_perception_msgs::msg::DetectedObject source_obj; + autoware_perception_msgs::msg::DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; - autoware_auto_perception_msgs::msg::DetectedObject target_obj; + autoware_perception_msgs::msg::DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double giou = get2dGeneralizedIoU(source_obj, target_obj); @@ -153,20 +153,20 @@ TEST(object_recognition_utils, test_get2dGeneralizedIoU) TEST(matching, test_get2dPrecision) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dPrecision; const double quart_circle = 0.16237976320958225; { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -180,13 +180,13 @@ TEST(matching, test_get2dPrecision) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -200,13 +200,13 @@ TEST(matching, test_get2dPrecision) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double precision = get2dPrecision(source_obj, target_obj); @@ -220,20 +220,20 @@ TEST(matching, test_get2dPrecision) TEST(matching, test_get2dRecall) { - using autoware_auto_perception_msgs::msg::DetectedObject; + using autoware_perception_msgs::msg::DetectedObject; using object_recognition_utils::get2dRecall; const double quart_circle = 0.16237976320958225; { // non overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(1.5, 1.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); @@ -247,13 +247,13 @@ TEST(matching, test_get2dRecall) { // partially overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.5, 0.5, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); @@ -267,13 +267,13 @@ TEST(matching, test_get2dRecall) { // fully overlapped DetectedObject source_obj; source_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI); - source_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + source_obj.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; source_obj.shape.dimensions.x = 1.0; source_obj.shape.dimensions.y = 1.0; DetectedObject target_obj; target_obj.kinematics.pose_with_covariance.pose = createPose(0.0, 0.0, M_PI_2); - target_obj.shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + target_obj.shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; target_obj.shape.dimensions.x = 1.0; const double recall = get2dRecall(source_obj, target_obj); diff --git a/common/object_recognition_utils/test/src/test_object_classification.cpp b/common/object_recognition_utils/test/src/test_object_classification.cpp index 1637dc16346cd..697a7e8447732 100644 --- a/common/object_recognition_utils/test/src/test_object_classification.cpp +++ b/common/object_recognition_utils/test/src/test_object_classification.cpp @@ -20,10 +20,10 @@ constexpr double epsilon = 1e-06; namespace { -autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassification( +autoware_perception_msgs::msg::ObjectClassification createObjectClassification( const std::uint8_t label, const double probability) { - autoware_auto_perception_msgs::msg::ObjectClassification classification; + autoware_perception_msgs::msg::ObjectClassification classification; classification.label = label; classification.probability = probability; @@ -34,17 +34,17 @@ autoware_auto_perception_msgs::msg::ObjectClassification createObjectClassificat TEST(object_classification, test_getHighestProbLabel) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::getHighestProbLabel; { // empty - std::vector classifications; + std::vector classifications; std::uint8_t label = getHighestProbLabel(classifications); EXPECT_EQ(label, ObjectClassification::UNKNOWN); } { // normal case - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -54,7 +54,7 @@ TEST(object_classification, test_getHighestProbLabel) } { // labels with the same probability - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -67,7 +67,7 @@ TEST(object_classification, test_getHighestProbLabel) // Test isVehicle TEST(object_classification, test_isVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isVehicle; { // True Case with uint8_t @@ -87,7 +87,7 @@ TEST(object_classification, test_isVehicle) // True Case with object_classifications { // normal case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -96,7 +96,7 @@ TEST(object_classification, test_isVehicle) // False Case with object_classifications { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::PEDESTRIAN, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); EXPECT_FALSE(isVehicle(classification)); @@ -106,7 +106,7 @@ TEST(object_classification, test_isVehicle) // TEST isCarLikeVehicle TEST(object_classification, test_isCarLikeVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isCarLikeVehicle; { // True Case with uint8_t @@ -126,7 +126,7 @@ TEST(object_classification, test_isCarLikeVehicle) // True Case with object_classifications { // normal case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classification.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.7)); @@ -135,7 +135,7 @@ TEST(object_classification, test_isCarLikeVehicle) // False Case with object_classifications { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); EXPECT_FALSE(isCarLikeVehicle(classification)); @@ -146,7 +146,7 @@ TEST(object_classification, test_isCarLikeVehicle) // getHighestProbLabel() returns only first highest-scored label. // so, in edge case it returns a label earlier added. { // When car and non-car label has same probability - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); EXPECT_FALSE( @@ -157,7 +157,7 @@ TEST(object_classification, test_isCarLikeVehicle) // TEST isLargeVehicle TEST(object_classification, test_isLargeVehicle) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::isLargeVehicle; { // True Case with uint8_t @@ -176,7 +176,7 @@ TEST(object_classification, test_isLargeVehicle) } { // false case - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::MOTORCYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::BICYCLE, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); @@ -188,7 +188,7 @@ TEST(object_classification, test_isLargeVehicle) // getHighestProbLabel() returns only first highest-scored label. // so, in edge case it returns a label earlier added. { // When large-vehicle and non-large-vehicle label has same probability - std::vector classification; + std::vector classification; classification.push_back(createObjectClassification(ObjectClassification::BUS, 0.8)); classification.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); EXPECT_TRUE(isLargeVehicle(classification)); // evaluated with earlier appended "BUS" label @@ -197,18 +197,18 @@ TEST(object_classification, test_isLargeVehicle) TEST(object_classification, test_getHighestProbClassification) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::getHighestProbClassification; { // empty - std::vector classifications; + std::vector classifications; auto classification = getHighestProbClassification(classifications); EXPECT_EQ(classification.label, ObjectClassification::UNKNOWN); EXPECT_DOUBLE_EQ(classification.probability, 0.0); } { // normal case - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -219,7 +219,7 @@ TEST(object_classification, test_getHighestProbClassification) } { // labels with the same probability - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); @@ -232,7 +232,7 @@ TEST(object_classification, test_getHighestProbClassification) TEST(object_classification, test_fromString) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::toLabel; using object_recognition_utils::toObjectClassification; using object_recognition_utils::toObjectClassifications; @@ -266,7 +266,7 @@ TEST(object_classification, test_fromString) TEST(object_classification, test_convertLabelToString) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using object_recognition_utils::convertLabelToString; // from label @@ -290,7 +290,7 @@ TEST(object_classification, test_convertLabelToString) // from ObjectClassifications { - std::vector classifications; + std::vector classifications; classifications.push_back(createObjectClassification(ObjectClassification::CAR, 0.5)); classifications.push_back(createObjectClassification(ObjectClassification::TRUCK, 0.8)); classifications.push_back(createObjectClassification(ObjectClassification::BUS, 0.7)); 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 b466273f4defd..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 @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-06; namespace { -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedPath; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; @@ -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/common/path_distance_calculator/Readme.md b/common/path_distance_calculator/Readme.md index acbdc6998c46f..fed0476047ba9 100644 --- a/common/path_distance_calculator/Readme.md +++ b/common/path_distance_calculator/Readme.md @@ -11,10 +11,10 @@ Note that the distance means the arc-length along the path, not the Euclidean di ### Input -| Name | Type | Description | -| ----------------------------------------------------------------- | ---------------------------------------- | -------------- | -| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_auto_planning_msgs::msg::Path` | Reference path | -| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | +| Name | Type | Description | +| ----------------------------------------------------------------- | ----------------------------------- | -------------- | +| `/planning/scenario_planning/lane_driving/behavior_planning/path` | `autoware_planning_msgs::msg::Path` | Reference path | +| `/tf` | `tf2_msgs/TFMessage` | TF (self-pose) | ### Output diff --git a/common/path_distance_calculator/package.xml b/common/path_distance_calculator/package.xml index e796bbd0d20f7..0ccc6419b13de 100644 --- a/common/path_distance_calculator/package.xml +++ b/common/path_distance_calculator/package.xml @@ -10,7 +10,7 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs + autoware_planning_msgs motion_utils rclcpp rclcpp_components diff --git a/common/path_distance_calculator/src/path_distance_calculator.cpp b/common/path_distance_calculator/src/path_distance_calculator.cpp index aa20dd1ffc7ce..798f5df70c596 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.cpp +++ b/common/path_distance_calculator/src/path_distance_calculator.cpp @@ -25,9 +25,9 @@ PathDistanceCalculator::PathDistanceCalculator(const rclcpp::NodeOptions & options) : Node("path_distance_calculator", options), self_pose_listener_(this) { - sub_path_ = create_subscription( + sub_path_ = create_subscription( "~/input/path", rclcpp::QoS(1), - [this](const autoware_auto_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); + [this](const autoware_planning_msgs::msg::Path::SharedPtr msg) { path_ = msg; }); pub_dist_ = create_publisher("~/output/distance", rclcpp::QoS(1)); diff --git a/common/path_distance_calculator/src/path_distance_calculator.hpp b/common/path_distance_calculator/src/path_distance_calculator.hpp index f709701024f51..838afb903c55f 100644 --- a/common/path_distance_calculator/src/path_distance_calculator.hpp +++ b/common/path_distance_calculator/src/path_distance_calculator.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include class PathDistanceCalculator : public rclcpp::Node @@ -27,11 +27,11 @@ class PathDistanceCalculator : public rclcpp::Node explicit PathDistanceCalculator(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_path_; + rclcpp::Subscription::SharedPtr sub_path_; rclcpp::Publisher::SharedPtr pub_dist_; rclcpp::TimerBase::SharedPtr timer_; tier4_autoware_utils::SelfPoseListener self_pose_listener_; - autoware_auto_planning_msgs::msg::Path::SharedPtr path_; + autoware_planning_msgs::msg::Path::SharedPtr path_; }; #endif // PATH_DISTANCE_CALCULATOR_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 5d9001affa308..665e959ed2460 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -17,9 +17,9 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -36,14 +36,16 @@ geometry_msgs::msg::Polygon rotatePolygon( /// @return rotated polygon Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle); Polygon2d toPolygon2d( - const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::DetectedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::TrackedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::DetectedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::TrackedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::PredictedObject & object); Polygon2d toFootprint( const geometry_msgs::msg::Pose & base_link_pose, const double base_to_front, const double base_to_rear, const double width); -double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); +double getArea(const autoware_perception_msgs::msg::Shape & shape); Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 63cf6305e8158..334df8f32af91 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -27,9 +27,8 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include #include #include #include @@ -39,6 +38,7 @@ #include #include #include +#include #include @@ -129,21 +129,19 @@ inline geometry_msgs::msg::Point getPoint(const geometry_msgs::msg::PoseWithCova } template <> -inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::PathPoint & p) { return p.pose.position; } template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Point getPoint(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose.position; } template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline geometry_msgs::msg::Point getPoint(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.pose.position; } @@ -168,20 +166,19 @@ inline geometry_msgs::msg::Pose getPose(const geometry_msgs::msg::PoseStamped & } template <> -inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::PathPoint & p) { return p.pose; } template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline geometry_msgs::msg::Pose getPose(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.pose; } template <> -inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline geometry_msgs::msg::Pose getPose(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.pose; } @@ -194,20 +191,19 @@ double getLongitudinalVelocity([[maybe_unused]] const T & p) } template <> -inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::PathPoint & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::PathPoint & p) { return p.longitudinal_velocity_mps; } template <> -inline double getLongitudinalVelocity( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +inline double getLongitudinalVelocity(const tier4_planning_msgs::msg::PathPointWithLaneId & p) { return p.point.longitudinal_velocity_mps; } template <> -inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) +inline double getLongitudinalVelocity(const autoware_planning_msgs::msg::TrajectoryPoint & p) { return p.longitudinal_velocity_mps; } @@ -233,21 +229,21 @@ inline void setPose(const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::P template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPoint & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::PathPoint & p) { p.pose = pose; } template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const geometry_msgs::msg::Pose & pose, tier4_planning_msgs::msg::PathPointWithLaneId & p) { p.point.pose = pose; } template <> inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const geometry_msgs::msg::Pose & pose, autoware_planning_msgs::msg::TrajectoryPoint & p) { p.pose = pose; } @@ -269,21 +265,21 @@ void setLongitudinalVelocity([[maybe_unused]] const float velocity, [[maybe_unus template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) + const float velocity, autoware_planning_msgs::msg::TrajectoryPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::PathPoint & p) + const float velocity, autoware_planning_msgs::msg::PathPoint & p) { p.longitudinal_velocity_mps = velocity; } template <> inline void setLongitudinalVelocity( - const float velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) + const float velocity, tier4_planning_msgs::msg::PathPointWithLaneId & p) { p.point.longitudinal_velocity_mps = velocity; } diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index c34d3b5fdfdd0..334ee226a5f22 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -12,10 +12,10 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_vehicle_msgs autoware_internal_msgs + autoware_perception_msgs + autoware_planning_msgs + autoware_vehicle_msgs builtin_interfaces diagnostic_msgs geometry_msgs @@ -26,6 +26,7 @@ tf2 tf2_geometry_msgs tier4_debug_msgs + tier4_planning_msgs unique_identifier_msgs visualization_msgs diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index 79c9f9937cd4d..01e02d1cf0038 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -118,11 +118,11 @@ Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle) } Polygon2d toPolygon2d( - 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) { Polygon2d polygon; - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const auto point0 = tier4_autoware_utils::calcOffsetPose( pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) .position; @@ -140,7 +140,7 @@ Polygon2d toPolygon2d( appendPointToPolygon(polygon, point1); appendPointToPolygon(polygon, point2); appendPointToPolygon(polygon, point3); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { const double radius = shape.dimensions.x / 2.0; constexpr int circle_discrete_num = 6; for (int i = 0; i < circle_discrete_num; ++i) { @@ -157,7 +157,7 @@ Polygon2d toPolygon2d( pose.position.y; appendPointToPolygon(polygon, point); } - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { const double poly_yaw = tf2::getYaw(pose.orientation); const auto rotated_footprint = rotatePolygon(shape.footprint, poly_yaw); for (const auto rel_point : rotated_footprint.points) { @@ -180,21 +180,21 @@ Polygon2d toPolygon2d( } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::DetectedObject & object) + const autoware_perception_msgs::msg::DetectedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::TrackedObject & object) + const autoware_perception_msgs::msg::TrackedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.pose_with_covariance.pose, object.shape); } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::PredictedObject & object) + const autoware_perception_msgs::msg::PredictedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); @@ -223,13 +223,13 @@ Polygon2d toFootprint( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } -double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) +double getArea(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 getRectangleArea(shape.dimensions); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return getCircleArea(shape.dimensions); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { return getPolygonArea(shape.footprint); } diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp index 7370c3b650887..7689544bd79cc 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -137,8 +137,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -159,8 +159,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double diameter = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const auto poly = toPolygon2d(pose, shape); @@ -183,8 +183,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 0.5; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.push_back(createPoint32(-x, -y)); shape.footprint.points.push_back(createPoint32(-x, y)); shape.footprint.points.push_back(createPoint32(x, y)); @@ -240,8 +240,8 @@ TEST(boost_geometry, boost_getArea) const double x = 1.0; const double y = 2.0; - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -252,8 +252,8 @@ TEST(boost_geometry, boost_getArea) { // cylinder const double diameter = 1.0; - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const double area = getArea(shape); @@ -265,8 +265,8 @@ TEST(boost_geometry, boost_getArea) const double y = 2.0; // clock wise - autoware_auto_perception_msgs::msg::Shape clock_wise_shape; - clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape clock_wise_shape; + clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON; clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); clock_wise_shape.footprint.points.push_back(createPoint32(0.0, y)); clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); @@ -276,8 +276,8 @@ TEST(boost_geometry, boost_getArea) EXPECT_DOUBLE_EQ(clock_wise_area, -x * y); // anti clock wise - autoware_auto_perception_msgs::msg::Shape anti_clock_wise_shape; - anti_clock_wise_shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape anti_clock_wise_shape; + anti_clock_wise_shape.type = autoware_perception_msgs::msg::Shape::POLYGON; anti_clock_wise_shape.footprint.points.push_back(createPoint32(0.0, 0.0)); anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, 0.0)); anti_clock_wise_shape.footprint.points.push_back(createPoint32(x, y)); diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index c10e04736b5bb..b492da2d72e88 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -90,7 +90,7 @@ TEST(geometry, getPoint) } { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -101,7 +101,7 @@ TEST(geometry, getPoint) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -163,7 +163,7 @@ TEST(geometry, getPose) } { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -182,7 +182,7 @@ TEST(geometry, getPose) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.pose.position.x = x_ans; p.pose.position.y = y_ans; p.pose.position.z = z_ans; @@ -208,13 +208,13 @@ TEST(geometry, getLongitudinalVelocity) const double velocity = 1.0; { - autoware_auto_planning_msgs::msg::PathPoint p; + autoware_planning_msgs::msg::PathPoint p; p.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p; + autoware_planning_msgs::msg::TrajectoryPoint p; p.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } @@ -266,7 +266,7 @@ TEST(geometry, setPose) } { - autoware_auto_planning_msgs::msg::PathPoint p_out{}; + autoware_planning_msgs::msg::PathPoint p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); @@ -278,7 +278,7 @@ TEST(geometry, setPose) } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p_out{}; + autoware_planning_msgs::msg::TrajectoryPoint p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.pose.position.y, y_ans); @@ -313,13 +313,13 @@ TEST(geometry, setLongitudinalVelocity) const double velocity = 1.0; { - autoware_auto_planning_msgs::msg::PathPoint p{}; + autoware_planning_msgs::msg::PathPoint p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); } { - autoware_auto_planning_msgs::msg::TrajectoryPoint p{}; + autoware_planning_msgs::msg::TrajectoryPoint p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.longitudinal_velocity_mps, velocity); } diff --git a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index a8179f3b3cd60..5c55a09af95b1 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -24,7 +24,7 @@ TEST(geometry, getPoint_PathWithLaneId) const double y_ans = 2.0; const double z_ans = 3.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -46,7 +46,7 @@ TEST(geometry, getPose_PathWithLaneId) const double q_z_ans = 0.3; const double q_w_ans = 0.4; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.pose.position.x = x_ans; p.point.pose.position.y = y_ans; p.point.pose.position.z = z_ans; @@ -70,7 +70,7 @@ TEST(geometry, getLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p; + tier4_planning_msgs::msg::PathPointWithLaneId p; p.point.longitudinal_velocity_mps = velocity; EXPECT_DOUBLE_EQ(getLongitudinalVelocity(p), velocity); } @@ -96,7 +96,7 @@ TEST(geometry, setPose_PathWithLaneId) p.orientation.z = q_z_ans; p.orientation.w = q_w_ans; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p_out{}; + tier4_planning_msgs::msg::PathPointWithLaneId p_out{}; setPose(p, p_out); EXPECT_DOUBLE_EQ(p_out.point.pose.position.x, x_ans); EXPECT_DOUBLE_EQ(p_out.point.pose.position.y, y_ans); @@ -113,7 +113,7 @@ TEST(geometry, setLongitudinalVelocity_PathWithLaneId) const double velocity = 1.0; - autoware_auto_planning_msgs::msg::PathPointWithLaneId p{}; + tier4_planning_msgs::msg::PathPointWithLaneId p{}; setLongitudinalVelocity(velocity, p); EXPECT_DOUBLE_EQ(p.point.longitudinal_velocity_mps, velocity); } diff --git a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp index 4fc6aeb71771d..7dbbb65c94560 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/car_pose.hpp @@ -53,8 +53,8 @@ namespace rviz_plugins { -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 dummy_perception_publisher::msg::Object; class CarInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp index fc40b08be822b..c1ec65a0488bd 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/interactive_object.hpp @@ -82,8 +82,8 @@ namespace rviz_plugins { -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 dummy_perception_publisher::msg::Object; class InteractiveObject diff --git a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp index 195ec8e2a3a6d..642159aceaf3d 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/pedestrian_pose.hpp @@ -53,8 +53,8 @@ namespace rviz_plugins { -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 dummy_perception_publisher::msg::Object; class PedestrianInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp index 2bc3129b51321..3f1a9b55c30ab 100644 --- a/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp +++ b/common/tier4_perception_rviz_plugin/src/tools/unknown_pose.hpp @@ -48,8 +48,8 @@ namespace rviz_plugins { -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 dummy_perception_publisher::msg::Object; class UnknownInitialPoseTool : public InteractiveObjectTool diff --git a/common/tier4_planning_rviz_plugin/README.md b/common/tier4_planning_rviz_plugin/README.md index ef9df25f2b657..5b6286139af0d 100644 --- a/common/tier4_planning_rviz_plugin/README.md +++ b/common/tier4_planning_rviz_plugin/README.md @@ -11,11 +11,11 @@ This plugin displays the path, trajectory, and maximum speed. ### Input -| Name | Type | Description | -| -------------------------------------------------- | ---------------------------------------------- | ------------------------------------------ | -| `/input/path` | `autoware_auto_planning_msgs::msg::Path` | The topic on which to subscribe path | -| `/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | -| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | +| Name | Type | Description | +| -------------------------------------------------- | ----------------------------------------- | ------------------------------------------ | +| `/input/path` | `autoware_planning_msgs::msg::Path` | The topic on which to subscribe path | +| `/input/trajectory` | `autoware_planning_msgs::msg::Trajectory` | The topic on which to subscribe trajectory | +| `/planning/scenario_planning/current_max_velocity` | `tier4_planning_msgs/msg/VelocityLimit` | The topic on which to publish max velocity | ### Output diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp index 66b4a31f0993f..ab6a0b25fd3b4 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display.hpp @@ -17,9 +17,9 @@ #include "tier4_planning_rviz_plugin/path/display_base.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -188,7 +188,7 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay }; class AutowarePathWithLaneIdDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT public: @@ -198,9 +198,9 @@ class AutowarePathWithLaneIdDisplay private: void preProcessMessageDetail() override; void preVisualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; void visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) override; rviz_common::properties::BoolProperty property_lane_id_view_; @@ -212,7 +212,7 @@ class AutowarePathWithLaneIdDisplay }; class AutowarePathDisplay -: public AutowarePathWithDrivableAreaDisplay +: public AutowarePathWithDrivableAreaDisplay { Q_OBJECT private: @@ -220,7 +220,7 @@ class AutowarePathDisplay }; class AutowareTrajectoryDisplay -: public AutowarePathBaseDisplay +: public AutowarePathBaseDisplay { Q_OBJECT private: diff --git a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp index c915e38977012..ed79e122c0a01 100644 --- a/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/tier4_planning_rviz_plugin/path/display_base.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include diff --git a/common/tier4_planning_rviz_plugin/package.xml b/common/tier4_planning_rviz_plugin/package.xml index 4af4a371ef651..5d755d212c859 100644 --- a/common/tier4_planning_rviz_plugin/package.xml +++ b/common/tier4_planning_rviz_plugin/package.xml @@ -11,7 +11,6 @@ ament_cmake autoware_cmake - autoware_auto_planning_msgs autoware_planning_msgs libqt5-core libqt5-gui diff --git a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml index 0a80327199606..dac8c41670f3c 100644 --- a/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_planning_rviz_plugin/plugins/plugin_description.xml @@ -2,12 +2,12 @@ - Display path points of autoware_auto_planning_msg::Path + Display path points of autoware_planning_msg::Path - Display path_with_lane_id of autoware_auto_planning_msg::PathWithLaneId + Display path_with_lane_id of autoware_planning_msg::PathWithLaneId - Display trajectory points of autoware_auto_planning_msg::Trajectory + Display trajectory points of autoware_planning_msg::Trajectory points.size(); // clear previous text @@ -73,8 +73,7 @@ void AutowarePathWithLaneIdDisplay::preVisualizePathFootprintDetail( } void AutowarePathWithLaneIdDisplay::visualizePathFootprintDetail( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, - const size_t p_idx) + const tier4_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, const size_t p_idx) { const auto & point = msg_ptr->points.at(p_idx); diff --git a/common/tier4_state_rviz_plugin/README.md b/common/tier4_state_rviz_plugin/README.md index ac1e188fb36cd..e45be3bea13dc 100644 --- a/common/tier4_state_rviz_plugin/README.md +++ b/common/tier4_state_rviz_plugin/README.md @@ -16,7 +16,7 @@ This plugin also can engage from the panel. | `/api/localization/initialization_state` | `autoware_adapi_v1_msgs::msg::LocalizationInitializationState` | The topic represents the state of localization initialization | | `/api/motion/state` | `autoware_adapi_v1_msgs::msg::MotionState` | The topic represents the state of motion | | `/api/autoware/get/emergency` | `tier4_external_api_msgs::msg::Emergency` | The topic represents the state of external emergency | -| `/vehicle/status/gear_status` | `autoware_auto_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | +| `/vehicle/status/gear_status` | `autoware_vehicle_msgs::msg::GearReport` | The topic represents the state of gear | ### Output diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 1db152e9632f8..65ccd217d8016 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -13,8 +13,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs + autoware_vehicle_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp index 9863cbbbf802b..c1ab9018e12f9 100644 --- a/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/include/autoware_state_panel.hpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include @@ -109,12 +109,12 @@ public Q_SLOTS: // NOLINT for Qt QVBoxLayout * makeFailSafeGroup(); // QVBoxLayout * makeDiagnosticGroup(); - // void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); + // void onShift(const autoware_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; - rclcpp::Subscription::SharedPtr sub_gear_; + rclcpp::Subscription::SharedPtr sub_gear_; rclcpp::Client::SharedPtr client_emergency_stop_; rclcpp::Subscription::SharedPtr sub_emergency_; diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md index 61260ecfda723..763504c4709a9 100644 --- a/common/tier4_system_rviz_plugin/README.md +++ b/common/tier4_system_rviz_plugin/README.md @@ -6,6 +6,6 @@ This plugin display the Hazard information from Autoware; and output notices whe ## Input -| Name | Type | Description | -| --------------------------------- | ----------------------------------------------------- | ------------------------------------------------------------ | -| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | +| Name | Type | Description | +| --------------------------------- | ------------------------------------------------ | ------------------------------------------------------------ | +| `/system/emergency/hazard_status` | `autoware_system_msgs::msg::HazardStatusStamped` | The topic represents the emergency information from Autoware | diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml index adae997ea07aa..bd72ecf97def4 100644 --- a/common/tier4_system_rviz_plugin/package.xml +++ b/common/tier4_system_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_system_msgs + autoware_system_msgs diagnostic_msgs libqt5-core libqt5-gui diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp index 61c2bd378dab1..c0db8cc86450b 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include @@ -162,7 +162,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) // MRM summary std::vector mrm_comfortable_stop_summary_list = {}; std::vector mrm_emergency_stop_summary_list = {}; - int hazard_level = autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT; + int hazard_level = autoware_system_msgs::msg::HazardStatus::NO_FAULT; { std::lock_guard message_lock(mutex_); if (last_msg_ptr_) { @@ -207,7 +207,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) std::ostringstream output_text; // Display the MRM Summary only when there is a fault - if (hazard_level != autoware_auto_system_msgs::msg::HazardStatus::NO_FAULT) { + if (hazard_level != autoware_system_msgs::msg::HazardStatus::NO_FAULT) { // Broadcasting the Basic Error Infos int number_of_comfortable_stop_messages = static_cast(mrm_comfortable_stop_summary_list.size()); @@ -243,7 +243,7 @@ void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) } void MrmSummaryOverlayDisplay::processMessage( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp index d0b0e32c3788f..0f59ba5582fed 100644 --- a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp @@ -59,14 +59,14 @@ #include #include -#include +#include #endif namespace rviz_plugins { class MrmSummaryOverlayDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -85,7 +85,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; + const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -101,7 +101,7 @@ private Q_SLOTS: static constexpr int hand_width_ = 4; std::mutex mutex_; - autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; + autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_traffic_light_rviz_plugin/README.md b/common/tier4_traffic_light_rviz_plugin/README.md index 91fb5bc124cb7..cc37d17768d49 100644 --- a/common/tier4_traffic_light_rviz_plugin/README.md +++ b/common/tier4_traffic_light_rviz_plugin/README.md @@ -8,9 +8,9 @@ This plugin panel publishes dummy traffic light signals. ### Output -| Name | Type | Description | -| ------------------------------------------------------- | --------------------------------------------------- | ----------------------------- | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficSignalArray` | Publish traffic light signals | +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ----------------------------- | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | Publish traffic light signals | ## HowToUse diff --git a/common/tier4_traffic_light_rviz_plugin/package.xml b/common/tier4_traffic_light_rviz_plugin/package.xml index 2b118b429f1af..b123f8e2bdc3c 100644 --- a/common/tier4_traffic_light_rviz_plugin/package.xml +++ b/common/tier4_traffic_light_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs + autoware_map_msgs autoware_perception_msgs lanelet2_extension libqt5-core diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp index 35c5a88a2f8a6..db33abd72283d 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.cpp @@ -139,66 +139,66 @@ void TrafficLightPublishPanel::onSetTrafficLightState() const auto shape = light_shape_combo_->currentText(); const auto status = light_status_combo_->currentText(); - TrafficSignalElement traffic_light; + TrafficLightElement traffic_light; traffic_light.confidence = traffic_light_confidence_input_->value(); if (color == "RED") { - traffic_light.color = TrafficSignalElement::RED; + traffic_light.color = TrafficLightElement::RED; } else if (color == "AMBER") { - traffic_light.color = TrafficSignalElement::AMBER; + traffic_light.color = TrafficLightElement::AMBER; } else if (color == "GREEN") { - traffic_light.color = TrafficSignalElement::GREEN; + traffic_light.color = TrafficLightElement::GREEN; } else if (color == "WHITE") { - traffic_light.color = TrafficSignalElement::WHITE; + traffic_light.color = TrafficLightElement::WHITE; } else if (color == "UNKNOWN") { - traffic_light.color = TrafficSignalElement::UNKNOWN; + traffic_light.color = TrafficLightElement::UNKNOWN; } if (shape == "CIRCLE") { - traffic_light.shape = TrafficSignalElement::CIRCLE; + traffic_light.shape = TrafficLightElement::CIRCLE; } else if (shape == "LEFT_ARROW") { - traffic_light.shape = TrafficSignalElement::LEFT_ARROW; + traffic_light.shape = TrafficLightElement::LEFT_ARROW; } else if (shape == "RIGHT_ARROW") { - traffic_light.shape = TrafficSignalElement::RIGHT_ARROW; + traffic_light.shape = TrafficLightElement::RIGHT_ARROW; } else if (shape == "UP_ARROW") { - traffic_light.shape = TrafficSignalElement::UP_ARROW; + traffic_light.shape = TrafficLightElement::UP_ARROW; } else if (shape == "DOWN_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_ARROW; } else if (shape == "DOWN_LEFT_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_LEFT_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_LEFT_ARROW; } else if (shape == "DOWN_RIGHT_ARROW") { - traffic_light.shape = TrafficSignalElement::DOWN_RIGHT_ARROW; + traffic_light.shape = TrafficLightElement::DOWN_RIGHT_ARROW; } else if (shape == "UNKNOWN") { - traffic_light.shape = TrafficSignalElement::UNKNOWN; + traffic_light.shape = TrafficLightElement::UNKNOWN; } if (status == "SOLID_OFF") { - traffic_light.status = TrafficSignalElement::SOLID_OFF; + traffic_light.status = TrafficLightElement::SOLID_OFF; } else if (status == "SOLID_ON") { - traffic_light.status = TrafficSignalElement::SOLID_ON; + traffic_light.status = TrafficLightElement::SOLID_ON; } else if (status == "FLASHING") { - traffic_light.status = TrafficSignalElement::FLASHING; + traffic_light.status = TrafficLightElement::FLASHING; } else if (status == "UNKNOWN") { - traffic_light.status = TrafficSignalElement::UNKNOWN; + traffic_light.status = TrafficLightElement::UNKNOWN; } - TrafficSignal traffic_signal; + TrafficLightGroup traffic_signal; traffic_signal.elements.push_back(traffic_light); - traffic_signal.traffic_signal_id = traffic_light_id; + traffic_signal.traffic_light_group_id = traffic_light_id; - for (auto & signal : extra_traffic_signals_.signals) { - if (signal.traffic_signal_id == traffic_light_id) { + for (auto & signal : extra_traffic_signals_.traffic_light_groups) { + if (signal.traffic_light_group_id == traffic_light_id) { signal = traffic_signal; return; } } - extra_traffic_signals_.signals.push_back(traffic_signal); + extra_traffic_signals_.traffic_light_groups.push_back(traffic_signal); } void TrafficLightPublishPanel::onResetTrafficLightState() { - extra_traffic_signals_.signals.clear(); + extra_traffic_signals_.traffic_light_groups.clear(); enable_publish_ = false; publish_button_->setText("PUBLISH"); @@ -218,10 +218,10 @@ void TrafficLightPublishPanel::onInitialize() using std::placeholders::_1; raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - pub_traffic_signals_ = raw_node_->create_publisher( + pub_traffic_signals_ = raw_node_->create_publisher( "/perception/traffic_light_recognition/traffic_signals", rclcpp::QoS(1)); - sub_vector_map_ = raw_node_->create_subscription( + sub_vector_map_ = raw_node_->create_subscription( "/map/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&TrafficLightPublishPanel::onVectorMap, this, _1)); createWallTimer(); @@ -252,20 +252,20 @@ void TrafficLightPublishPanel::onTimer() pub_traffic_signals_->publish(extra_traffic_signals_); } - traffic_table_->setRowCount(extra_traffic_signals_.signals.size()); + traffic_table_->setRowCount(extra_traffic_signals_.traffic_light_groups.size()); - if (extra_traffic_signals_.signals.empty()) { + if (extra_traffic_signals_.traffic_light_groups.empty()) { return; } - for (size_t i = 0; i < extra_traffic_signals_.signals.size(); ++i) { - const auto & signal = extra_traffic_signals_.signals.at(i); + for (size_t i = 0; i < extra_traffic_signals_.traffic_light_groups.size(); ++i) { + const auto & signal = extra_traffic_signals_.traffic_light_groups.at(i); if (signal.elements.empty()) { continue; } - auto id_label = new QLabel(QString::number(signal.traffic_signal_id)); + auto id_label = new QLabel(QString::number(signal.traffic_light_group_id)); id_label->setAlignment(Qt::AlignCenter); auto color_label = new QLabel(); @@ -273,23 +273,23 @@ void TrafficLightPublishPanel::onTimer() const auto & light = signal.elements.front(); switch (light.color) { - case TrafficSignalElement::RED: + case TrafficLightElement::RED: color_label->setText("RED"); color_label->setStyleSheet("background-color: #FF0000;"); break; - case TrafficSignalElement::AMBER: + case TrafficLightElement::AMBER: color_label->setText("AMBER"); color_label->setStyleSheet("background-color: #FFBF00;"); break; - case TrafficSignalElement::GREEN: + case TrafficLightElement::GREEN: color_label->setText("GREEN"); color_label->setStyleSheet("background-color: #7CFC00;"); break; - case TrafficSignalElement::WHITE: + case TrafficLightElement::WHITE: color_label->setText("WHITE"); color_label->setStyleSheet("background-color: #FFFFFF;"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: color_label->setText("UNKNOWN"); color_label->setStyleSheet("background-color: #808080;"); break; @@ -301,28 +301,28 @@ void TrafficLightPublishPanel::onTimer() shape_label->setAlignment(Qt::AlignCenter); switch (light.shape) { - case TrafficSignalElement::CIRCLE: + case TrafficLightElement::CIRCLE: shape_label->setText("CIRCLE"); break; - case TrafficSignalElement::LEFT_ARROW: + case TrafficLightElement::LEFT_ARROW: shape_label->setText("LEFT_ARROW"); break; - case TrafficSignalElement::RIGHT_ARROW: + case TrafficLightElement::RIGHT_ARROW: shape_label->setText("RIGHT_ARROW"); break; - case TrafficSignalElement::UP_ARROW: + case TrafficLightElement::UP_ARROW: shape_label->setText("UP_ARROW"); break; - case TrafficSignalElement::DOWN_ARROW: + case TrafficLightElement::DOWN_ARROW: shape_label->setText("DOWN_ARROW"); break; - case TrafficSignalElement::DOWN_LEFT_ARROW: + case TrafficLightElement::DOWN_LEFT_ARROW: shape_label->setText("DOWN_LEFT_ARROW"); break; - case TrafficSignalElement::DOWN_RIGHT_ARROW: + case TrafficLightElement::DOWN_RIGHT_ARROW: shape_label->setText("DOWN_RIGHT_ARROW"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: shape_label->setText("UNKNOWN"); break; default: @@ -333,16 +333,16 @@ void TrafficLightPublishPanel::onTimer() status_label->setAlignment(Qt::AlignCenter); switch (light.status) { - case TrafficSignalElement::SOLID_OFF: + case TrafficLightElement::SOLID_OFF: status_label->setText("SOLID_OFF"); break; - case TrafficSignalElement::SOLID_ON: + case TrafficLightElement::SOLID_ON: status_label->setText("SOLID_ON"); break; - case TrafficSignalElement::FLASHING: + case TrafficLightElement::FLASHING: status_label->setText("FLASHING"); break; - case TrafficSignalElement::UNKNOWN: + case TrafficLightElement::UNKNOWN: status_label->setText("UNKNOWN"); break; default: @@ -361,7 +361,7 @@ void TrafficLightPublishPanel::onTimer() traffic_table_->update(); } -void TrafficLightPublishPanel::onVectorMap(const HADMapBin::ConstSharedPtr msg) +void TrafficLightPublishPanel::onVectorMap(const LaneletMapBin::ConstSharedPtr msg) { if (received_vector_map_) return; // NOTE: examples from map_loader/lanelet2_map_visualization_node.cpp diff --git a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp index e54b6a301873b..2979563062fea 100644 --- a/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp +++ b/common/tier4_traffic_light_rviz_plugin/src/traffic_light_publish_panel.hpp @@ -26,8 +26,8 @@ #include #include -#include -#include +#include +#include #endif #include @@ -35,10 +35,10 @@ namespace rviz_plugins { -using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_perception_msgs::msg::TrafficSignal; -using autoware_perception_msgs::msg::TrafficSignalArray; -using autoware_perception_msgs::msg::TrafficSignalElement; +using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::TrafficLightElement; +using autoware_perception_msgs::msg::TrafficLightGroup; +using autoware_perception_msgs::msg::TrafficLightGroupArray; class TrafficLightPublishPanel : public rviz_common::Panel { Q_OBJECT @@ -56,12 +56,12 @@ public Q_SLOTS: protected: void onTimer(); void createWallTimer(); - void onVectorMap(const HADMapBin::ConstSharedPtr msg); + void onVectorMap(const LaneletMapBin::ConstSharedPtr msg); rclcpp::Node::SharedPtr raw_node_; rclcpp::TimerBase::SharedPtr pub_timer_; - rclcpp::Publisher::SharedPtr pub_traffic_signals_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Publisher::SharedPtr pub_traffic_signals_; + rclcpp::Subscription::SharedPtr sub_vector_map_; QSpinBox * publishing_rate_input_; QComboBox * traffic_light_id_input_; @@ -74,7 +74,7 @@ public Q_SLOTS: QPushButton * publish_button_; QTableWidget * traffic_table_; - TrafficSignalArray extra_traffic_signals_; + TrafficLightGroupArray extra_traffic_signals_; bool enable_publish_{false}; std::set traffic_light_ids_; diff --git a/common/tier4_vehicle_rviz_plugin/README.md b/common/tier4_vehicle_rviz_plugin/README.md index 09576ac327bec..9560963b1f5c0 100644 --- a/common/tier4_vehicle_rviz_plugin/README.md +++ b/common/tier4_vehicle_rviz_plugin/README.md @@ -11,12 +11,12 @@ This plugin provides a visual and easy-to-understand display of vehicle speed, t ### Input -| Name | Type | Description | -| --------------------------------- | ------------------------------------------------------- | ---------------------------------- | -| `/vehicle/status/velocity_status` | `autoware_auto_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | -| `/control/turn_signal_cmd` | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | -| `/vehicle/status/steering_status` | `autoware_auto_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | -| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | +| Name | Type | Description | +| --------------------------------- | -------------------------------------------------- | ---------------------------------- | +| `/vehicle/status/velocity_status` | `autoware_vehicle_msgs::msg::VelocityReport` | The topic is vehicle twist | +| `/control/turn_signal_cmd` | `autoware_vehicle_msgs::msg::TurnIndicatorsReport` | The topic is status of turn signal | +| `/vehicle/status/steering_status` | `autoware_vehicle_msgs::msg::SteeringReport` | The topic is status of steering | +| `/localization/acceleration` | `geometry_msgs::msg::AccelWithCovarianceStamped` | The topic is the acceleration | ## Parameter diff --git a/common/tier4_vehicle_rviz_plugin/package.xml b/common/tier4_vehicle_rviz_plugin/package.xml index f6c131fdc99f3..922ebd2c5e318 100644 --- a/common/tier4_vehicle_rviz_plugin/package.xml +++ b/common/tier4_vehicle_rviz_plugin/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp index 5d3684d0351c6..5a9f7e602efc7 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.cpp @@ -164,7 +164,7 @@ void ConsoleMeterDisplay::update(float wall_dt, float ros_dt) } void ConsoleMeterDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp index cba0fa16b50fe..8f1a62fc35361 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/console_meter.hpp @@ -28,13 +28,13 @@ #include #include -#include +#include #endif namespace rviz_plugins { class ConsoleMeterDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -52,7 +52,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; rviz_common::properties::IntProperty * property_left_; @@ -86,7 +86,7 @@ private Q_SLOTS: Arc outer_arc_; std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp index 60a81e45c9c29..ffc2484c3673a 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.cpp @@ -167,7 +167,7 @@ void SteeringAngleDisplay::update(float wall_dt, float ros_dt) } void SteeringAngleDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp index d56b5a8d742b9..867a4c6110bb0 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/steering_angle.hpp @@ -26,13 +26,13 @@ #include #include -#include +#include #endif namespace rviz_plugins { class SteeringAngleDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -50,7 +50,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::ColorProperty * property_text_color_; @@ -65,7 +65,7 @@ private Q_SLOTS: private: std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp index b9ff54ef44ecd..ec16b693a5974 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.cpp @@ -84,7 +84,7 @@ void TurnSignalDisplay::onDisable() } void TurnSignalDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; @@ -123,19 +123,19 @@ void TurnSignalDisplay::update(float wall_dt, float ros_dt) // turn signal color QColor white_color(Qt::white); white_color.setAlpha(255); - if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { + if (signal_type == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); painter.drawPolygon(left_arrow_polygon_, 7); painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(right_arrow_polygon_, 7); - } else if (signal_type == autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { + } else if (signal_type == autoware_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { painter.setPen(QPen(white_color, static_cast(2), Qt::DotLine)); painter.drawPolygon(right_arrow_polygon_, 7); painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(left_arrow_polygon_, 7); - } else if (signal_type == autoware_auto_vehicle_msgs::msg::HazardLightsReport::ENABLE) { + } else if (signal_type == autoware_vehicle_msgs::msg::HazardLightsReport::ENABLE) { painter.setBrush(QBrush(Qt::white, Qt::SolidPattern)); painter.setPen(QPen(white_color, static_cast(2), Qt::SolidLine)); painter.drawPolygon(right_arrow_polygon_, 7); diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp index 06a1d2e5f0d54..eb64edd2d595e 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/turn_signal.hpp @@ -24,14 +24,14 @@ #include #include -#include -#include +#include +#include #endif namespace rviz_plugins { class TurnSignalDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -49,7 +49,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr msg_ptr) override; jsk_rviz_plugins::OverlayObject::Ptr overlay_; rviz_common::properties::IntProperty * property_left_; rviz_common::properties::IntProperty * property_top_; @@ -62,7 +62,7 @@ private Q_SLOTS: QPointF left_arrow_polygon_[7]; std::mutex mutex_; - autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; + autoware_vehicle_msgs::msg::TurnIndicatorsReport::ConstSharedPtr last_msg_ptr_; }; } // namespace rviz_plugins diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp index b5db539f437cb..ccf84d4e64895 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.cpp @@ -102,7 +102,7 @@ void VelocityHistoryDisplay::reset() } bool VelocityHistoryDisplay::validateFloats( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr) { if (!rviz_common::validateFloats(msg_ptr->longitudinal_velocity)) { return false; @@ -120,7 +120,7 @@ void VelocityHistoryDisplay::update(float wall_dt, float ros_dt) } void VelocityHistoryDisplay::processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) { if (!isEnabled()) { return; diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp index 9b5819df98bef..96345205289e6 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/velocity_history.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include @@ -36,7 +36,7 @@ namespace rviz_plugins { class VelocityHistoryDisplay -: public rviz_common::RosTopicDisplay +: public rviz_common::RosTopicDisplay { Q_OBJECT @@ -53,7 +53,7 @@ private Q_SLOTS: protected: void update(float wall_dt, float ros_dt) override; void processMessage( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; + const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg_ptr) override; std::unique_ptr setColorDependsOnVelocity( const double vel_max, const double cmd_vel); std::unique_ptr gradation( @@ -67,11 +67,9 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_vel_max_; private: - std::deque< - std::tuple> + std::deque> histories_; - bool validateFloats( - const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); + bool validateFloats(const autoware_vehicle_msgs::msg::VelocityReport::ConstSharedPtr & msg_ptr); std::mutex mutex_; }; diff --git a/common/traffic_light_recognition_marker_publisher/Readme.md b/common/traffic_light_recognition_marker_publisher/Readme.md index 6a51499c4e5da..d87fc33aefd33 100644 --- a/common/traffic_light_recognition_marker_publisher/Readme.md +++ b/common/traffic_light_recognition_marker_publisher/Readme.md @@ -12,10 +12,10 @@ This node publishes a marker array for visualizing traffic signal recognition re ### Input -| Name | Type | Description | -| ------------------------------------------------------- | -------------------------------------------------------- | ------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Vector map for getting traffic signal information | -| `/perception/traffic_light_recognition/traffic_signals` | `autoware_auto_perception_msgs::msg::TrafficSignalArray` | The result of traffic signal recognition | +| Name | Type | Description | +| ------------------------------------------------------- | ------------------------------------------------------- | ------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Vector map for getting traffic signal information | +| `/perception/traffic_light_recognition/traffic_signals` | `autoware_perception_msgs::msg::TrafficLightGroupArray` | The result of traffic signal recognition | ### Output diff --git a/common/traffic_light_recognition_marker_publisher/package.xml b/common/traffic_light_recognition_marker_publisher/package.xml index d12022c01684e..2b65c5b868313 100644 --- a/common/traffic_light_recognition_marker_publisher/package.xml +++ b/common/traffic_light_recognition_marker_publisher/package.xml @@ -12,8 +12,8 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs - autoware_auto_perception_msgs + autoware_map_msgs + autoware_perception_msgs geometry_msgs lanelet2_extension rclcpp diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp index 6076d3fa32f26..5d887a2296137 100644 --- a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.cpp @@ -26,17 +26,17 @@ TrafficLightRecognitionMarkerPublisher::TrafficLightRecognitionMarkerPublisher( { using std::placeholders::_1; - sub_map_ptr_ = this->create_subscription( + sub_map_ptr_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&TrafficLightRecognitionMarkerPublisher::onMap, this, _1)); - sub_tlr_ptr_ = this->create_subscription( + sub_tlr_ptr_ = this->create_subscription( "~/input/traffic_signals", rclcpp::QoS{1}, std::bind(&TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray, this, _1)); pub_marker_ptr_ = this->create_publisher("~/output/marker", rclcpp::QoS{1}); } -void TrafficLightRecognitionMarkerPublisher::onMap(const HADMapBin::ConstSharedPtr msg_ptr) +void TrafficLightRecognitionMarkerPublisher::onMap(const LaneletMapBin::ConstSharedPtr msg_ptr) { is_map_ready_ = false; lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); @@ -70,10 +70,10 @@ void TrafficLightRecognitionMarkerPublisher::onTrafficSignalArray( } MarkerArray markers; marker_id = 0; - for (const auto & tl : msg_ptr->signals) { - if (tl_position_map_.count(tl.map_primitive_id) == 0) continue; - auto tl_position = tl_position_map_[tl.map_primitive_id]; - for (const auto tl_light : tl.lights) { + for (const auto & tl : msg_ptr->traffic_light_groups) { + if (tl_position_map_.count(tl.traffic_light_group_id) == 0) continue; + auto tl_position = tl_position_map_[tl.traffic_light_group_id]; + for (const auto tl_light : tl.elements) { const auto marker = getTrafficLightMarker(tl_position, tl_light.color, tl_light.shape); markers.markers.emplace_back(marker); marker_id++; diff --git a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp index de9aea14b7e5a..c1cac302647a1 100644 --- a/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp +++ b/common/traffic_light_recognition_marker_publisher/src/traffic_light_recognition_marker_publisher.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -32,21 +32,21 @@ class TrafficLightRecognitionMarkerPublisher : public rclcpp::Node { public: - using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; - using TrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; - using TrafficLight = autoware_auto_perception_msgs::msg::TrafficLight; + using LaneletMapBin = autoware_map_msgs::msg::LaneletMapBin; + using TrafficSignalArray = autoware_perception_msgs::msg::TrafficLightGroupArray; + using TrafficLight = autoware_perception_msgs::msg::TrafficLightElement; using MarkerArray = visualization_msgs::msg::MarkerArray; using Pose = geometry_msgs::msg::Pose; explicit TrafficLightRecognitionMarkerPublisher(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_ptr_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_map_ptr_; + rclcpp::Subscription::SharedPtr sub_tlr_ptr_; rclcpp::Publisher::SharedPtr pub_marker_ptr_; - void onMap(const HADMapBin::ConstSharedPtr msg_ptr); + void onMap(const LaneletMapBin::ConstSharedPtr msg_ptr); void onTrafficSignalArray(const TrafficSignalArray::ConstSharedPtr msg_ptr); visualization_msgs::msg::Marker getTrafficLightMarker( const Pose & tl_pose, const uint8_t tl_color, const uint8_t tl_shape); diff --git a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp index 9c3acd4f45fa4..cf4ecdd9774b8 100644 --- a/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp +++ b/common/traffic_light_utils/include/traffic_light_utils/traffic_light_utils.hpp @@ -15,8 +15,8 @@ #ifndef TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ #define TRAFFIC_LIGHT_UTILS__TRAFFIC_LIGHT_UTILS_HPP_ -#include "autoware_perception_msgs/msg/traffic_signal.hpp" -#include "autoware_perception_msgs/msg/traffic_signal_element.hpp" +#include "autoware_perception_msgs/msg/traffic_light_element.hpp" +#include "autoware_perception_msgs/msg/traffic_light_group.hpp" #include "tier4_perception_msgs/msg/traffic_light.hpp" #include "tier4_perception_msgs/msg/traffic_light_element.hpp" #include "tier4_perception_msgs/msg/traffic_light_roi.hpp" @@ -50,7 +50,7 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c * @return True if a circle-shaped light with the specified color is found, false otherwise. */ bool hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_color); /** * @brief Checks if a traffic light state includes a light with the specified shape. @@ -62,7 +62,7 @@ bool hasTrafficLightCircleColor( * @return True if a light with the specified shape is found, false otherwise. */ bool hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape); /** * @brief Determines if a traffic signal indicates a stop for the given lanelet. @@ -78,7 +78,7 @@ bool hasTrafficLightShape( */ bool isTrafficSignalStop( const lanelet::ConstLanelet & lanelet, - const autoware_perception_msgs::msg::TrafficSignal & tl_state); + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state); tf2::Vector3 getTrafficLightTopLeft(const lanelet::ConstLineString3d & traffic_light); diff --git a/common/traffic_light_utils/src/traffic_light_utils.cpp b/common/traffic_light_utils/src/traffic_light_utils.cpp index 0bd3d85a9b71f..8aea884510ec2 100644 --- a/common/traffic_light_utils/src/traffic_light_utils.cpp +++ b/common/traffic_light_utils/src/traffic_light_utils.cpp @@ -52,11 +52,11 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficLight & signal, float c } bool hasTrafficLightCircleColor( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_color) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_color) { const auto it_lamp = std::find_if(tl_state.elements.begin(), tl_state.elements.end(), [&lamp_color](const auto & x) { - return x.shape == autoware_perception_msgs::msg::TrafficSignalElement::CIRCLE && + return x.shape == autoware_perception_msgs::msg::TrafficLightElement::CIRCLE && x.color == lamp_color; }); @@ -64,7 +64,7 @@ bool hasTrafficLightCircleColor( } bool hasTrafficLightShape( - const autoware_perception_msgs::msg::TrafficSignal & tl_state, const uint8_t & lamp_shape) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state, const uint8_t & lamp_shape) { const auto it_lamp = std::find_if( tl_state.elements.begin(), tl_state.elements.end(), @@ -75,10 +75,10 @@ bool hasTrafficLightShape( bool isTrafficSignalStop( const lanelet::ConstLanelet & lanelet, - const autoware_perception_msgs::msg::TrafficSignal & tl_state) + const autoware_perception_msgs::msg::TrafficLightGroup & tl_state) { if (hasTrafficLightCircleColor( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::GREEN)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::GREEN)) { return false; } @@ -90,18 +90,18 @@ bool isTrafficSignalStop( if ( turn_direction == "right" && hasTrafficLightShape( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::RIGHT_ARROW)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::RIGHT_ARROW)) { return false; } if ( turn_direction == "left" && hasTrafficLightShape( - tl_state, autoware_perception_msgs::msg::TrafficSignalElement::LEFT_ARROW)) { + tl_state, autoware_perception_msgs::msg::TrafficLightElement::LEFT_ARROW)) { return false; } if ( turn_direction == "straight" && - hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficSignalElement::UP_ARROW)) { + hasTrafficLightShape(tl_state, autoware_perception_msgs::msg::TrafficLightElement::UP_ARROW)) { return false; } 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_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/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/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_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_height_fitter/package.xml b/map/map_height_fitter/package.xml index 568c77f2509c6..c10e65cdfaab1 100644 --- a/map/map_height_fitter/package.xml +++ b/map/map_height_fitter/package.xml @@ -18,7 +18,6 @@ ament_cmake autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs geometry_msgs lanelet2_extension diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 0c99d33772aea..e01201f90f7e3 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -38,7 +38,7 @@ struct MapHeightFitter::Impl explicit Impl(rclcpp::Node * node); void on_pcd_map(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); - void on_vector_map(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void on_vector_map(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); bool get_partial_point_cloud_map(const Point & point); double get_ground_height(const Point & point) const; std::optional fit(const Point & position, const std::string & frame); @@ -59,7 +59,7 @@ struct MapHeightFitter::Impl // for fitting by vector_map_loader lanelet::LaneletMapPtr vector_map_; - rclcpp::Subscription::SharedPtr sub_vector_map_; + rclcpp::Subscription::SharedPtr sub_vector_map_; }; MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), node_(node) @@ -95,7 +95,7 @@ MapHeightFitter::Impl::Impl(rclcpp::Node * node) : tf2_listener_(tf2_buffer_), n } else if (fit_target_ == "vector_map") { const auto durable_qos = rclcpp::QoS(1).transient_local(); - sub_vector_map_ = node_->create_subscription( + sub_vector_map_ = node_->create_subscription( "~/vector_map", durable_qos, std::bind(&MapHeightFitter::Impl::on_vector_map, this, std::placeholders::_1)); @@ -163,7 +163,7 @@ bool MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point) } void MapHeightFitter::Impl::on_vector_map( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { vector_map_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, vector_map_); diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 8a31ecee50be5..21bd39303250f 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -130,7 +130,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature -lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. +lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. @@ -144,7 +144,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Published Topics -- ~output/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : Binary data of loaded Lanelet2 Map +- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map ### Parameters @@ -156,7 +156,7 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Feature -lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messages into visualization_msgs/MarkerArray. +lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray. ### How to Run @@ -164,7 +164,7 @@ lanelet2_map_visualization visualizes autoware_auto_mapping_msgs/HADMapBin messa ### Subscribed Topics -- ~input/lanelet2_map (autoware_auto_mapping_msgs/HADMapBin) : binary data of Lanelet2 Map +- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map ### Published Topics diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index 0adc7612e9f61..4e85ddec056c1 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -using autoware_auto_mapping_msgs::msg::HADMapBin; +using autoware_map_msgs::msg::LaneletMapBin; using tier4_map_msgs::msg::MapProjectorInfo; class Lanelet2MapLoaderNode : public rclcpp::Node @@ -38,7 +38,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node static lanelet::LaneletMapPtr load_map( const std::string & lanelet2_filename, const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static HADMapBin create_map_bin_msg( + static LaneletMapBin create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now); @@ -48,7 +48,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; + rclcpp::Publisher::SharedPtr pub_map_bin_; }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp index e5a5fe3e3a6fa..cb640e4dc83d5 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -29,12 +29,12 @@ class Lanelet2MapVisualizationNode : public rclcpp::Node explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); private: - rclcpp::Subscription::SharedPtr sub_map_bin_; + rclcpp::Subscription::SharedPtr sub_map_bin_; rclcpp::Publisher::SharedPtr pub_marker_; bool viz_lanelets_centerline_; - void onMapBin(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); }; #endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index a9b657f843447..4eaf8600dcab4 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -19,7 +19,6 @@ ament_cmake_auto autoware_cmake - autoware_auto_mapping_msgs autoware_map_msgs component_interface_specs component_interface_utils diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 617f3dd503ce0..b097e1809a385 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -86,7 +86,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( // create publisher and publish pub_map_bin_ = - create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); + create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); pub_map_bin_->publish(map_bin_msg); RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); } @@ -141,18 +141,18 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( return nullptr; } -HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg( +LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) { std::string format_version{}, map_version{}; 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; diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp index bdfbcf904cf36..c81236bec86c0 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp @@ -39,7 +39,7 @@ #include #include -#include +#include #include #include @@ -73,7 +73,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt viz_lanelets_centerline_ = true; - sub_map_bin_ = this->create_subscription( + sub_map_bin_ = this->create_subscription( "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); @@ -82,7 +82,7 @@ Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOpt } void Lanelet2MapVisualizationNode::onMapBin( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); diff --git a/map/map_tf_generator/README.md b/map/map_tf_generator/README.md index 643f4233c06f0..eef36c34750ca 100644 --- a/map/map_tf_generator/README.md +++ b/map/map_tf_generator/README.md @@ -25,9 +25,9 @@ The following are the supported methods to calculate the position of the `viewer #### vector_map_tf_generator -| Name | Type | Description | -| ----------------- | -------------------------------------------- | ------------------------------------------------------------- | -| `/map/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Subscribe vector map to calculate position of `viewer` frames | +| Name | Type | Description | +| ----------------- | --------------------------------------- | ------------------------------------------------------------- | +| `/map/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | Subscribe vector map to calculate position of `viewer` frames | ### Output diff --git a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp index 20ca1c037a578..f242254a456a1 100644 --- a/map/map_tf_generator/src/vector_map_tf_generator_node.cpp +++ b/map/map_tf_generator/src/vector_map_tf_generator_node.cpp @@ -15,7 +15,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ class VectorMapTFGeneratorNode : public rclcpp::Node map_frame_ = declare_parameter("map_frame"); viewer_frame_ = declare_parameter("viewer_frame"); - sub_ = create_subscription( + sub_ = create_subscription( "vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapTFGeneratorNode::onVectorMap, this, std::placeholders::_1)); @@ -44,12 +44,12 @@ class VectorMapTFGeneratorNode : public rclcpp::Node private: std::string map_frame_; std::string viewer_frame_; - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; std::shared_ptr static_broadcaster_; std::shared_ptr lanelet_map_ptr_; - void onVectorMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg) + void onVectorMap(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr_); diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml index 0d1ccc284f888..231a1845bb0ea 100644 --- a/perception/bytetrack/package.xml +++ b/perception/bytetrack/package.xml @@ -15,7 +15,7 @@ cudnn_cmake_module tensorrt_cmake_module - autoware_auto_perception_msgs + autoware_perception_msgs cuda_utils cv_bridge eigen diff --git a/perception/bytetrack/src/bytetrack_node.cpp b/perception/bytetrack/src/bytetrack_node.cpp index eb5e73312a890..64adc83a51abd 100644 --- a/perception/bytetrack/src/bytetrack_node.cpp +++ b/perception/bytetrack/src/bytetrack_node.cpp @@ -17,7 +17,7 @@ #include #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" +#include "autoware_perception_msgs/msg/object_classification.hpp" #include @@ -62,7 +62,7 @@ void ByteTrackNode::on_connect() void ByteTrackNode::on_rect( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr msg) { - using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + using Label = autoware_perception_msgs::msg::ObjectClassification; tier4_perception_msgs::msg::DetectedObjectsWithFeature out_objects; tier4_perception_msgs::msg::DynamicObjectArray out_objects_uuid; @@ -97,7 +97,7 @@ void ByteTrackNode::on_rect( object.feature.roi.height = static_cast(output_height); object.object.existence_probability = tracked_object.score; object.object.classification.emplace_back( - autoware_auto_perception_msgs::build