From 72989d9a8640e4d0993e030bf700a0e5d669ce40 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 4 Jun 2024 23:47:57 +0900 Subject: [PATCH 01/14] feat!: replace autoware_auto_msgs with autoware_msgs for common modules (#7239) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan --- .../CMakeLists.txt | 2 +- .../autoware_overlay_rviz_plugin/README.md | 12 +- .../include/gear_display.hpp | 4 +- .../include/signal_display.hpp | 26 +- .../include/speed_display.hpp | 4 +- .../include/speed_limit_display.hpp | 4 +- .../include/steering_wheel_display.hpp | 5 +- .../include/traffic_display.hpp | 9 +- .../include/turn_signals_display.hpp | 8 +- .../autoware_overlay_rviz_plugin/package.xml | 2 +- .../src/gear_display.cpp | 17 +- .../src/signal_display.cpp | 98 +-- .../src/speed_display.cpp | 4 +- .../src/speed_limit_display.cpp | 2 +- .../src/steering_wheel_display.cpp | 2 +- .../src/traffic_display.cpp | 6 +- .../src/turn_signals_display.cpp | 12 +- .../autoware_perception_rviz_plugin/README.md | 20 +- .../detected_objects_display.hpp | 6 +- .../object_polygon_detail.hpp | 64 +- .../object_polygon_display_base.hpp | 14 +- .../predicted_objects_display.hpp | 6 +- .../tracked_objects_display.hpp | 8 +- .../package.xml | 2 +- .../plugins_description.xml | 6 +- .../detected_objects_display.cpp | 2 +- .../object_polygon_detail.cpp | 36 +- .../predicted_objects_display.cpp | 2 +- .../tracked_objects_display.cpp | 2 +- .../component_interface_specs/perception.hpp | 4 +- .../component_interface_specs/planning.hpp | 4 +- .../component_interface_specs/vehicle.hpp | 16 +- common/component_interface_specs/package.xml | 5 +- .../test/test_fake_test_node.cpp | 3 +- common/goal_distance_calculator/Readme.md | 8 +- .../goal_distance_calculator.hpp | 4 +- .../goal_distance_calculator_node.hpp | 8 +- common/goal_distance_calculator/package.xml | 2 +- .../src/goal_distance_calculator.cpp | 2 +- .../src/goal_distance_calculator_node.cpp | 4 +- .../test_spline_interpolation_points_2d.cpp | 2 +- common/motion_utils/README.md | 4 +- common/motion_utils/docs/vehicle/vehicle.md | 8 +- .../motion_utils/resample/resample.hpp | 39 +- .../motion_utils/trajectory/conversion.hpp | 51 +- .../motion_utils/trajectory/interpolation.hpp | 14 +- .../trajectory/path_with_lane_id.hpp | 16 +- .../trajectory/tmp_conversion.hpp | 48 ++ .../motion_utils/trajectory/trajectory.hpp | 617 +++++++++--------- .../vehicle/vehicle_state_checker.hpp | 4 +- common/motion_utils/package.xml | 5 +- .../src/factor/velocity_factor_interface.cpp | 22 +- common/motion_utils/src/resample/resample.cpp | 42 +- .../src/trajectory/conversion.cpp | 24 +- .../src/trajectory/interpolation.cpp | 8 +- .../src/trajectory/path_with_lane_id.cpp | 16 +- .../src/trajectory/trajectory.cpp | 601 ++++++++--------- .../test/src/resample/test_resample.cpp | 100 +-- .../benchmark_calcLateralOffset.cpp | 2 +- .../src/trajectory/test_interpolation.cpp | 20 +- .../src/trajectory/test_path_with_lane_id.cpp | 6 +- .../test/src/trajectory/test_trajectory.cpp | 22 +- .../test_vehicle_state_checker_helper.hpp | 8 +- .../object_recognition_utils/conversion.hpp | 12 +- .../object_recognition_utils/geometry.hpp | 15 +- .../object_classification.hpp | 4 +- .../predicted_path_utils.hpp | 16 +- common/object_recognition_utils/package.xml | 2 +- .../src/conversion.cpp | 10 +- .../src/predicted_path_utils.cpp | 15 +- .../test/src/test_conversion.cpp | 12 +- .../test/src/test_geometry.cpp | 6 +- .../test/src/test_matching.cpp | 68 +- .../test/src/test_object_classification.cpp | 46 +- .../test/src/test_predicted_path_utils.cpp | 2 +- common/path_distance_calculator/Readme.md | 8 +- common/path_distance_calculator/package.xml | 2 +- .../src/path_distance_calculator.cpp | 4 +- .../src/path_distance_calculator.hpp | 6 +- .../geometry/boost_polygon_utils.hpp | 18 +- .../geometry/geometry.hpp | 40 +- common/tier4_autoware_utils/package.xml | 7 +- .../src/geometry/boost_polygon_utils.cpp | 22 +- .../src/geometry/test_boost_polygon_utils.cpp | 28 +- .../test/src/geometry/test_geometry.cpp | 20 +- .../test_path_with_lane_id_geometry.cpp | 10 +- .../src/tools/car_pose.hpp | 4 +- .../src/tools/interactive_object.hpp | 4 +- .../src/tools/pedestrian_pose.hpp | 4 +- .../src/tools/unknown_pose.hpp | 4 +- common/tier4_planning_rviz_plugin/README.md | 10 +- .../path/display.hpp | 16 +- .../path/display_base.hpp | 2 +- common/tier4_planning_rviz_plugin/package.xml | 1 - .../plugins/plugin_description.xml | 6 +- .../src/path/display.cpp | 5 +- common/tier4_state_rviz_plugin/README.md | 2 +- common/tier4_state_rviz_plugin/package.xml | 3 +- .../src/include/autoware_state_panel.hpp | 6 +- common/tier4_system_rviz_plugin/README.md | 6 +- common/tier4_system_rviz_plugin/package.xml | 2 +- .../src/mrm_summary_overlay_display.cpp | 8 +- .../src/mrm_summary_overlay_display.hpp | 8 +- .../tier4_traffic_light_rviz_plugin/README.md | 6 +- .../package.xml | 2 +- .../src/traffic_light_publish_panel.cpp | 98 +-- .../src/traffic_light_publish_panel.hpp | 20 +- common/tier4_vehicle_rviz_plugin/README.md | 12 +- common/tier4_vehicle_rviz_plugin/package.xml | 2 +- .../src/tools/console_meter.cpp | 2 +- .../src/tools/console_meter.hpp | 8 +- .../src/tools/steering_angle.cpp | 2 +- .../src/tools/steering_angle.hpp | 8 +- .../src/tools/turn_signal.cpp | 8 +- .../src/tools/turn_signal.hpp | 10 +- .../src/tools/velocity_history.cpp | 4 +- .../src/tools/velocity_history.hpp | 12 +- .../Readme.md | 8 +- .../package.xml | 4 +- ...fic_light_recognition_marker_publisher.cpp | 14 +- ...fic_light_recognition_marker_publisher.hpp | 16 +- .../traffic_light_utils.hpp | 10 +- .../src/traffic_light_utils.cpp | 16 +- 123 files changed, 1419 insertions(+), 1431 deletions(-) create mode 100644 common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp 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/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp new file mode 100644 index 0000000000000..1eb0a43976daa --- /dev/null +++ b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp @@ -0,0 +1,48 @@ +// Copyright 2021 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ +#define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ + +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_planning_msgs/msg/trajectory_point.hpp" + +#include + +namespace motion_utils +{ +/** + * @brief Convert std::vector to + * autoware_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory); + +/** + * @brief Convert autoware_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_planning_msgs::msg::Trajectory & trajectory); + +} // namespace motion_utils + +#endif // MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ diff --git a/common/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..a7e6737cd2d42 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; 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; } From f2ee838f50d372e1a8ae780c82100017dc694325 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 5 Jun 2024 00:03:50 +0900 Subject: [PATCH 02/14] feat!: replace autoware_auto_msgs with autoware_msgs for sensing modules (#7247) Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan --- sensing/pointcloud_preprocessor/docs/vector-map-filter.md | 8 ++++---- .../docs/vector-map-inside-area-filter.md | 8 ++++---- .../concatenate_data/concatenate_pointclouds.hpp | 4 ++-- .../vector_map_filter/lanelet2_map_filter_nodelet.hpp | 6 +++--- .../vector_map_filter/vector_map_inside_area_filter.hpp | 4 ++-- .../src/vector_map_filter/lanelet2_map_filter_nodelet.cpp | 4 ++-- .../vector_map_filter/vector_map_inside_area_filter.cpp | 4 ++-- sensing/vehicle_velocity_converter/README.md | 8 ++++---- .../vehicle_velocity_converter.hpp | 7 +++---- sensing/vehicle_velocity_converter/package.xml | 2 +- .../src/vehicle_velocity_converter.cpp | 4 ++-- 11 files changed, 29 insertions(+), 30 deletions(-) diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md index fedd3c081cdeb..c38a4c719fcf3 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-filter.md +++ b/sensing/pointcloud_preprocessor/docs/vector-map-filter.md @@ -10,10 +10,10 @@ The `vector_map_filter` is a node that removes points on the outside of lane by ### Input -| Name | Type | Description | -| -------------------- | -------------------------------------------- | ---------------- | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | -| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map | +| Name | Type | Description | +| -------------------- | --------------------------------------- | ---------------- | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map | ### Output diff --git a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md index 4bafac7872e78..bd2f7d98919d5 100644 --- a/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md +++ b/sensing/pointcloud_preprocessor/docs/vector-map-inside-area-filter.md @@ -19,10 +19,10 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, so please ### Input -| Name | Type | Description | -| -------------------- | -------------------------------------------- | ------------------------------------ | -| `~/input` | `sensor_msgs::msg::PointCloud2` | input points | -| `~/input/vector_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map used for filtering points | +| Name | Type | Description | +| -------------------- | --------------------------------------- | ------------------------------------ | +| `~/input` | `sensor_msgs::msg::PointCloud2` | input points | +| `~/input/vector_map` | `autoware_map_msgs::msg::LaneletMapBin` | vector map used for filtering points | ### Output diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp index f46d6595b2a76..4e183a9816c2e 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_pointclouds.hpp @@ -68,7 +68,7 @@ #include #include -#include +#include #include #include #include @@ -127,7 +127,7 @@ class PointCloudConcatenationComponent : public rclcpp::Node /** \brief A vector of subscriber. */ std::vector::SharedPtr> filters_; - rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_twist_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index ed0d753a68ecc..42078922d39b9 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -59,7 +59,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; rclcpp::Subscription::SharedPtr pointcloud_sub_; rclcpp::Publisher::SharedPtr filtered_pointcloud_pub_; @@ -71,7 +71,7 @@ class Lanelet2MapFilterComponent : public rclcpp::Node void pointcloudCallback(const PointCloud2ConstPtr msg); - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); bool transformPointCloud( const std::string & in_target_frame, const PointCloud2ConstPtr & in_cloud_ptr, diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp index 74209c1a22e4c..894768d385438 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp @@ -36,10 +36,10 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; - rclcpp::Subscription::SharedPtr map_sub_; + rclcpp::Subscription::SharedPtr map_sub_; lanelet::ConstPolygons3d polygon_lanelets_; - void mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg); + void mapCallback(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); // parameter std::string polygon_type_; diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp index 502c85d1746e8..8f7cb2abec6a4 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/lanelet2_map_filter_nodelet.cpp @@ -51,7 +51,7 @@ Lanelet2MapFilterComponent::Lanelet2MapFilterComponent(const rclcpp::NodeOptions // Set subscriber { - map_sub_ = this->create_subscription( + map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&Lanelet2MapFilterComponent::mapCallback, this, _1)); pointcloud_sub_ = this->create_subscription( @@ -259,7 +259,7 @@ void Lanelet2MapFilterComponent::pointcloudCallback(const PointCloud2ConstPtr cl } void Lanelet2MapFilterComponent::mapCallback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr map_msg) { lanelet_map_ptr_ = std::make_shared(); lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_); diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp index 5b8755714b375..8fcf15326829f 100644 --- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp +++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp @@ -71,7 +71,7 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent( using std::placeholders::_1; // Set subscriber - map_sub_ = this->create_subscription( + map_sub_ = this->create_subscription( "input/vector_map", rclcpp::QoS{1}.transient_local(), std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1)); } @@ -107,7 +107,7 @@ void VectorMapInsideAreaFilterComponent::filter( } void VectorMapInsideAreaFilterComponent::mapCallback( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr map_msg) + const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr map_msg) { tf_input_frame_ = map_msg->header.frame_id; diff --git a/sensing/vehicle_velocity_converter/README.md b/sensing/vehicle_velocity_converter/README.md index 92881f9321f28..3c7292f3fcdc4 100644 --- a/sensing/vehicle_velocity_converter/README.md +++ b/sensing/vehicle_velocity_converter/README.md @@ -2,15 +2,15 @@ ## Purpose -This package converts autoware_auto_vehicle_msgs::msg::VehicleReport message to geometry_msgs::msg::TwistWithCovarianceStamped for gyro odometer node. +This package converts autoware_vehicle_msgs::msg::VehicleReport message to geometry_msgs::msg::TwistWithCovarianceStamped for gyro odometer node. ## Inputs / Outputs ### Input -| Name | Type | Description | -| ----------------- | ------------------------------------------------ | ---------------- | -| `velocity_status` | `autoware_auto_vehicle_msgs::msg::VehicleReport` | vehicle velocity | +| Name | Type | Description | +| ----------------- | ------------------------------------------- | ---------------- | +| `velocity_status` | `autoware_vehicle_msgs::msg::VehicleReport` | vehicle velocity | ### Output diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index 660ad330f07f3..4a1a66b842892 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -17,7 +17,7 @@ #include -#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_vehicle_msgs/msg/velocity_report.hpp" #include #include @@ -32,10 +32,9 @@ class VehicleVelocityConverter : public rclcpp::Node ~VehicleVelocityConverter() = default; private: - void callbackVelocityReport(const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg); + void callbackVelocityReport(const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg); - rclcpp::Subscription::SharedPtr - vehicle_report_sub_; + rclcpp::Subscription::SharedPtr vehicle_report_sub_; rclcpp::Publisher::SharedPtr twist_with_covariance_pub_; diff --git a/sensing/vehicle_velocity_converter/package.xml b/sensing/vehicle_velocity_converter/package.xml index c44c55bcd40eb..39780deaccc28 100644 --- a/sensing/vehicle_velocity_converter/package.xml +++ b/sensing/vehicle_velocity_converter/package.xml @@ -10,7 +10,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_vehicle_msgs + autoware_vehicle_msgs geometry_msgs rclcpp ament_lint_auto diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 0f4a22bbc9730..360ec4cae58d5 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -22,7 +22,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_co frame_id_ = declare_parameter("frame_id"); speed_scale_factor_ = declare_parameter("speed_scale_factor"); - vehicle_report_sub_ = create_subscription( + vehicle_report_sub_ = create_subscription( "velocity_status", rclcpp::QoS{100}, std::bind(&VehicleVelocityConverter::callbackVelocityReport, this, std::placeholders::_1)); @@ -31,7 +31,7 @@ VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_co } void VehicleVelocityConverter::callbackVelocityReport( - const autoware_auto_vehicle_msgs::msg::VelocityReport::SharedPtr msg) + const autoware_vehicle_msgs::msg::VelocityReport::SharedPtr msg) { if (msg->header.frame_id != frame_id_) { RCLCPP_WARN(get_logger(), "frame_id is not base_link."); From 09b68993e3bb20b1e2fb60bd5e3cd58c019ab477 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 5 Jun 2024 00:10:54 +0900 Subject: [PATCH 03/14] feat!: replace autoware_auto_msgs with autoware_msgs for perception modules (#7245) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Ryohsuke Mitsudome Co-authored-by: Cynthia Liu Co-authored-by: NorahXiong Co-authored-by: beginningfan Co-authored-by: M. Fatih Cırıt Co-authored-by: Yukihito Saito --- perception/bytetrack/package.xml | 2 +- perception/bytetrack/src/bytetrack_node.cpp | 6 +-- perception/cluster_merger/README.md | 6 +-- .../README.md | 16 +++--- .../node.hpp | 16 +++--- .../package.xml | 3 +- .../src/node.cpp | 40 ++++++++------- .../detected_object_feature_remover/README.md | 6 +-- .../detected_object_feature_remover.hpp | 4 +- .../package.xml | 2 +- .../object_lanelet_filter.hpp | 29 ++++++----- .../object_position_filter.hpp | 12 ++--- .../debugger.hpp | 12 ++--- .../obstacle_pointcloud_based_validator.hpp | 32 ++++++------ .../occupancy_grid_based_validator.hpp | 16 +++--- .../utils/utils.hpp | 10 ++-- .../object-lanelet-filter.md | 14 +++--- .../object-position-filter.md | 14 +++--- .../obstacle-pointcloud-based-validator.md | 14 +++--- .../occupancy-grid-based-validator.md | 14 +++--- .../detected_object_validation/package.xml | 4 +- .../src/object_lanelet_filter.cpp | 32 ++++++------ .../src/object_position_filter.cpp | 10 ++-- .../obstacle_pointcloud_based_validator.cpp | 24 ++++----- .../src/occupancy_grid_based_validator.cpp | 16 +++--- .../detected_object_validation/src/utils.cpp | 4 +- .../test/test_utils.cpp | 4 +- perception/detection_by_tracker/README.md | 6 +-- .../include/detection_by_tracker/debugger.hpp | 42 +++++++--------- .../detection_by_tracker_core.hpp | 24 ++++----- .../src/detection_by_tracker_core.cpp | 50 +++++++++---------- perception/detection_by_tracker/src/utils.cpp | 4 +- perception/elevation_map_loader/README.md | 2 +- .../elevation_map_loader_node.hpp | 6 +-- perception/elevation_map_loader/package.xml | 1 - .../src/elevation_map_loader_node.cpp | 4 +- perception/euclidean_cluster/lib/utils.cpp | 10 ++-- .../voxel_grid_based_euclidean_cluster.cpp | 4 +- perception/euclidean_cluster/package.xml | 2 +- .../docs/pointpainting-fusion.md | 10 ++-- .../docs/roi-detected-object-fusion.md | 14 +++--- .../fusion_node.hpp | 8 +-- .../pointpainting_fusion/node.hpp | 2 +- .../roi_detected_object_fusion/node.hpp | 2 +- .../utils/geometry.hpp | 4 +- .../utils/utils.hpp | 2 +- .../image_projection_based_fusion/package.xml | 2 +- .../src/pointpainting_fusion/node.cpp | 30 +++++------ .../src/roi_cluster_fusion/node.cpp | 4 +- .../src/roi_pointcloud_fusion/node.cpp | 2 +- .../src/utils/utils.cpp | 4 +- .../test/test_geometry.cpp | 6 +-- .../CMakeLists.txt | 2 + .../package.xml | 2 +- .../src/cluster2d.cpp | 10 ++-- .../src/debugger.cpp | 4 +- .../cluster2d.hpp | 1 - .../lidar_apollo_segmentation_tvm/package.xml | 2 +- .../src/cluster2d.cpp | 8 +-- perception/lidar_centerpoint/README.md | 10 ++-- .../detection_class_remapper.hpp | 10 ++-- .../include/lidar_centerpoint/node.hpp | 10 ++-- .../postprocess/non_maximum_suppression.hpp | 4 +- .../include/lidar_centerpoint/ros_utils.hpp | 10 ++-- .../lib/detection_class_remapper.cpp | 2 +- .../lidar_centerpoint/lib/ros_utils.cpp | 10 ++-- perception/lidar_centerpoint/package.xml | 2 +- perception/lidar_centerpoint/src/node.cpp | 8 +-- .../test/test_detection_class_remapper.cpp | 18 +++---- .../lidar_centerpoint/test/test_nms.cpp | 16 +++--- .../lidar_centerpoint/test/test_ros_utils.cpp | 27 +++++----- perception/lidar_centerpoint_tvm/README.md | 10 ++-- .../include/lidar_centerpoint_tvm/node.hpp | 10 ++-- .../lidar_centerpoint_tvm/ros_utils.hpp | 10 ++-- .../single_inference_node.hpp | 12 ++--- .../lidar_centerpoint_tvm/lib/ros_utils.cpp | 10 ++-- perception/lidar_centerpoint_tvm/package.xml | 2 +- perception/lidar_centerpoint_tvm/src/node.cpp | 6 +-- .../src/single_inference_node.cpp | 8 +-- perception/map_based_prediction/README.md | 24 ++++----- .../map_based_prediction_node.hpp | 49 +++++++++--------- .../map_based_prediction/path_generator.hpp | 20 ++++---- perception/map_based_prediction/package.xml | 1 - .../src/map_based_prediction_node.cpp | 35 +++++++------ .../test_path_generator.cpp | 16 +++--- perception/multi_object_tracker/README.md | 20 ++++---- .../data_association/data_association.hpp | 4 +- .../debugger/debug_object.hpp | 6 +-- .../debugger/debugger.hpp | 10 ++-- .../multi_object_tracker_core.hpp | 10 ++-- .../processor/input_manager.hpp | 6 +-- .../processor/processor.hpp | 14 +++--- .../tracker/model/bicycle_tracker.hpp | 16 +++--- .../tracker/model/big_vehicle_tracker.hpp | 16 +++--- .../model/multiple_vehicle_tracker.hpp | 6 +-- .../tracker/model/normal_vehicle_tracker.hpp | 16 +++--- .../tracker/model/pass_through_tracker.hpp | 10 ++-- .../model/pedestrian_and_bicycle_tracker.hpp | 6 +-- .../tracker/model/pedestrian_tracker.hpp | 16 +++--- .../tracker/model/tracker_base.hpp | 21 ++++---- .../tracker/model/unknown_tracker.hpp | 16 +++--- .../multi_object_tracker/utils/utils.hpp | 29 ++++++----- perception/multi_object_tracker/package.xml | 2 +- .../src/data_association/data_association.cpp | 6 +-- .../src/debugger/debug_object.cpp | 6 +-- .../src/debugger/debugger.cpp | 6 +-- .../src/multi_object_tracker_core.cpp | 2 +- .../src/processor/input_manager.cpp | 2 +- .../src/processor/processor.cpp | 23 ++++----- .../src/tracker/model/bicycle_tracker.cpp | 30 ++++++----- .../src/tracker/model/big_vehicle_tracker.cpp | 37 +++++++------- .../model/multiple_vehicle_tracker.cpp | 10 ++-- .../tracker/model/normal_vehicle_tracker.cpp | 37 +++++++------- .../tracker/model/pass_through_tracker.cpp | 6 +-- .../model/pedestrian_and_bicycle_tracker.cpp | 10 ++-- .../src/tracker/model/pedestrian_tracker.cpp | 36 ++++++------- .../src/tracker/model/tracker_base.cpp | 10 ++-- .../src/tracker/model/unknown_tracker.cpp | 17 +++---- perception/object_merger/README.md | 34 ++++++------- .../data_association/data_association.hpp | 6 +-- .../include/object_merger/node.hpp | 16 +++--- .../include/object_merger/utils/utils.hpp | 4 +- perception/object_merger/package.xml | 2 +- .../data_association/data_association.cpp | 8 +-- .../src/object_association_merger/node.cpp | 18 +++---- perception/object_range_splitter/README.md | 14 +++--- .../include/object_range_splitter/node.hpp | 10 ++-- perception/object_range_splitter/package.xml | 2 +- perception/object_range_splitter/src/node.cpp | 16 +++--- perception/object_velocity_splitter/README.md | 8 +-- .../object_velocity_splitter_node.hpp | 6 +-- .../object_velocity_splitter/package.xml | 2 +- .../object_velocity_splitter_node.cpp | 4 +- .../package.xml | 2 +- .../README.md | 8 +-- ...dar_crossing_objects_noise_filter_node.hpp | 6 +-- .../package.xml | 2 +- ...dar_crossing_objects_noise_filter_node.cpp | 4 +- ...radar_crossing_objects_filter_is_noise.cpp | 4 +- .../radar_fusion_to_detected_object/README.md | 8 +-- .../radar_fusion_to_detected_object.hpp | 6 +-- .../radar_fusion_to_detected_object_node.hpp | 6 +-- .../package.xml | 2 +- .../src/radar_fusion_to_detected_object.cpp | 4 +- ..._object_fusion_to_detected_object_node.cpp | 4 +- perception/radar_object_clustering/README.md | 6 +-- .../radar_object_clustering_node.hpp | 6 +-- .../radar_object_clustering/package.xml | 2 +- .../radar_object_clustering_node.cpp | 8 +-- perception/radar_object_tracker/README.md | 14 +++--- .../data_association/data_association.hpp | 4 +- .../radar_object_tracker_node.hpp | 29 ++++++----- .../constant_turn_rate_motion_tracker.hpp | 14 +++--- .../tracker/model/linear_motion_tracker.hpp | 14 +++--- .../tracker/model/tracker_base.hpp | 19 ++++--- .../utils/radar_object_tracker_utils.hpp | 10 ++-- .../radar_object_tracker/utils/utils.hpp | 6 +-- perception/radar_object_tracker/package.xml | 2 +- .../src/data_association/data_association.cpp | 6 +-- .../radar_object_tracker_node.cpp | 28 +++++------ .../constant_turn_rate_motion_tracker.cpp | 30 +++++------ .../tracker/model/linear_motion_tracker.cpp | 30 +++++------ .../src/tracker/model/tracker_base.cpp | 6 +-- .../src/utils/radar_object_tracker_utils.cpp | 8 +-- .../test/test_radar_object_tracker_utils.cpp | 2 +- .../radar_tracks_msgs_converter/README.md | 10 ++-- .../radar_tracks_msgs_converter_node.hpp | 28 +++++------ .../radar_tracks_msgs_converter/package.xml | 2 +- .../radar_tracks_msgs_converter_node.cpp | 2 +- perception/shape_estimation/README.md | 6 +-- .../corrector/corrector_interface.hpp | 4 +- .../corrector/no_corrector.hpp | 2 +- .../reference_shape_size_corrector.hpp | 2 +- .../shape_estimation/corrector/utils.hpp | 8 +-- .../corrector/vehicle_corrector.hpp | 2 +- .../shape_estimation/filter/bus_filter.hpp | 2 +- .../shape_estimation/filter/car_filter.hpp | 2 +- .../filter/filter_interface.hpp | 5 +- .../shape_estimation/filter/no_filter.hpp | 2 +- .../filter/trailer_filter.hpp | 2 +- .../shape_estimation/filter/truck_filter.hpp | 2 +- .../include/shape_estimation/filter/utils.hpp | 6 +-- .../shape_estimation/model/bounding_box.hpp | 5 +- .../shape_estimation/model/convex_hull.hpp | 2 +- .../shape_estimation/model/cylinder.hpp | 2 +- .../model/model_interface.hpp | 4 +- .../shape_estimation/shape_estimator.hpp | 16 +++--- .../lib/corrector/no_corrector.cpp | 2 +- .../shape_estimation/lib/corrector/utils.cpp | 8 +-- .../lib/filter/bus_filter.cpp | 2 +- .../lib/filter/car_filter.cpp | 2 +- .../shape_estimation/lib/filter/no_filter.cpp | 2 +- .../lib/filter/trailer_filter.cpp | 2 +- .../lib/filter/truck_filter.cpp | 2 +- .../shape_estimation/lib/filter/utils.cpp | 6 +-- .../lib/model/bounding_box.cpp | 8 +-- .../lib/model/convex_hull.cpp | 6 +-- .../shape_estimation/lib/model/cylinder.cpp | 6 +-- .../shape_estimation/lib/shape_estimator.cpp | 12 ++--- perception/shape_estimation/package.xml | 2 +- perception/shape_estimation/src/node.cpp | 12 ++--- perception/shape_estimation/src/node.hpp | 4 +- .../shape_estimation/test_shape_estimator.cpp | 18 +++---- perception/simple_object_merger/README.md | 6 +-- .../simple_object_merger_node.hpp | 6 +-- perception/simple_object_merger/package.xml | 2 +- .../simple_object_merger_node.cpp | 8 +-- perception/tensorrt_yolox/package.xml | 2 +- .../src/tensorrt_yolox_node.cpp | 2 +- perception/tracking_object_merger/README.md | 12 ++--- .../data_association/data_association.hpp | 14 +++--- .../decorative_tracker_merger.hpp | 35 ++++++------- .../utils/tracker_state.hpp | 14 +++--- .../tracking_object_merger/utils/utils.hpp | 18 +++---- perception/tracking_object_merger/package.xml | 2 +- .../src/data_association/data_association.cpp | 10 ++-- .../src/decorative_tracker_merger.cpp | 10 ++-- .../src/utils/tracker_state.cpp | 8 +-- .../src/utils/utils.cpp | 34 ++++++------- perception/traffic_light_arbiter/README.md | 16 +++--- .../signal_match_validator.hpp | 8 +-- .../traffic_light_arbiter.hpp | 12 ++--- perception/traffic_light_arbiter/package.xml | 2 +- .../src/signal_match_validator.cpp | 36 ++++++------- .../src/traffic_light_arbiter.cpp | 18 +++---- perception/traffic_light_classifier/README.md | 4 +- .../README.md | 2 +- .../traffic_light_map_based_detector/node.hpp | 6 +-- .../package.xml | 3 +- .../src/node.cpp | 4 +- .../node.hpp | 12 ++--- .../package.xml | 2 +- .../src/node.cpp | 14 +++--- .../README.md | 18 +++---- .../nodelet.hpp | 6 +-- .../package.xml | 2 +- .../src/nodelet.cpp | 4 +- .../traffic_light_visualization/README.md | 8 +-- .../traffic_light_map_visualizer/node.hpp | 14 +++--- .../src/traffic_light_map_visualizer/node.cpp | 18 +++---- 240 files changed, 1225 insertions(+), 1265 deletions(-) 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