From 6a9d298ce89f5c9fe1f9e859290d98019d31a2ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Sun, 7 Jan 2024 12:57:13 +0100 Subject: [PATCH 01/49] Port VelocityReport message --- external/concealer/include/concealer/autoware_universe.hpp | 4 ++-- external/concealer/package.xml | 2 ++ external/concealer/src/autoware_universe.cpp | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 855e21b871e..856e914d647 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include #include #include @@ -47,7 +47,7 @@ class AutowareUniverse : public Autoware PublisherWrapper setSteeringReport; PublisherWrapper setGearReport; PublisherWrapper setControlModeReport; - PublisherWrapper setVelocityReport; + PublisherWrapper setVelocityReport; PublisherWrapper setTurnIndicatorsReport; // clang-format on diff --git a/external/concealer/package.xml b/external/concealer/package.xml index b6244bade4d..680d46d2d77 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -18,6 +18,8 @@ autoware_auto_system_msgs autoware_auto_vehicle_msgs + autoware_vehicle_msgs + tier4_external_api_msgs tier4_planning_msgs diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 11174493820..9a5c9044226 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -131,7 +131,7 @@ auto AutowareUniverse::updateVehicleState() -> void setVelocityReport([this]() { const auto twist = current_twist.load(); - autoware_auto_vehicle_msgs::msg::VelocityReport message; + autoware_vehicle_msgs::msg::VelocityReport message; message.header.stamp = get_clock()->now(); message.header.frame_id = "base_link"; message.longitudinal_velocity = twist.linear.x; From ac3898e87e7a6a8449641ee24bd558e6b4483be8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Sun, 7 Jan 2024 13:37:12 +0100 Subject: [PATCH 02/49] Delete unused vehicle_state_command and vehicle_control_command --- .../traffic_simulator/include/traffic_simulator/api/api.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp index 89e141727af..ed5cf388245 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/api/api.hpp @@ -17,8 +17,6 @@ #include -#include -#include #include #include #include From 6dbbedbe8f8102c9c9237d3801078edd67dacf9c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Sun, 7 Jan 2024 14:23:53 +0100 Subject: [PATCH 03/49] Port TurnIndicatorsReport --- docs/developer_guide/Communication.md | 2 +- external/concealer/include/concealer/autoware_universe.hpp | 4 ++-- external/concealer/src/autoware_universe.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 1157fd965c9..e320d4890b6 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -30,7 +30,7 @@ | `/vehicle/status/control_mode` | [`autoware_auto_vehicle_msgs/msg/ControlModeReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl) | | | `/vehicle/status/gear_status` | [`autoware_auto_vehicle_msgs/msg/GearReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearReport.idl) | | | `/vehicle/status/steering_status` | [`autoware_auto_vehicle_msgs/msg/SteeringReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/SteeringReport.idl) | | -| `/vehicle/status/turn_indicators_status` | [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl) | | +| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl) | | | `/vehicle/status/velocity_status` | [`autoware_auto_vehicle_msgs/msg/VelocityReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/VelocityReport.idl) | | | `/perception/object_recognition/detection/objects` | [`autoware_auto_perception_msgs/msg/DetectedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/DetectedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | | `/perception/object_recognition/ground_truth/objects` | [`autoware_auto_perception_msgs/msg/TrackedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrackedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 856e914d647..38edd83bd50 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include #include @@ -48,7 +48,7 @@ class AutowareUniverse : public Autoware PublisherWrapper setGearReport; PublisherWrapper setControlModeReport; PublisherWrapper setVelocityReport; - PublisherWrapper setTurnIndicatorsReport; + PublisherWrapper setTurnIndicatorsReport; // clang-format on const rclcpp::TimerBase::SharedPtr localization_update_timer; diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 9a5c9044226..dc84d949651 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -141,7 +141,7 @@ auto AutowareUniverse::updateVehicleState() -> void }()); setTurnIndicatorsReport([this]() { - autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport message; + autoware_vehicle_msgs::msg::TurnIndicatorsReport message; message.stamp = get_clock()->now(); message.report = getTurnIndicatorsCommand().command; return message; From d3f0135b01eb7fb64ad438db0d429ffe84d038ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Sun, 7 Jan 2024 14:46:40 +0100 Subject: [PATCH 04/49] Port TurnIndicatorsCommand --- .../concealer/include/concealer/autoware.hpp | 4 ++-- .../include/concealer/autoware_universe.hpp | 2 +- .../concealer/field_operator_application.hpp | 8 ++++---- ...perator_application_for_autoware_universe.hpp | 4 ++-- external/concealer/src/autoware.cpp | 6 +++--- .../concealer/src/field_operator_application.cpp | 16 ++++++++-------- ...perator_application_for_autoware_universe.cpp | 2 +- 7 files changed, 21 insertions(+), 21 deletions(-) diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp index ff27ba70d0c..676f047457e 100644 --- a/external/concealer/include/concealer/autoware.hpp +++ b/external/concealer/include/concealer/autoware.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include @@ -58,7 +58,7 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster double = 0; virtual auto getTurnIndicatorsCommand() const - -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; + -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand; virtual auto getVehicleCommand() const -> std::tuple< autoware_auto_control_msgs::msg::AckermannControlCommand, diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 38edd83bd50..bb22369a729 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -39,7 +39,7 @@ class AutowareUniverse : public Autoware // clang-format off SubscriberWrapper getAckermannControlCommand; SubscriberWrapper getGearCommandImpl; - SubscriberWrapper getTurnIndicatorsCommand; + SubscriberWrapper getTurnIndicatorsCommand; SubscriberWrapper getPathWithLaneId; PublisherWrapper setAcceleration; diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 38905be51de..548e091cc4a 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include @@ -145,7 +145,7 @@ class FieldOperatorApplication : public rclcpp::Node virtual auto restrictTargetSpeed(double) const -> double = 0; virtual auto getTurnIndicatorsCommand() const - -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; + -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand; virtual auto rethrow() const noexcept(false) -> void; @@ -155,11 +155,11 @@ class FieldOperatorApplication : public rclcpp::Node }; } // namespace concealer -namespace autoware_auto_vehicle_msgs::msg +namespace autoware_vehicle_msgs::msg { auto operator<<(std::ostream &, const TurnIndicatorsCommand &) -> std::ostream &; auto operator>>(std::istream &, TurnIndicatorsCommand &) -> std::istream &; -} // namespace autoware_auto_vehicle_msgs::msg +} // namespace autoware_vehicle_msgs::msg #endif // CONCEALER__AUTOWARE_USER_HPP_ diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index c8ac121fb51..452a5fb746a 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -62,7 +62,7 @@ class FieldOperatorApplicationFor #endif SubscriberWrapper getMrmState; SubscriberWrapper getTrajectory; - SubscriberWrapper getTurnIndicatorsCommandImpl; + SubscriberWrapper getTurnIndicatorsCommandImpl; ServiceWithValidation requestCooperateCommands; ServiceWithValidation requestEngage; @@ -147,7 +147,7 @@ class FieldOperatorApplicationFor auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray override; auto getTurnIndicatorsCommand() const - -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand override; + -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand override; auto getEmergencyStateName() const -> std::string override; diff --git a/external/concealer/src/autoware.cpp b/external/concealer/src/autoware.cpp index a209e4c8f84..4c9290e44c9 100644 --- a/external/concealer/src/autoware.cpp +++ b/external/concealer/src/autoware.cpp @@ -45,12 +45,12 @@ auto Autoware::set(const geometry_msgs::msg::Twist & twist) -> void { current_tw auto Autoware::set(const geometry_msgs::msg::Pose & pose) -> void { current_pose.store(pose); } auto Autoware::getTurnIndicatorsCommand() const - -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand + -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand { static auto turn_indicators_command = []() { - autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; + autoware_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; turn_indicators_command.command = - autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; + autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; return turn_indicators_command; }(); turn_indicators_command.stamp = now(); diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index b1d658b9bab..344a67969a1 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -147,12 +147,12 @@ auto FieldOperatorApplication::shutdownAutoware() -> void } auto FieldOperatorApplication::getTurnIndicatorsCommand() const - -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand + -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand { static auto turn_indicators_command = []() { - autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; + autoware_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; turn_indicators_command.command = - autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; + autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; return turn_indicators_command; }(); turn_indicators_command.stamp = now(); @@ -162,14 +162,14 @@ auto FieldOperatorApplication::getTurnIndicatorsCommand() const auto FieldOperatorApplication::rethrow() const -> void { task_queue.rethrow(); } } // namespace concealer -namespace autoware_auto_vehicle_msgs::msg +namespace autoware_vehicle_msgs::msg { auto operator<<( - std::ostream & out, const autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & message) + std::ostream & out, const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) -> std::ostream & { #define CASE(IDENTIFIER) \ - case autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER: \ + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER: \ out << #IDENTIFIER; \ break @@ -189,11 +189,11 @@ auto operator<<( return out; } -auto operator>>(std::istream & is, autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand & message) +auto operator>>(std::istream & is, autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) -> std::istream & { #define STATE(IDENTIFIER) \ - {#IDENTIFIER, autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER} + {#IDENTIFIER, autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER} std::unordered_map state_dictionary{ STATE(DISABLE), diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index ca7f7ffd423..4abff428514 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -388,7 +388,7 @@ auto FieldOperatorApplicationFor::getWaypoints() const } auto FieldOperatorApplicationFor::getTurnIndicatorsCommand() const - -> autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand + -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand { return getTurnIndicatorsCommandImpl(); } From 1b1ac5a3f896eb788fc5bf5d5d4ea2d75a84074d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 13:17:34 +0100 Subject: [PATCH 05/49] Port SteeringReport --- external/concealer/include/concealer/autoware_universe.hpp | 4 ++-- external/concealer/src/autoware_universe.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index bb22369a729..4411c8cdf19 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include #include @@ -44,7 +44,7 @@ class AutowareUniverse : public Autoware PublisherWrapper setAcceleration; PublisherWrapper setOdometry; - PublisherWrapper setSteeringReport; + PublisherWrapper setSteeringReport; PublisherWrapper setGearReport; PublisherWrapper setControlModeReport; PublisherWrapper setVelocityReport; diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index dc84d949651..6502c25041e 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -123,7 +123,7 @@ auto AutowareUniverse::updateVehicleState() -> void }()); setSteeringReport([this]() { - autoware_auto_vehicle_msgs::msg::SteeringReport message; + autoware_vehicle_msgs::msg::SteeringReport message; message.stamp = get_clock()->now(); message.steering_tire_angle = getSteeringAngle(); return message; From a65aede1f6f55dbf2e9fbd27fd42dbea487fe0d1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 13:21:36 +0100 Subject: [PATCH 06/49] Port GearReport --- external/concealer/include/concealer/autoware_universe.hpp | 4 ++-- external/concealer/src/autoware_universe.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 4411c8cdf19..1e2d06ae0ba 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include @@ -45,7 +45,7 @@ class AutowareUniverse : public Autoware PublisherWrapper setAcceleration; PublisherWrapper setOdometry; PublisherWrapper setSteeringReport; - PublisherWrapper setGearReport; + PublisherWrapper setGearReport; PublisherWrapper setControlModeReport; PublisherWrapper setVelocityReport; PublisherWrapper setTurnIndicatorsReport; diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 6502c25041e..245ea9aa3da 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -116,7 +116,7 @@ auto AutowareUniverse::updateVehicleState() -> void }()); setGearReport([this]() { - autoware_auto_vehicle_msgs::msg::GearReport message; + autoware_vehicle_msgs::msg::GearReport message; message.stamp = get_clock()->now(); message.report = getGearCommand().command; return message; From c4a68d2e493dc2e90d63f4b3116de2bb8aa00901 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 14:00:00 +0100 Subject: [PATCH 07/49] Port GearCommand --- external/concealer/include/concealer/autoware.hpp | 6 +++--- .../include/concealer/autoware_universe.hpp | 6 +++--- ..._operator_application_for_autoware_universe.hpp | 2 +- external/concealer/src/autoware.cpp | 6 +++--- external/concealer/src/autoware_universe.cpp | 6 +++--- .../sim_model_delay_steer_acc_geared.hpp | 2 +- .../sim_model_delay_steer_map_acc_geared.hpp | 2 +- .../sim_model_ideal_steer_acc_geared.hpp | 2 +- .../vehicle_model/sim_model_interface.hpp | 6 +++--- .../sim_model_delay_steer_acc_geared.cpp | 4 ++-- .../sim_model_delay_steer_map_acc_geared.cpp | 4 ++-- .../sim_model_ideal_steer_acc_geared.cpp | 4 ++-- simulation/simulation_interface/CMakeLists.txt | 2 +- .../include/simulation_interface/conversions.hpp | 6 +++--- simulation/simulation_interface/package.xml | 2 +- ...icle_msgs.proto => autoware_vehicle_msgs.proto} | 2 +- .../proto/traffic_simulator_msgs.proto | 4 ++-- .../simulation_interface/src/conversions.cpp | 14 +++++++------- .../traffic_simulator/entity/entity_base.hpp | 2 +- 19 files changed, 41 insertions(+), 41 deletions(-) rename simulation/simulation_interface/proto/{autoware_auto_vehicle_msgs.proto => autoware_vehicle_msgs.proto} (94%) diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp index 676f047457e..eb52c472680 100644 --- a/external/concealer/include/concealer/autoware.hpp +++ b/external/concealer/include/concealer/autoware.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -48,7 +48,7 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster double = 0; - virtual auto getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand; + virtual auto getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand; virtual auto getSteeringAngle() const -> double = 0; @@ -62,7 +62,7 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster std::tuple< autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> = 0; + autoware_vehicle_msgs::msg::GearCommand> = 0; virtual auto getRouteLanelets() const -> std::vector = 0; diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 1e2d06ae0ba..e823a46cc65 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -38,7 +38,7 @@ class AutowareUniverse : public Autoware { // clang-format off SubscriberWrapper getAckermannControlCommand; - SubscriberWrapper getGearCommandImpl; + SubscriberWrapper getGearCommandImpl; SubscriberWrapper getTurnIndicatorsCommand; SubscriberWrapper getPathWithLaneId; @@ -82,13 +82,13 @@ class AutowareUniverse : public Autoware auto updateVehicleState() -> void; - auto getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand override; + auto getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand override; auto getGearSign() const -> double override; auto getVehicleCommand() const -> std::tuple< autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> override; + autoware_vehicle_msgs::msg::GearCommand> override; auto getRouteLanelets() const -> std::vector; }; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 452a5fb746a..3f4dd002f6c 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -26,7 +26,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/external/concealer/src/autoware.cpp b/external/concealer/src/autoware.cpp index 4c9290e44c9..203e8a259c5 100644 --- a/external/concealer/src/autoware.cpp +++ b/external/concealer/src/autoware.cpp @@ -24,11 +24,11 @@ Autoware::Autoware() { } -auto Autoware::getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand +auto Autoware::getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand { static auto gear_command = []() { - autoware_auto_vehicle_msgs::msg::GearCommand gear_command; - gear_command.command = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; + autoware_vehicle_msgs::msg::GearCommand gear_command; + gear_command.command = autoware_vehicle_msgs::msg::GearCommand::DRIVE; return gear_command; }(); gear_command.stamp = now(); diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 245ea9aa3da..fc291dd4ce1 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -148,14 +148,14 @@ auto AutowareUniverse::updateVehicleState() -> void }()); } -auto AutowareUniverse::getGearCommand() const -> autoware_auto_vehicle_msgs::msg::GearCommand +auto AutowareUniverse::getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand { return getGearCommandImpl(); } auto AutowareUniverse::getGearSign() const -> double { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; // @todo Add support for GearCommand::NONE to return 0.0 // @sa https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L475 return getGearCommand().command == GearCommand::REVERSE or @@ -166,7 +166,7 @@ auto AutowareUniverse::getGearSign() const -> double auto AutowareUniverse::getVehicleCommand() const -> std::tuple< autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> + autoware_vehicle_msgs::msg::GearCommand> { return std::make_tuple(getAckermannControlCommand(), getGearCommand()); } diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 1704f7872d2..6499c742ff4 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -148,7 +148,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index edc91b94a27..d4d25611b61 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -327,7 +327,7 @@ class SimModelDelaySteerMapAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index c0c5a9822c5..a44afb7874b 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -105,7 +105,7 @@ class SimModelIdealSteerAccGeared : public SimModelInterface * @brief update state considering current gear * @param [in] state current state * @param [in] prev_state previous state - * @param [in] gear current gear (defined in autoware_auto_msgs/GearCommand) + * @param [in] gear current gear (defined in autoware_msgs/GearCommand) * @param [in] dt delta time to update state */ void updateStateWithGear( diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp index 3fab57e9f50..59fba715edb 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp @@ -15,7 +15,7 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ -#include +#include #include /** @@ -31,7 +31,7 @@ class SimModelInterface Eigen::VectorXd input_; //!< @brief vehicle input vector //!< @brief gear command defined in autoware_auto_msgs/GearCommand - uint8_t gear_ = autoware_auto_vehicle_msgs::msg::GearCommand::DRIVE; + uint8_t gear_ = autoware_vehicle_msgs::msg::GearCommand::DRIVE; public: /** @@ -72,7 +72,7 @@ class SimModelInterface /** * @brief set gear - * @param [in] gear gear command defined in autoware_auto_msgs/GearCommand + * @param [in] gear gear command defined in autoware_msgs/GearCommand */ void setGear(const uint8_t gear); diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 74002da0849..64fe47c3c11 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( @@ -131,7 +131,7 @@ void SimModelDelaySteerAccGeared::updateStateWithGear( state(IDX::ACCX) = (state(IDX::VX) - prev_state(IDX::VX)) / std::max(dt, 1.0e-5); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp index 033581b66a5..936f4a7b104 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_delay_steer_map_acc_geared.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include // copied from https://github.com/tier4/autoware.universe/blob/v0.17.0/common/interpolation/src/linear_interpolation.cpp @@ -280,7 +280,7 @@ Eigen::VectorXd SimModelDelaySteerMapAccGeared::calcModel( void SimModelDelaySteerMapAccGeared::updateStateWithGear( Eigen::VectorXd & state, const Eigen::VectorXd & prev_state, const uint8_t gear, const double dt) { - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 0976349bb0e..615a06f11f6 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -13,7 +13,7 @@ // limitations under the License. #include -#include +#include #include SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(double wheelbase) @@ -69,7 +69,7 @@ void SimModelIdealSteerAccGeared::updateStateWithGear( state(IDX::YAW) = prev_state(IDX::YAW); }; - using autoware_auto_vehicle_msgs::msg::GearCommand; + using autoware_vehicle_msgs::msg::GearCommand; if ( gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || diff --git a/simulation/simulation_interface/CMakeLists.txt b/simulation/simulation_interface/CMakeLists.txt index 6e2cc1fb292..d183a06fb44 100644 --- a/simulation/simulation_interface/CMakeLists.txt +++ b/simulation/simulation_interface/CMakeLists.txt @@ -24,7 +24,7 @@ ament_auto_find_build_dependencies() set(PROTO_FILES "proto/autoware_auto_control_msgs.proto" - "proto/autoware_auto_vehicle_msgs.proto" + "proto/autoware_vehicle_msgs.proto" "proto/builtin_interfaces.proto" "proto/geometry_msgs.proto" "proto/rosgraph_msgs.proto" diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 4ec8320616b..319d9dfaea1 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include @@ -175,14 +175,14 @@ void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannControlCommand); -DEFINE_CONVERSION(autoware_auto_vehicle_msgs, GearCommand); +DEFINE_CONVERSION(autoware_vehicle_msgs, GearCommand); #undef DEFINE_CONVERSION auto toProto( const std::tuple< autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> &, + autoware_vehicle_msgs::msg::GearCommand> &, traffic_simulator_msgs::VehicleCommand &) -> void; template diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index 787fa799c34..d713de7b017 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -11,7 +11,7 @@ ament_cmake_auto autoware_auto_control_msgs - autoware_auto_vehicle_msgs + autoware_vehicle_msgs boost builtin_interfaces geometry_msgs diff --git a/simulation/simulation_interface/proto/autoware_auto_vehicle_msgs.proto b/simulation/simulation_interface/proto/autoware_vehicle_msgs.proto similarity index 94% rename from simulation/simulation_interface/proto/autoware_auto_vehicle_msgs.proto rename to simulation/simulation_interface/proto/autoware_vehicle_msgs.proto index 6b0b314a780..cf1f9767f7c 100644 --- a/simulation/simulation_interface/proto/autoware_auto_vehicle_msgs.proto +++ b/simulation/simulation_interface/proto/autoware_vehicle_msgs.proto @@ -2,7 +2,7 @@ syntax = "proto3"; import "builtin_interfaces.proto"; -package autoware_auto_vehicle_msgs; +package autoware_vehicle_msgs; enum GearCommand_Constants { NONE = 0; diff --git a/simulation/simulation_interface/proto/traffic_simulator_msgs.proto b/simulation/simulation_interface/proto/traffic_simulator_msgs.proto index 3e7a8be5b91..6b972f42a1e 100644 --- a/simulation/simulation_interface/proto/traffic_simulator_msgs.proto +++ b/simulation/simulation_interface/proto/traffic_simulator_msgs.proto @@ -1,7 +1,7 @@ syntax = "proto3"; import "autoware_auto_control_msgs.proto"; -import "autoware_auto_vehicle_msgs.proto"; +import "autoware_vehicle_msgs.proto"; import "geometry_msgs.proto"; package traffic_simulator_msgs; @@ -148,7 +148,7 @@ message Waypoints { message VehicleCommand { autoware_auto_control_msgs.AckermannControlCommand ackermann_control_command = 1; - autoware_auto_vehicle_msgs.GearCommand gear_command = 2; + autoware_vehicle_msgs.GearCommand gear_command = 2; } /** diff --git a/simulation/simulation_interface/src/conversions.cpp b/simulation/simulation_interface/src/conversions.cpp index 1804eb11c1a..4d434fa81c1 100644 --- a/simulation/simulation_interface/src/conversions.cpp +++ b/simulation/simulation_interface/src/conversions.cpp @@ -546,14 +546,14 @@ void toMsg( } auto toProto( - const autoware_auto_vehicle_msgs::msg::GearCommand & message, - autoware_auto_vehicle_msgs::GearCommand & proto) -> void + const autoware_vehicle_msgs::msg::GearCommand & message, + autoware_vehicle_msgs::GearCommand & proto) -> void { toProto(message.stamp, *proto.mutable_stamp()); #define CASE(NAME) \ - case autoware_auto_vehicle_msgs::msg::GearCommand::NAME: \ - proto.set_command(autoware_auto_vehicle_msgs::GearCommand_Constants::NAME); \ + case autoware_vehicle_msgs::msg::GearCommand::NAME: \ + proto.set_command(autoware_vehicle_msgs::GearCommand_Constants::NAME); \ break switch (message.command) { @@ -586,8 +586,8 @@ auto toProto( } auto toMsg( - const autoware_auto_vehicle_msgs::GearCommand & proto, - autoware_auto_vehicle_msgs::msg::GearCommand & message) -> void + const autoware_vehicle_msgs::GearCommand & proto, + autoware_vehicle_msgs::msg::GearCommand & message) -> void { toMsg(proto.stamp(), message.stamp); message.command = proto.command(); @@ -596,7 +596,7 @@ auto toMsg( auto toProto( const std::tuple< autoware_auto_control_msgs::msg::AckermannControlCommand, - autoware_auto_vehicle_msgs::msg::GearCommand> & message, + autoware_vehicle_msgs::msg::GearCommand> & message, traffic_simulator_msgs::VehicleCommand & proto) -> void { toProto(std::get<0>(message), *proto.mutable_ackermann_control_command()); diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index a089fc77584..46b1933db90 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -16,7 +16,7 @@ #define TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_ #include -#include +#include #include #include #include From 6fdbc90cee75cc08b230321fa64c5ad78670b3c1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 14:07:02 +0100 Subject: [PATCH 08/49] Port ControlModeReport --- external/concealer/include/concealer/autoware_universe.hpp | 4 ++-- external/concealer/package.xml | 2 -- external/concealer/src/autoware_universe.cpp | 4 ++-- external/concealer/src/field_operator_application.cpp | 2 +- 4 files changed, 5 insertions(+), 7 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index e823a46cc65..3fe22a93f19 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -46,7 +46,7 @@ class AutowareUniverse : public Autoware PublisherWrapper setOdometry; PublisherWrapper setSteeringReport; PublisherWrapper setGearReport; - PublisherWrapper setControlModeReport; + PublisherWrapper setControlModeReport; PublisherWrapper setVelocityReport; PublisherWrapper setTurnIndicatorsReport; // clang-format on diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 680d46d2d77..55c1259706d 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -16,8 +16,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs - autoware_auto_vehicle_msgs - autoware_vehicle_msgs diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index fc291dd4ce1..0ff600f01c4 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -110,8 +110,8 @@ auto AutowareUniverse::updateLocalization() -> void auto AutowareUniverse::updateVehicleState() -> void { setControlModeReport([this]() { - autoware_auto_vehicle_msgs::msg::ControlModeReport message; - message.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; + autoware_vehicle_msgs::msg::ControlModeReport message; + message.mode = autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; return message; }()); diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 344a67969a1..4f75f6e6fe7 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -215,4 +215,4 @@ auto operator>>(std::istream & is, autoware_vehicle_msgs::msg::TurnIndicatorsCom return is; } -} // namespace autoware_auto_vehicle_msgs::msg +} // namespace autoware_vehicle_msgs::msg From 6ebf21e578a9463d36895f4400a0ff2ba426ead8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 14:07:23 +0100 Subject: [PATCH 09/49] Adjust docs to autoware_auto_vehicle_msgs to autoware_vehicle_msgs port --- docs/developer_guide/Communication.md | 14 +++++++------- docs/developer_guide/OpenSCENARIOSupport.md | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index e320d4890b6..d2bb77c21a4 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -7,8 +7,8 @@ |--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| | `/autoware/state` | [`autoware_auto_system_msgs/msg/AutowareState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl) | Used in UserDefinedValueCondition : `currentAutowareState` | | `/control/command/control_cmd` | [`autoware_auto_control_msgs/msg/AckermannControlCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_control_msgs/msg/AckermannControlCommand.idl) | | -| `/control/command/gear_cmd` | [`autoware_auto_vehicle_msgs/msg/GearCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearCommand.idl) | | -| `/control/command/turn_indicators_cmd` | [`autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | +| `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/GearCommand.idl) | | +| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.idl) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | | `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | | `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`autoware_auto_planning_msgs/msg/PathWithLaneId`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/PathWithLaneId.idl) | | | `/planning/scenario_planning/trajectory` | [`autoware_auto_planning_msgs/msg/Trajectory`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/Trajectory.idl) | | @@ -27,11 +27,11 @@ | `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | | | `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | | `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/vehicle/status/control_mode` | [`autoware_auto_vehicle_msgs/msg/ControlModeReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl) | | -| `/vehicle/status/gear_status` | [`autoware_auto_vehicle_msgs/msg/GearReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/GearReport.idl) | | -| `/vehicle/status/steering_status` | [`autoware_auto_vehicle_msgs/msg/SteeringReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/SteeringReport.idl) | | -| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl) | | -| `/vehicle/status/velocity_status` | [`autoware_auto_vehicle_msgs/msg/VelocityReport`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/VelocityReport.idl) | | +| `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | +| `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | +| `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | +| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | +| `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | | `/perception/object_recognition/detection/objects` | [`autoware_auto_perception_msgs/msg/DetectedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/DetectedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | | `/perception/object_recognition/ground_truth/objects` | [`autoware_auto_perception_msgs/msg/TrackedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrackedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | | `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | diff --git a/docs/developer_guide/OpenSCENARIOSupport.md b/docs/developer_guide/OpenSCENARIOSupport.md index e71a23da9d5..0ca0b990d61 100644 --- a/docs/developer_guide/OpenSCENARIOSupport.md +++ b/docs/developer_guide/OpenSCENARIOSupport.md @@ -170,7 +170,7 @@ See Reference for specific strings. |----------------------------|----------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| | currentState | `.currentState` | Returns Autoware's [state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | | currentEmergencyState | `.currentEmergencyState` | Returns Autoware's [emergency state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | -| currentTurnIndicatorsState | `.currentTurnIndicatorsState` | Returns Autoware's [turn indicators state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | +| currentTurnIndicatorsState | `.currentTurnIndicatorsState` | Returns Autoware's [turn indicators state](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | | RelativeHeadingCondition | `RelativeHeadingCondition()`
`RelativeHeadingCondition(, , )` | Returns the relative angle to the lane heading. | #### External ROS 2 topic condition From 376072e2d02433d61440e47fa5c800f71ec32e70 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 14:51:15 +0100 Subject: [PATCH 10/49] Port AutowareState --- docs/developer_guide/Communication.md | 2 +- docs/developer_guide/OpenSCENARIOSupport.md | 2 +- .../include/concealer/transition_assertion.hpp | 2 +- external/concealer/package.xml | 1 + .../openscenario_interpreter_example/package.xml | 2 +- .../openscenario_interpreter_example/src/timeout.cpp | 10 +++++----- 6 files changed, 10 insertions(+), 9 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index d2bb77c21a4..6e0f84f0263 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -5,7 +5,7 @@ | topic | type | note | |--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| -| `/autoware/state` | [`autoware_auto_system_msgs/msg/AutowareState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl) | Used in UserDefinedValueCondition : `currentAutowareState` | +| `/autoware/state` | [`autoware_auto_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | | `/control/command/control_cmd` | [`autoware_auto_control_msgs/msg/AckermannControlCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_control_msgs/msg/AckermannControlCommand.idl) | | | `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/GearCommand.idl) | | | `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.idl) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | diff --git a/docs/developer_guide/OpenSCENARIOSupport.md b/docs/developer_guide/OpenSCENARIOSupport.md index 0ca0b990d61..5ae9dc694b1 100644 --- a/docs/developer_guide/OpenSCENARIOSupport.md +++ b/docs/developer_guide/OpenSCENARIOSupport.md @@ -168,7 +168,7 @@ See Reference for specific strings. | Name | Syntax | Description | |----------------------------|----------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| currentState | `.currentState` | Returns Autoware's [state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/AutowareState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | +| currentState | `.currentState` | Returns Autoware's [state](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | | currentEmergencyState | `.currentEmergencyState` | Returns Autoware's [emergency state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | | currentTurnIndicatorsState | `.currentTurnIndicatorsState` | Returns Autoware's [turn indicators state](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | | RelativeHeadingCondition | `RelativeHeadingCondition()`
`RelativeHeadingCondition(, , )` | Returns the relative angle to the lane heading. | diff --git a/external/concealer/include/concealer/transition_assertion.hpp b/external/concealer/include/concealer/transition_assertion.hpp index 55724ba70b8..2f5d4c442ec 100644 --- a/external/concealer/include/concealer/transition_assertion.hpp +++ b/external/concealer/include/concealer/transition_assertion.hpp @@ -15,7 +15,7 @@ #ifndef CONCEALER__TRANSITION_ASSERTION_HPP_ #define CONCEALER__TRANSITION_ASSERTION_HPP_ -#include +#include #include #include #include diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 55c1259706d..bb6b7551b88 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -16,6 +16,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_system_msgs + autoware_system_msgs autoware_vehicle_msgs diff --git a/openscenario/openscenario_interpreter_example/package.xml b/openscenario/openscenario_interpreter_example/package.xml index 90fd7b4ab3a..8cfd5fff647 100644 --- a/openscenario/openscenario_interpreter_example/package.xml +++ b/openscenario/openscenario_interpreter_example/package.xml @@ -10,7 +10,7 @@ ament_cmake - autoware_auto_system_msgs + autoware_system_msgs boost libboost-dev rclcpp diff --git a/openscenario/openscenario_interpreter_example/src/timeout.cpp b/openscenario/openscenario_interpreter_example/src/timeout.cpp index 7559a3dcc60..e2bf4571e15 100644 --- a/openscenario/openscenario_interpreter_example/src/timeout.cpp +++ b/openscenario/openscenario_interpreter_example/src/timeout.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -85,11 +85,11 @@ int main(const int argc, char const * const * const argv) using tier4_simulation_msgs::msg::UserDefinedValue; using tier4_simulation_msgs::msg::UserDefinedValueType; - autoware_auto_system_msgs::msg::AutowareState status; + autoware_system_msgs::msg::AutowareState status; - auto subscription = node->create_subscription( + auto subscription = node->create_subscription( "/autoware/state", rclcpp::QoS(1).reliable(), - [&](const autoware_auto_system_msgs::msg::AutowareState::SharedPtr message) { + [&](const autoware_system_msgs::msg::AutowareState::SharedPtr message) { status = *message; }); @@ -99,7 +99,7 @@ int main(const int argc, char const * const * const argv) { static auto duration_since_autoware_engaged = std::chrono::high_resolution_clock::now(); - if (status.state != autoware_auto_system_msgs::msg::AutowareState::DRIVING) { + if (status.state != autoware_system_msgs::msg::AutowareState::DRIVING) { duration_since_autoware_engaged = std::chrono::high_resolution_clock::now(); } From 7636e3ceb5e806534d65d6767b86e657e182e923 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 14:52:41 +0100 Subject: [PATCH 11/49] Port PathWithLaneId --- external/concealer/include/concealer/autoware_universe.hpp | 4 ++-- .../include/concealer/field_operator_application.hpp | 2 +- .../field_operator_application_for_autoware_universe.hpp | 4 ++-- external/concealer/package.xml | 1 + 4 files changed, 6 insertions(+), 5 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 3fe22a93f19..bb3514770f2 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -16,7 +16,7 @@ #define CONCEALER__AUTOWARE_UNIVERSE_HPP_ #include -#include +#include #include #include #include @@ -40,7 +40,7 @@ class AutowareUniverse : public Autoware SubscriberWrapper getAckermannControlCommand; SubscriberWrapper getGearCommandImpl; SubscriberWrapper getTurnIndicatorsCommand; - SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getPathWithLaneId; PublisherWrapper setAcceleration; PublisherWrapper setOdometry; diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 548e091cc4a..ca70dacf348 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 3f4dd002f6c..7e2cb2087ed 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include #include @@ -105,7 +105,7 @@ class FieldOperatorApplicationFor auto sendSIGINT() -> void override; public: - SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getPathWithLaneId; public: template diff --git a/external/concealer/package.xml b/external/concealer/package.xml index bb6b7551b88..fb915d4a041 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -15,6 +15,7 @@ autoware_auto_control_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_planning_msgs autoware_auto_system_msgs autoware_system_msgs autoware_vehicle_msgs From af9bf3ec03bb3fdaed5035ba39bef4afeb67906f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 14:59:18 +0100 Subject: [PATCH 12/49] Finish porting autoware_auto_planning_msgs to autoware_planning_msgs --- docs/developer_guide/Communication.md | 2 +- external/concealer/package.xml | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 6e0f84f0263..58196a994c4 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -10,7 +10,7 @@ | `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/GearCommand.idl) | | | `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.idl) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | | `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | -| `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`autoware_auto_planning_msgs/msg/PathWithLaneId`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/PathWithLaneId.idl) | | +| `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`autoware_planning_msgs/msg/PathWithLaneId`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_planning_msgs/msg/PathWithLaneId.msg) | | | `/planning/scenario_planning/trajectory` | [`autoware_auto_planning_msgs/msg/Trajectory`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/Trajectory.idl) | | | `/system/emergency/emergency_state` | [`autoware_auto_system_msgs/msg/EmergencyState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl) | Used in UserDefinedValueCondition : `currentEmergencyState` | | `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | diff --git a/external/concealer/package.xml b/external/concealer/package.xml index fb915d4a041..c3fe31e8567 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -14,7 +14,6 @@ autoware_adapi_v1_msgs autoware_auto_control_msgs autoware_auto_perception_msgs - autoware_auto_planning_msgs autoware_planning_msgs autoware_auto_system_msgs autoware_system_msgs From 3a9bd9b6b1e6f9db294b0117d80dd3d9379f7474 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 15:01:38 +0100 Subject: [PATCH 13/49] FIX: replace old Trajectory message with tier4_autoware_msgs which is used in scenario_simulator_v2 --- docs/developer_guide/Communication.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 58196a994c4..38b75e5e91a 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -11,7 +11,7 @@ | `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.idl) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | | `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | | `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`autoware_planning_msgs/msg/PathWithLaneId`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_planning_msgs/msg/PathWithLaneId.msg) | | -| `/planning/scenario_planning/trajectory` | [`autoware_auto_planning_msgs/msg/Trajectory`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_planning_msgs/msg/Trajectory.idl) | | +| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | | `/system/emergency/emergency_state` | [`autoware_auto_system_msgs/msg/EmergencyState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl) | Used in UserDefinedValueCondition : `currentEmergencyState` | | `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | | `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in /simulation/openscenario_interpreter | From 4b9217256137a6490c339e8d63988d94b914f2ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 8 Jan 2024 15:20:08 +0100 Subject: [PATCH 14/49] Port HADMapBin to LaneletMapBin --- .../include/traffic_simulator/hdmap_utils/hdmap_utils.hpp | 6 +++--- simulation/traffic_simulator/package.xml | 1 + .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 4 ++-- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 24375e4b211..7d6e948159d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include #include @@ -293,7 +293,7 @@ class HdMapUtils const bool include_opposite_direction = true) const -> std::vector; - auto toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin; + auto toMapBin() const -> autoware_map_msgs::msg::LaneletMapBin; auto toMapPoints(const lanelet::Id, const std::vector & s) const -> std::vector; @@ -383,7 +383,7 @@ class HdMapUtils auto getVectorFromPose(const geometry_msgs::msg::Pose &, const double magnitude) const -> geometry_msgs::msg::Vector3; - auto mapCallback(const autoware_auto_mapping_msgs::msg::HADMapBin &) const -> void; + auto mapCallback(const autoware_map_msgs::msg::LaneletMapBin &) const -> void; auto overwriteLaneletsCenterline() -> void; diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index a319e0b8fad..bc055a68410 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -39,6 +39,7 @@ tier4_debug_msgs autoware_auto_perception_msgs autoware_perception_msgs + autoware_map_msgs tinyxml2_vendor pluginlib traffic_simulator_msgs diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index c17cebc71c6..6ed99ed6b54 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1475,7 +1475,7 @@ auto HdMapUtils::getLongitudinalDistance( return distance; } -auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin +auto HdMapUtils::toMapBin() const -> autoware_map_msgs::msg::LaneletMapBin { std::stringstream ss; boost::archive::binary_oarchive oa(ss); @@ -1483,7 +1483,7 @@ auto HdMapUtils::toMapBin() const -> autoware_auto_mapping_msgs::msg::HADMapBin auto id_counter = lanelet::utils::getId(); oa << id_counter; std::string tmp_str = ss.str(); - autoware_auto_mapping_msgs::msg::HADMapBin msg; + autoware_map_msgs::msg::LaneletMapBin msg; msg.data.clear(); msg.data.resize(tmp_str.size()); msg.data.assign(tmp_str.begin(), tmp_str.end()); From 6fffa0545eea5f51a58db57ff2cff9d8c335274a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Tue, 9 Jan 2024 14:48:22 +0100 Subject: [PATCH 15/49] Port AckermannControlCommand to Control --- .../concealer/include/concealer/autoware.hpp | 4 +-- .../include/concealer/autoware_universe.hpp | 6 ++-- .../concealer/field_operator_application.hpp | 2 +- ...ator_application_for_autoware_universe.hpp | 6 ++-- external/concealer/package.xml | 2 +- external/concealer/src/autoware_universe.cpp | 12 +++---- .../simulation_interface/CMakeLists.txt | 2 +- .../simulation_interface/conversions.hpp | 10 +++--- simulation/simulation_interface/package.xml | 2 +- .../proto/autoware_auto_control_msgs.proto | 24 -------------- .../proto/autoware_control_msgs.proto | 30 +++++++++++++++++ .../proto/traffic_simulator_msgs.proto | 4 +-- .../simulation_interface/src/conversions.cpp | 32 +++++++++---------- .../test/expect_equal_macros.hpp | 4 +-- .../test/test_conversions.cpp | 10 +++--- .../traffic_simulator/entity/entity_base.hpp | 2 +- 16 files changed, 79 insertions(+), 73 deletions(-) delete mode 100644 simulation/simulation_interface/proto/autoware_auto_control_msgs.proto create mode 100644 simulation/simulation_interface/proto/autoware_control_msgs.proto diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp index eb52c472680..aeb281aa9c0 100644 --- a/external/concealer/include/concealer/autoware.hpp +++ b/external/concealer/include/concealer/autoware.hpp @@ -16,7 +16,7 @@ #define CONCEALER__AUTOWARE_HPP_ #include -#include +#include #include #include #include @@ -61,7 +61,7 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster autoware_vehicle_msgs::msg::TurnIndicatorsCommand; virtual auto getVehicleCommand() const -> std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, + autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> = 0; virtual auto getRouteLanelets() const -> std::vector = 0; diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index bb3514770f2..21c81e15164 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -15,7 +15,7 @@ #ifndef CONCEALER__AUTOWARE_UNIVERSE_HPP_ #define CONCEALER__AUTOWARE_UNIVERSE_HPP_ -#include +#include #include #include #include @@ -37,7 +37,7 @@ namespace concealer class AutowareUniverse : public Autoware { // clang-format off - SubscriberWrapper getAckermannControlCommand; + SubscriberWrapper getCommand; SubscriberWrapper getGearCommandImpl; SubscriberWrapper getTurnIndicatorsCommand; SubscriberWrapper getPathWithLaneId; @@ -87,7 +87,7 @@ class AutowareUniverse : public Autoware auto getGearSign() const -> double override; auto getVehicleCommand() const -> std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, + autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> override; auto getRouteLanelets() const -> std::vector; diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index ca70dacf348..01af6d7da17 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 7e2cb2087ed..86f152a8194 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -22,7 +22,7 @@ #include #include #include -#include +#include #include #include #include @@ -53,7 +53,7 @@ class FieldOperatorApplicationFor friend class TransitionAssertion>; // clang-format off - SubscriberWrapper getAckermannControlCommand; + SubscriberWrapper getCommand; SubscriberWrapper getAutowareState; SubscriberWrapper getCooperateStatusArray; SubscriberWrapper getEmergencyState; @@ -112,7 +112,7 @@ class FieldOperatorApplicationFor CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) : FieldOperatorApplication(std::forward(xs)...), // clang-format off - getAckermannControlCommand("/control/command/control_cmd", *this), + getCommand("/control/command/control_cmd", *this), getAutowareState("/api/iv_msgs/autoware/state", *this), getCooperateStatusArray("/api/external/get/rtc_status", *this, [this](const auto & v) { latest_cooperate_status_array = v; }), getEmergencyState("/api/external/get/emergency", *this, [this](const auto & v) { receiveEmergencyState(v); }), diff --git a/external/concealer/package.xml b/external/concealer/package.xml index c3fe31e8567..710ce0dafa5 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -12,7 +12,7 @@ autoware_adapi_v1_msgs - autoware_auto_control_msgs + autoware_control_msgs autoware_auto_perception_msgs autoware_planning_msgs autoware_auto_system_msgs diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 0ff600f01c4..4d7310ec032 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -17,7 +17,7 @@ namespace concealer { AutowareUniverse::AutowareUniverse() -: getAckermannControlCommand("/control/command/control_cmd", *this), +: getCommand("/control/command/control_cmd", *this), getGearCommandImpl("/control/command/gear_cmd", *this), getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", *this), getPathWithLaneId( @@ -65,17 +65,17 @@ auto AutowareUniverse::stopAndJoin() -> void auto AutowareUniverse::getAcceleration() const -> double { - return getAckermannControlCommand().longitudinal.acceleration; + return getCommand().longitudinal.acceleration; } auto AutowareUniverse::getVelocity() const -> double { - return getAckermannControlCommand().longitudinal.speed; + return getCommand().longitudinal.velocity; } auto AutowareUniverse::getSteeringAngle() const -> double { - return getAckermannControlCommand().lateral.steering_tire_angle; + return getCommand().lateral.steering_tire_angle; } auto AutowareUniverse::updateLocalization() -> void @@ -165,10 +165,10 @@ auto AutowareUniverse::getGearSign() const -> double } auto AutowareUniverse::getVehicleCommand() const -> std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, + autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> { - return std::make_tuple(getAckermannControlCommand(), getGearCommand()); + return std::make_tuple(getCommand(), getGearCommand()); } auto AutowareUniverse::getRouteLanelets() const -> std::vector diff --git a/simulation/simulation_interface/CMakeLists.txt b/simulation/simulation_interface/CMakeLists.txt index d183a06fb44..00f59161a4d 100644 --- a/simulation/simulation_interface/CMakeLists.txt +++ b/simulation/simulation_interface/CMakeLists.txt @@ -23,7 +23,7 @@ include(FindProtobuf REQUIRED) ament_auto_find_build_dependencies() set(PROTO_FILES - "proto/autoware_auto_control_msgs.proto" + "proto/autoware_control_msgs.proto" "proto/autoware_vehicle_msgs.proto" "proto/builtin_interfaces.proto" "proto/geometry_msgs.proto" diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 319d9dfaea1..5f8b041fc28 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -172,16 +172,16 @@ void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header); auto toProto(const PACKAGE::msg::TYPENAME &, PACKAGE::TYPENAME &)->void; \ auto toMsg(const PACKAGE::TYPENAME &, PACKAGE::msg::TYPENAME &)->void -DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannLateralCommand); -DEFINE_CONVERSION(autoware_auto_control_msgs, LongitudinalCommand); -DEFINE_CONVERSION(autoware_auto_control_msgs, AckermannControlCommand); +DEFINE_CONVERSION(autoware_control_msgs, Lateral); +DEFINE_CONVERSION(autoware_control_msgs, Longitudinal); +DEFINE_CONVERSION(autoware_control_msgs, Control); DEFINE_CONVERSION(autoware_vehicle_msgs, GearCommand); #undef DEFINE_CONVERSION auto toProto( const std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, + autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> &, traffic_simulator_msgs::VehicleCommand &) -> void; diff --git a/simulation/simulation_interface/package.xml b/simulation/simulation_interface/package.xml index d713de7b017..f2ba55c71ac 100644 --- a/simulation/simulation_interface/package.xml +++ b/simulation/simulation_interface/package.xml @@ -10,7 +10,7 @@ ament_cmake ament_cmake_auto - autoware_auto_control_msgs + autoware_control_msgs autoware_vehicle_msgs boost builtin_interfaces diff --git a/simulation/simulation_interface/proto/autoware_auto_control_msgs.proto b/simulation/simulation_interface/proto/autoware_auto_control_msgs.proto deleted file mode 100644 index 9b65d53125c..00000000000 --- a/simulation/simulation_interface/proto/autoware_auto_control_msgs.proto +++ /dev/null @@ -1,24 +0,0 @@ -syntax = "proto3"; - -import "builtin_interfaces.proto"; - -package autoware_auto_control_msgs; - -message AckermannLateralCommand { - builtin_interfaces.Time stamp = 1; - float steering_tire_angle = 2; - float steering_tire_rotation_rate = 3; -} - -message LongitudinalCommand { - builtin_interfaces.Time stamp = 1; - float speed = 2; - float acceleration = 3; - float jerk = 4; -} - -message AckermannControlCommand { - builtin_interfaces.Time stamp = 1; - AckermannLateralCommand lateral = 2; - LongitudinalCommand longitudinal = 3; -} diff --git a/simulation/simulation_interface/proto/autoware_control_msgs.proto b/simulation/simulation_interface/proto/autoware_control_msgs.proto new file mode 100644 index 00000000000..4ae0543c96c --- /dev/null +++ b/simulation/simulation_interface/proto/autoware_control_msgs.proto @@ -0,0 +1,30 @@ +syntax = "proto3"; + +import "builtin_interfaces.proto"; + +package autoware_control_msgs; + +message Lateral { + builtin_interfaces.Time stamp = 1; + builtin_interfaces.Time cotrol_time = 2; + float steering_tire_angle = 3; + float steering_tire_rotation_rate = 4; + bool is_defined_steering_tire_rotation_rate = 5; +} + +message Longitudinal { + builtin_interfaces.Time stamp = 1; + builtin_interfaces.Time cotrol_time = 2; + float velocity = 3; + float acceleration = 4; + float jerk = 5; + bool is_defined_acceleration = 6; + bool is_defined_jerk = 7; +} + +message Control { + builtin_interfaces.Time stamp = 1; + builtin_interfaces.Time cotrol_time = 2; + Lateral lateral = 3; + Longitudinal longitudinal = 4; +} diff --git a/simulation/simulation_interface/proto/traffic_simulator_msgs.proto b/simulation/simulation_interface/proto/traffic_simulator_msgs.proto index 6b972f42a1e..0f571a5cff8 100644 --- a/simulation/simulation_interface/proto/traffic_simulator_msgs.proto +++ b/simulation/simulation_interface/proto/traffic_simulator_msgs.proto @@ -1,6 +1,6 @@ syntax = "proto3"; -import "autoware_auto_control_msgs.proto"; +import "autoware_control_msgs.proto"; import "autoware_vehicle_msgs.proto"; import "geometry_msgs.proto"; @@ -147,7 +147,7 @@ message Waypoints { } message VehicleCommand { - autoware_auto_control_msgs.AckermannControlCommand ackermann_control_command = 1; + autoware_control_msgs.Control control = 1; autoware_vehicle_msgs.GearCommand gear_command = 2; } diff --git a/simulation/simulation_interface/src/conversions.cpp b/simulation/simulation_interface/src/conversions.cpp index 4d434fa81c1..b124363e67e 100644 --- a/simulation/simulation_interface/src/conversions.cpp +++ b/simulation/simulation_interface/src/conversions.cpp @@ -490,8 +490,8 @@ void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header) } void toProto( - const autoware_auto_control_msgs::msg::AckermannLateralCommand & message, - autoware_auto_control_msgs::AckermannLateralCommand & proto) + const autoware_control_msgs::msg::Lateral & message, + autoware_control_msgs::Lateral & proto) { toProto(message.stamp, *proto.mutable_stamp()); proto.set_steering_tire_angle(message.steering_tire_angle); @@ -499,8 +499,8 @@ void toProto( } void toMsg( - const autoware_auto_control_msgs::AckermannLateralCommand & proto, - autoware_auto_control_msgs::msg::AckermannLateralCommand & message) + const autoware_control_msgs::Lateral & proto, + autoware_control_msgs::msg::Lateral & message) { toMsg(proto.stamp(), message.stamp); message.steering_tire_angle = proto.steering_tire_angle(); @@ -508,28 +508,28 @@ void toMsg( } void toProto( - const autoware_auto_control_msgs::msg::LongitudinalCommand & message, - autoware_auto_control_msgs::LongitudinalCommand & proto) + const autoware_control_msgs::msg::Longitudinal & message, + autoware_control_msgs::Longitudinal & proto) { toProto(message.stamp, *proto.mutable_stamp()); - proto.set_speed(message.speed); + proto.set_velocity(message.velocity); proto.set_acceleration(message.acceleration); proto.set_jerk(message.jerk); } void toMsg( - const autoware_auto_control_msgs::LongitudinalCommand & proto, - autoware_auto_control_msgs::msg::LongitudinalCommand & message) + const autoware_control_msgs::Longitudinal & proto, + autoware_control_msgs::msg::Longitudinal & message) { toMsg(proto.stamp(), message.stamp); - message.speed = proto.speed(); + message.velocity = proto.velocity(); message.acceleration = proto.acceleration(); message.jerk = proto.jerk(); } void toProto( - const autoware_auto_control_msgs::msg::AckermannControlCommand & message, - autoware_auto_control_msgs::AckermannControlCommand & proto) + const autoware_control_msgs::msg::Control & message, + autoware_control_msgs::Control & proto) { toProto(message.stamp, *proto.mutable_stamp()); toProto(message.lateral, *proto.mutable_lateral()); @@ -537,8 +537,8 @@ void toProto( } void toMsg( - const autoware_auto_control_msgs::AckermannControlCommand & proto, - autoware_auto_control_msgs::msg::AckermannControlCommand & message) + const autoware_control_msgs::Control & proto, + autoware_control_msgs::msg::Control & message) { toMsg(proto.stamp(), message.stamp); toMsg(proto.lateral(), message.lateral); @@ -595,11 +595,11 @@ auto toMsg( auto toProto( const std::tuple< - autoware_auto_control_msgs::msg::AckermannControlCommand, + autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> & message, traffic_simulator_msgs::VehicleCommand & proto) -> void { - toProto(std::get<0>(message), *proto.mutable_ackermann_control_command()); + toProto(std::get<0>(message), *proto.mutable_control()); toProto(std::get<1>(message), *proto.mutable_gear_command()); } diff --git a/simulation/simulation_interface/test/expect_equal_macros.hpp b/simulation/simulation_interface/test/expect_equal_macros.hpp index 2620c1af700..2cfe37ec892 100644 --- a/simulation/simulation_interface/test/expect_equal_macros.hpp +++ b/simulation/simulation_interface/test/expect_equal_macros.hpp @@ -156,14 +156,14 @@ * @brief Expect equal macros for autoware related messages. */ #define EXPECT_CONTROL_COMMAND_EQ(MSG, PROTO) \ - EXPECT_DOUBLE_EQ(MSG.longitudinal.speed, PROTO.longitudinal().speed()); \ + EXPECT_DOUBLE_EQ(MSG.longitudinal.velocity, PROTO.longitudinal().velocity()); \ EXPECT_DOUBLE_EQ(MSG.longitudinal.acceleration, PROTO.longitudinal().acceleration()); \ EXPECT_DOUBLE_EQ(MSG.lateral.steering_tire_angle, PROTO.lateral().steering_tire_angle()); \ EXPECT_DOUBLE_EQ( \ MSG.lateral.steering_tire_rotation_rate, PROTO.lateral().steering_tire_rotation_rate()); #define EXPECT_VEHICLE_COMMAND_EQ(CONTROL_MSG, GEAR_MSG, EMERGENCY_MSG, PROTO) \ - EXPECT_CONTROL_COMMAND_EQ(CONTROL_MSG, PROTO.ackermann_control_command()); \ + EXPECT_CONTROL_COMMAND_EQ(CONTROL_MSG, PROTO.control()); \ EXPECT_DOUBLE_EQ(GEAR_MSG.command, PROTO.gear_command().data()); \ EXPECT_DOUBLE_EQ(EMERGENCY_MSG.emergency, PROTO.vehicle_emergency_stamped().emergency()); diff --git a/simulation/simulation_interface/test/test_conversions.cpp b/simulation/simulation_interface/test/test_conversions.cpp index 0a7bf9bccb7..833baa4fd6d 100644 --- a/simulation/simulation_interface/test/test_conversions.cpp +++ b/simulation/simulation_interface/test/test_conversions.cpp @@ -438,20 +438,20 @@ TEST(Conversion, Clock) EXPECT_CLOCK_EQ(msg, proto); } -TEST(Conversion, AckermannControlCommand) +TEST(Conversion, Control) { - autoware_auto_control_msgs::AckermannControlCommand proto; - autoware_auto_control_msgs::msg::AckermannControlCommand msg; + autoware_control_msgs::Control proto; + autoware_control_msgs::msg::Control msg; msg.longitudinal.acceleration = 3; msg.lateral.steering_tire_angle = 1.4; msg.lateral.steering_tire_rotation_rate = 13.4; - msg.longitudinal.speed = 11.3; + msg.longitudinal.velocity = 11.3; simulation_interface::toProto(msg, proto); EXPECT_CONTROL_COMMAND_EQ(msg, proto); msg.longitudinal.acceleration = 0; msg.lateral.steering_tire_angle = 0; msg.lateral.steering_tire_rotation_rate = 0; - msg.longitudinal.speed = 0; + msg.longitudinal.velocity = 0; simulation_interface::toMsg(proto, msg); EXPECT_CONTROL_COMMAND_EQ(msg, proto); } diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp index 46b1933db90..6654b86553f 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp @@ -15,7 +15,7 @@ #ifndef TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_ #define TRAFFIC_SIMULATOR__ENTITY__ENTITY_BASE_HPP_ -#include +#include #include #include #include From 735258a95607b7a6f63f05dfdb751866335ac516 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Wed, 24 Jan 2024 14:34:32 +0100 Subject: [PATCH 16/49] Port TrafficSignalArray from autoware_auto_perception_msgs to autoware_perception_msgs --- ...ator_application_for_autoware_universe.hpp | 2 +- external/concealer/package.xml | 2 +- .../sensor_simulation/sensor_simulation.hpp | 3 +-- .../vehicle_model/sim_model_interface.hpp | 2 +- .../traffic_light_publisher.cpp | 24 ------------------- 5 files changed, 4 insertions(+), 29 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 86f152a8194..066a2fac439 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -23,7 +23,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 710ce0dafa5..08940d0bfc8 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -13,7 +13,7 @@ autoware_adapi_v1_msgs autoware_control_msgs - autoware_auto_perception_msgs + autoware_perception_msgs autoware_planning_msgs autoware_auto_system_msgs autoware_system_msgs diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 6927a364d4f..6b3437cbd1e 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -19,7 +19,6 @@ #include #include -#include #include #include #include @@ -97,7 +96,7 @@ class SensorSimulation rclcpp::Node & node, std::shared_ptr hdmap_utils) -> void { if (configuration.architecture_type() == "awf/universe") { - using Message = autoware_auto_perception_msgs::msg::TrafficSignalArray; + using Message = autoware_perception_msgs::msg::TrafficSignalArray; traffic_lights_detectors_.push_back(std::make_unique( std::make_shared>( "/perception/traffic_light_recognition/traffic_signals", &node, hdmap_utils))); diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp index 59fba715edb..b6145ccf413 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/vehicle_model/sim_model_interface.hpp @@ -30,7 +30,7 @@ class SimModelInterface Eigen::VectorXd state_; //!< @brief vehicle state vector Eigen::VectorXd input_; //!< @brief vehicle input vector - //!< @brief gear command defined in autoware_auto_msgs/GearCommand + //!< @brief gear command defined in autoware_msgs/GearCommand uint8_t gear_ = autoware_vehicle_msgs::msg::GearCommand::DRIVE; public: diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index 64f6b8bd3e7..422d6ccfd7f 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -12,35 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include namespace traffic_simulator { -template <> -auto TrafficLightPublisher::publish( - const rclcpp::Time & current_ros_time, - const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void -{ - autoware_auto_perception_msgs::msg::TrafficSignalArray message; - using TrafficLightType = autoware_auto_perception_msgs::msg::TrafficSignal; - message.header.frame_id = "camera_link"; // DIRTY HACK!!! - message.header.stamp = current_ros_time; - for (const auto & traffic_light : request.states()) { - TrafficLightType traffic_light_message; - traffic_light_message.map_primitive_id = traffic_light.id(); - for (auto bulb_status : traffic_light.traffic_light_status()) { - using TrafficLightBulbType = TrafficLightType::_lights_type::value_type; - TrafficLightBulbType light_bulb_message; - simulation_interface::toMsg(bulb_status, light_bulb_message); - traffic_light_message.lights.push_back(light_bulb_message); - } - message.signals.push_back(traffic_light_message); - } - traffic_light_state_array_publisher_->publish(message); -} - template <> auto TrafficLightPublisher::publish( const rclcpp::Time & current_ros_time, From d69a1e8074ba90b1fc50f8063f426bd0789d058d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Wed, 24 Jan 2024 16:50:59 +0100 Subject: [PATCH 17/49] Update documentation --- docs/developer_guide/Communication.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 38b75e5e91a..ac0ba5c1016 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -5,10 +5,10 @@ | topic | type | note | |--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| -| `/autoware/state` | [`autoware_auto_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | -| `/control/command/control_cmd` | [`autoware_auto_control_msgs/msg/AckermannControlCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_control_msgs/msg/AckermannControlCommand.idl) | | -| `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/GearCommand.idl) | | -| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.idl) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | +| `/autoware/state` | [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | +| `/control/command/control_cmd` | [`autoware_control_msgs/msg/Control`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_control_msgs/msg/Control.msg) | | +| `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/GearCommand.msg) | | +| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | | `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | | `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`autoware_planning_msgs/msg/PathWithLaneId`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_planning_msgs/msg/PathWithLaneId.msg) | | | `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | @@ -36,7 +36,7 @@ | `/perception/object_recognition/ground_truth/objects` | [`autoware_auto_perception_msgs/msg/TrackedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrackedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | | `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | | `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | -| `/perception/traffic_light_recognition/traffic_signals` | [`autoware_auto_perception_msgs/msg/TrafficSignalArray`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl) | for `architecture_type` equal to `awf/universe` | +| `/perception/traffic_light_recognition/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | for `architecture_type` equal to `awf/universe` | | `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | for `architecture_type` greater or equal to `awf/universe/20230906` | | `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | | | `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | | From 2c03a14fc07ba979b6addadc631eebfe283f40e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Thu, 29 Feb 2024 12:17:54 +0100 Subject: [PATCH 18/49] PathWithLaneId: change autoware_msgs to tier4_planning_msgs --- external/concealer/include/concealer/autoware_universe.hpp | 4 ++-- .../field_operator_application_for_autoware_universe.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 21c81e15164..48076ee09bc 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -16,7 +16,7 @@ #define CONCEALER__AUTOWARE_UNIVERSE_HPP_ #include -#include +#include #include #include #include @@ -40,7 +40,7 @@ class AutowareUniverse : public Autoware SubscriberWrapper getCommand; SubscriberWrapper getGearCommandImpl; SubscriberWrapper getTurnIndicatorsCommand; - SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getPathWithLaneId; PublisherWrapper setAcceleration; PublisherWrapper setOdometry; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 066a2fac439..2582bdc5ec7 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include #include @@ -105,7 +105,7 @@ class FieldOperatorApplicationFor auto sendSIGINT() -> void override; public: - SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getPathWithLaneId; public: template From 3588c05955c9c143d967a0d4e2e7fc8e548f7a39 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Thu, 29 Feb 2024 13:03:45 +0100 Subject: [PATCH 19/49] Concealer: Delete unused autoware_planning_msgs --- .../concealer/include/concealer/field_operator_application.hpp | 1 - external/concealer/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 01af6d7da17..b0b06bd6104 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -21,7 +21,6 @@ #include #include -#include #include #include #include diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 08940d0bfc8..05e3df71f8c 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -14,7 +14,6 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_perception_msgs - autoware_planning_msgs autoware_auto_system_msgs autoware_system_msgs autoware_vehicle_msgs From 89f3b893102d9074f285daddb924e98de740f073 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 13 May 2024 16:40:01 +0200 Subject: [PATCH 20/49] Port DetectedObjects and TrackedObjects --- docs/developer_guide/Communication.md | 4 +- .../sensor_simulation/sensor_simulation.hpp | 8 +-- .../simple_sensor_simulator/package.xml | 1 - .../detection_sensor/detection_sensor.cpp | 62 +++++++++---------- simulation/traffic_simulator/package.xml | 1 - 5 files changed, 37 insertions(+), 39 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index ac0ba5c1016..c35baf47bb9 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -32,8 +32,8 @@ | `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | | `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | | `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | -| `/perception/object_recognition/detection/objects` | [`autoware_auto_perception_msgs/msg/DetectedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/DetectedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/object_recognition/ground_truth/objects` | [`autoware_auto_perception_msgs/msg/TrackedObjects`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_perception_msgs/msg/TrackedObjects.idl) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | | `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | | `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | | `/perception/traffic_light_recognition/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | for `architecture_type` equal to `awf/universe` | diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 6b3437cbd1e..5e0e5c54536 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -17,8 +17,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -57,8 +57,8 @@ class SensorSimulation -> void { if (configuration.architecture_type().find("awf/universe") != std::string::npos) { - using Message = autoware_auto_perception_msgs::msg::DetectedObjects; - using GroundTruthMessage = autoware_auto_perception_msgs::msg::TrackedObjects; + using Message = autoware_perception_msgs::msg::DetectedObjects; + using GroundTruthMessage = autoware_perception_msgs::msg::TrackedObjects; detection_sensors_.push_back(std::make_unique>( current_simulation_time, configuration, node.create_publisher("/perception/object_recognition/detection/objects", 1), diff --git a/simulation/simple_sensor_simulator/package.xml b/simulation/simple_sensor_simulator/package.xml index d3298f701ea..a5dbdb8677d 100644 --- a/simulation/simple_sensor_simulator/package.xml +++ b/simulation/simple_sensor_simulator/package.xml @@ -35,7 +35,6 @@ tf2_ros tf2 - autoware_auto_perception_msgs autoware_perception_msgs boost eigen diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index de0152ff850..d36897858b0 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -15,8 +15,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -111,9 +111,9 @@ auto DetectionSensorBase::filterObjectsBySensorRange( } template <> -auto DetectionSensor::applyPositionNoise( - autoware_auto_perception_msgs::msg::DetectedObject detected_object) - -> autoware_auto_perception_msgs::msg::DetectedObject +auto DetectionSensor::applyPositionNoise( + autoware_perception_msgs::msg::DetectedObject detected_object) + -> autoware_perception_msgs::msg::DetectedObject { auto position_noise_distribution = std::normal_distribution<>(0.0, configuration_.pos_noise_stddev()); @@ -135,14 +135,14 @@ unique_identifier_msgs::msg::UUID generateUUIDMsg(const std::string & input) } template <> -auto DetectionSensor::update( +auto DetectionSensor::update( const double current_simulation_time, const std::vector & statuses, const rclcpp::Time & current_ros_time, const std::vector & lidar_detected_entities) -> void { auto makeObjectClassification = [](const auto & label) { - autoware_auto_perception_msgs::msg::ObjectClassification object_classification; + autoware_perception_msgs::msg::ObjectClassification object_classification; object_classification.label = label; object_classification.probability = 1; return object_classification; @@ -157,11 +157,11 @@ auto DetectionSensor::updat : lidar_detected_entities, configuration_.range()); - autoware_auto_perception_msgs::msg::DetectedObjects msg; + autoware_perception_msgs::msg::DetectedObjects msg; msg.header.stamp = current_ros_time; msg.header.frame_id = "map"; - autoware_auto_perception_msgs::msg::TrackedObjects ground_truth_msg; + autoware_perception_msgs::msg::TrackedObjects ground_truth_msg; ground_truth_msg.header = msg.header; previous_simulation_time_ = current_simulation_time; @@ -171,47 +171,47 @@ auto DetectionSensor::updat std::find(detected_objects.begin(), detected_objects.end(), status.name()) != detected_objects.end() and status.type().type() != traffic_simulator_msgs::EntityType_Enum::EntityType_Enum_EGO) { - autoware_auto_perception_msgs::msg::DetectedObject object; + autoware_perception_msgs::msg::DetectedObject object; switch (status.subtype().value()) { case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_UNKNOWN: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN)); break; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_CAR: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::CAR)); + autoware_perception_msgs::msg::ObjectClassification::CAR)); break; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRUCK: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK)); + autoware_perception_msgs::msg::ObjectClassification::TRUCK)); break; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BUS: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::BUS)); + autoware_perception_msgs::msg::ObjectClassification::BUS)); break; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_TRAILER: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER)); + autoware_perception_msgs::msg::ObjectClassification::TRAILER)); break; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_MOTORCYCLE: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE)); + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE)); object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; break; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_BICYCLE: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE)); + autoware_perception_msgs::msg::ObjectClassification::BICYCLE)); object.kinematics.orientation_availability = - autoware_auto_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; + autoware_perception_msgs::msg::DetectedObjectKinematics::SIGN_UNKNOWN; break; case traffic_simulator_msgs::EntitySubtype_Enum::EntitySubtype_Enum_PEDESTRIAN: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN)); + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN)); break; default: object.classification.push_back(makeObjectClassification( - autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN)); + autoware_perception_msgs::msg::ObjectClassification::UNKNOWN)); break; } @@ -240,9 +240,9 @@ auto DetectionSensor::updat static auto toTrackedObject = [&]( const std::string & name, - const autoware_auto_perception_msgs::msg::DetectedObject & detected_object) - -> autoware_auto_perception_msgs::msg::TrackedObject { - autoware_auto_perception_msgs::msg::TrackedObject tracked_object; + const autoware_perception_msgs::msg::DetectedObject & detected_object) + -> autoware_perception_msgs::msg::TrackedObject { + autoware_perception_msgs::msg::TrackedObject tracked_object; tracked_object.existence_probability = detected_object.existence_probability; tracked_object.classification = detected_object.classification; @@ -264,21 +264,21 @@ auto DetectionSensor::updat } } - static std::queue> + static std::queue> queue_objects; - static std::queue> + static std::queue> queue_ground_truth_objects; queue_objects.push(std::make_pair(msg, current_simulation_time)); queue_ground_truth_objects.push(std::make_pair(ground_truth_msg, current_simulation_time)); - static rclcpp::Publisher::SharedPtr + static rclcpp::Publisher::SharedPtr ground_truth_publisher = std::dynamic_pointer_cast< - rclcpp::Publisher>( + rclcpp::Publisher>( ground_truth_publisher_base_ptr_); - autoware_auto_perception_msgs::msg::DetectedObjects delayed_msg; - autoware_auto_perception_msgs::msg::TrackedObjects delayed_ground_truth_msg; + autoware_perception_msgs::msg::DetectedObjects delayed_msg; + autoware_perception_msgs::msg::TrackedObjects delayed_ground_truth_msg; if ( current_simulation_time - queue_objects.front().second >= @@ -295,7 +295,7 @@ auto DetectionSensor::updat queue_ground_truth_objects.pop(); } - autoware_auto_perception_msgs::msg::DetectedObjects noised_msg; + autoware_perception_msgs::msg::DetectedObjects noised_msg; noised_msg.header = delayed_msg.header; noised_msg.objects.reserve(delayed_msg.objects.size()); for (const auto & object : delayed_msg.objects) { diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index bc055a68410..b1e38a7c4b2 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -37,7 +37,6 @@ tf2_geometry_msgs tf2_ros tier4_debug_msgs - autoware_auto_perception_msgs autoware_perception_msgs autoware_map_msgs tinyxml2_vendor From 6562e1e64d98f7a5200be921775370d44cd92f1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Mon, 13 May 2024 16:52:36 +0200 Subject: [PATCH 21/49] Delete unused autoware_auto_system_msgs package --- docs/developer_guide/Communication.md | 2 +- docs/developer_guide/OpenSCENARIOSupport.md | 2 +- .../concealer/include/concealer/field_operator_application.hpp | 1 - .../field_operator_application_for_autoware_universe.hpp | 1 - external/concealer/package.xml | 1 - .../include/traffic_simulator/entity/ego_entity.hpp | 1 - 6 files changed, 2 insertions(+), 6 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index c35baf47bb9..94d473aedaf 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -12,7 +12,7 @@ | `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | | `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`autoware_planning_msgs/msg/PathWithLaneId`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_planning_msgs/msg/PathWithLaneId.msg) | | | `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | -| `/system/emergency/emergency_state` | [`autoware_auto_system_msgs/msg/EmergencyState`](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl) | Used in UserDefinedValueCondition : `currentEmergencyState` | +| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition : `currentEmergencyState` | | `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | | `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in /simulation/openscenario_interpreter | | `/api/external/get/rtc_status` | [`tier4_rtc_msgs::msg::CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | diff --git a/docs/developer_guide/OpenSCENARIOSupport.md b/docs/developer_guide/OpenSCENARIOSupport.md index 5ae9dc694b1..a21f42f9d71 100644 --- a/docs/developer_guide/OpenSCENARIOSupport.md +++ b/docs/developer_guide/OpenSCENARIOSupport.md @@ -169,7 +169,7 @@ See Reference for specific strings. | Name | Syntax | Description | |----------------------------|----------------------------------------------------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| | currentState | `.currentState` | Returns Autoware's [state](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | -| currentEmergencyState | `.currentEmergencyState` | Returns Autoware's [emergency state](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_system_msgs/msg/EmergencyState.idl). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | +| currentEmergencyState | `.currentEmergencyState` | Returns Autoware's [emergency state](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | | currentTurnIndicatorsState | `.currentTurnIndicatorsState` | Returns Autoware's [turn indicators state](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg). `` must be the name of Vehicle with ObjectController's property `isEgo` set to true. | | RelativeHeadingCondition | `RelativeHeadingCondition()`
`RelativeHeadingCondition(, , )` | Returns the relative angle to the lane heading. | diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index b0b06bd6104..db78ec51d0c 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -21,7 +21,6 @@ #include #include -#include #include #include #include diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 2582bdc5ec7..295252e6b81 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include diff --git a/external/concealer/package.xml b/external/concealer/package.xml index 05e3df71f8c..760a51db4bd 100644 --- a/external/concealer/package.xml +++ b/external/concealer/package.xml @@ -14,7 +14,6 @@ autoware_adapi_v1_msgs autoware_control_msgs autoware_perception_msgs - autoware_auto_system_msgs autoware_system_msgs autoware_vehicle_msgs diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp index a72ec7dcadf..2a24a75c961 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_SIMULATOR__ENTITY__EGO_ENTITY_HPP_ #include -#include #include #include #include From 20b64f69f612e6111828cc489d867025824c3bb9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 31 May 2024 13:45:11 +0900 Subject: [PATCH 22/49] feat: add control mode setting APIs to Autoware class --- external/concealer/include/concealer/autoware.hpp | 4 ++++ .../include/concealer/autoware_universe.hpp | 7 +++++++ external/concealer/src/autoware_universe.cpp | 12 +++++++++++- 3 files changed, 22 insertions(+), 1 deletion(-) diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp index ff27ba70d0c..185b0603958 100644 --- a/external/concealer/include/concealer/autoware.hpp +++ b/external/concealer/include/concealer/autoware.hpp @@ -73,6 +73,10 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster void; virtual auto rethrow() -> void; + + virtual auto setManualMode() -> void = 0; + + virtual auto setAutonomousMode() -> void = 0; }; } // namespace concealer diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 855e21b871e..e4a4b608c51 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -59,6 +59,9 @@ class AutowareUniverse : public Autoware std::atomic is_stop_requested = false; + std::atomic current_control_mode = + autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; + std::atomic is_thrown = false; std::exception_ptr thrown; @@ -91,6 +94,10 @@ class AutowareUniverse : public Autoware autoware_auto_vehicle_msgs::msg::GearCommand> override; auto getRouteLanelets() const -> std::vector; + + auto setManualMode() -> void override; + + auto setAutonomousMode() -> void override; }; } // namespace concealer diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index 6016eb0d322..e97771dfa0c 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -111,7 +111,7 @@ auto AutowareUniverse::updateVehicleState() -> void { setControlModeReport([this]() { autoware_auto_vehicle_msgs::msg::ControlModeReport message; - message.mode = autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; + message.mode = current_control_mode.load(); return message; }()); @@ -179,4 +179,14 @@ auto AutowareUniverse::getRouteLanelets() const -> std::vector } return ids; } + +auto AutowareUniverse::setManualMode() -> void +{ + current_control_mode.store(autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL); +} + +auto AutowareUniverse::setAutonomousMode() -> void +{ + current_control_mode.store(autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS); +} } // namespace concealer From 644f42c27fb90b707ce77a8c7474cdceb818788a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 31 May 2024 13:45:39 +0900 Subject: [PATCH 23/49] feat: use manual mode when follow trajectory action is enabled --- .../simple_sensor_simulator/src/simple_sensor_simulator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index ad7d92218e2..7f12645dd10 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -159,12 +159,14 @@ auto ScenarioSimulator::updateEntityStatus( if (isEgo(status.name())) { assert(ego_entity_simulation_ && "Ego is spawned but ego_entity_simulation_ is nullptr!"); if (req.overwrite_ego_status()) { + ego_entity_simulation_->autoware->setManualMode(); traffic_simulator_msgs::msg::EntityStatus ego_status_msg; simulation_interface::toMsg(status, ego_status_msg); ego_entity_simulation_->overwrite( ego_status_msg, current_scenario_time_ + step_time_, step_time_, req.npc_logic_started()); } else { + ego_entity_simulation_->autoware->setAutonomousMode(); ego_entity_simulation_->update( current_scenario_time_ + step_time_, step_time_, req.npc_logic_started()); } From f37bfd0818110c8201f5e4f975cecda745cd6cc1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 4 Jun 2024 14:11:55 +0900 Subject: [PATCH 24/49] feat: add requestAutowareControl to FieldOperatorApplicationFor class --- .../include/concealer/field_operator_application.hpp | 2 ++ ...ield_operator_application_for_autoware_universe.hpp | 7 ++++++- ...ield_operator_application_for_autoware_universe.cpp | 10 ++++++++++ simulation/traffic_simulator/src/entity/ego_entity.cpp | 1 + 4 files changed, 19 insertions(+), 1 deletion(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 75b1cd0a62b..51f8aa3f3c1 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -154,6 +154,8 @@ class FieldOperatorApplication : public rclcpp::Node virtual auto sendCooperateCommand(const std::string &, const std::string &) -> void = 0; virtual auto setVelocityLimit(double) -> void = 0; + + virtual auto requestAutowareControl(const bool) -> void = 0; }; } // namespace concealer diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index e9d14665928..fbc1693d104 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -43,6 +43,7 @@ #include #include #include +#include namespace concealer { @@ -72,6 +73,7 @@ class FieldOperatorApplicationFor ServiceWithValidation requestSetRoutePoints; ServiceWithValidation requestSetRtcAutoMode; ServiceWithValidation requestSetVelocityLimit; + ServiceWithValidation requestChangeAutowareControl; // clang-format on tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; @@ -132,7 +134,8 @@ class FieldOperatorApplicationFor // NOTE: /api/routing/set_route_points takes a long time to return. But the specified duration is not decided by any technical reasons. requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)), requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this), - requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this) + requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this), + requestChangeAutowareControl("/system/operation_mode/change_autoware_control", *this) // clang-format on { } @@ -171,6 +174,8 @@ class FieldOperatorApplicationFor auto sendCooperateCommand(const std::string &, const std::string &) -> void override; auto setVelocityLimit(double) -> void override; + + auto requestAutowareControl(const bool autoware_control) -> void override; }; } // namespace concealer diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index b86d4b3a657..dfc7a0a7875 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -481,6 +481,16 @@ auto FieldOperatorApplicationFor::requestAutoModeForCooperatio } } +auto FieldOperatorApplicationFor::requestAutowareContro( + const bool autoware_control) -> void +{ + task_queue.delay([this, autoware_control]() { + auto request = std::make_shared(); + request->autoware_control = autoware_control; + requestChangeAutowareControl(request); + }); +} + auto FieldOperatorApplicationFor::receiveEmergencyState( const tier4_external_api_msgs::msg::Emergency & message) -> void { diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e3a056c2dc8..c72e6111dd4 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -224,6 +224,7 @@ auto EgoEntity::requestFollowTrajectory( { polyline_trajectory_ = parameter; VehicleEntity::requestFollowTrajectory(parameter); + field_operator_application->requestAutowareControl(false); is_controlled_by_simulator_ = true; } From c3221a698bfdf8adb612a4fcce0d534284c5584a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 5 Jun 2024 11:44:41 +0900 Subject: [PATCH 25/49] fix: build errors --- .../src/field_operator_application_for_autoware_universe.cpp | 2 +- simulation/traffic_simulator/src/entity/ego_entity.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index dfc7a0a7875..0ee5b0c8785 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -481,7 +481,7 @@ auto FieldOperatorApplicationFor::requestAutoModeForCooperatio } } -auto FieldOperatorApplicationFor::requestAutowareContro( +auto FieldOperatorApplicationFor::requestAutowareControl( const bool autoware_control) -> void { task_queue.delay([this, autoware_control]() { diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index c72e6111dd4..10aa790ea65 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -159,6 +159,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) target_speed_ ? target_speed_.value() : status_.getTwist().linear.x)) { setStatus(CanonicalizedEntityStatus(*updated_status, hdmap_utils_ptr_)); } else { + field_operator_application->requestAutowareControl(true); is_controlled_by_simulator_ = false; } } From a1acf2284ea594bf23cb76a504503b57a8a6762e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 5 Jun 2024 11:45:17 +0900 Subject: [PATCH 26/49] feat: add new scenario for overriding with FollowTrajectoryAction --- ...ction.FollowTrajectoryAction-override.yaml | 235 ++++++++++++++++++ 1 file changed, 235 insertions(+) create mode 100644 test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-override.yaml diff --git a/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-override.yaml b/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-override.yaml new file mode 100644 index 00000000000..15420dc70cf --- /dev/null +++ b/test_runner/scenario_test_runner/scenario/RoutingAction.FollowTrajectoryAction-override.yaml @@ -0,0 +1,235 @@ +OpenSCENARIO: + FileHeader: + revMajor: 0 + revMinor: 0 + date: '1970-01-01T09:00:00+09:00' + description: '' + author: 'Kotaro Yoshimoto' + ParameterDeclarations: + ParameterDeclaration: [] + CatalogLocations: + VehicleCatalog: + Directory: + path: $(ros2 pkg prefix --share openscenario_experimental_catalog)/vehicle + RoadNetwork: + LogicFile: + filepath: $(ros2 pkg prefix --share kashiwanoha_map)/map + Entities: + ScenarioObject: + - name: ego + CatalogReference: &SAMPLE_VEHICLE + catalogName: sample_vehicle + entryName: sample_vehicle + ObjectController: + Controller: + name: '' + Properties: + Property: + - name: 'isEgo' + value: 'true' + - name: car_1 + CatalogReference: *SAMPLE_VEHICLE + - name: car_2 + CatalogReference: *SAMPLE_VEHICLE + Storyboard: + Init: + Actions: + Private: + - entityRef: ego + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 1 + offset: 0 + Orientation: &ORIENTATION_ZERO + type: relative + h: 0 + p: 0 + r: 0 + - RoutingAction: + AcquirePositionAction: + Position: &EGO_DESTINATION + LanePosition: + roadId: '' + laneId: '34507' + s: 50 + offset: 0 + Orientation: *ORIENTATION_ZERO + - entityRef: car_1 + PrivateAction: + - TeleportAction: + Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 40 + offset: -1.0 + Orientation: + type: relative + h: -1.57 + p: 0 + r: 0 + - LongitudinalAction: + SpeedAction: + SpeedActionDynamics: + dynamicsDimension: time + value: 0 + dynamicsShape: step + SpeedActionTarget: + AbsoluteTargetSpeed: + value: 0 + Story: + - name: '' + Act: + - name: '' + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: 'check stopped' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + SpeedCondition: + rule: lessThan + value: 3.0 + Action: + - name: 'follow_trajectory_override' + PrivateAction: + - RoutingAction: + FollowTrajectoryAction: + initialDistanceOffset: 1 + TimeReference: + Timing: + domainAbsoluteRelative: relative + offset: 0 + scale: 1 + TrajectoryFollowingMode: + followingMode: position + TrajectoryRef: + Trajectory: + closed: false + name: straight + Shape: + Polyline: + Vertex: + - Position: + LanePosition: + roadId: '' + laneId: '34513' + s: 40 + offset: 2 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34507' + s: 0 + offset: 0 + Orientation: *ORIENTATION_ZERO + - Position: + LanePosition: + roadId: '' + laneId: '34507' + s: 5 + offset: 0 + Orientation: *ORIENTATION_ZERO + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 10 + rule: greaterThan + - name: _EndCondition + ManeuverGroup: + - maximumExecutionCount: 1 + name: '' + Actors: + selectTriggeringEntities: false + EntityRef: + - entityRef: ego + Maneuver: + - name: '' + Event: + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + StoryboardElementStateCondition: + storyboardElementRef: follow_trajectory_override + storyboardElementType: action + state: completeState + - name: '' + delay: 0 + conditionEdge: none + ByEntityCondition: + TriggeringEntities: + triggeringEntitiesRule: any + EntityRef: + - entityRef: ego + EntityCondition: + ReachPositionCondition: + Position: *EGO_DESTINATION + tolerance: 1 + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitSuccess + - name: '' + priority: parallel + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 60 + rule: greaterThan + Action: + - name: '' + UserDefinedAction: + CustomCommandAction: + type: exitFailure + StartTrigger: + ConditionGroup: + - Condition: + - name: '' + delay: 0 + conditionEdge: none + ByValueCondition: + SimulationTimeCondition: + value: 0 + rule: greaterThan + StopTrigger: + ConditionGroup: [] From 6e6efbd40c62feffb2eae9179feb06cd376d47de Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 13:53:03 +0900 Subject: [PATCH 27/49] feat(traffic_simulator): support TrafficLightGroupArray in TrafficLightPublisher --- .../traffic_light_publisher.cpp | 45 +++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp index 422d6ccfd7f..191812572bb 100644 --- a/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp +++ b/simulation/traffic_simulator/src/traffic_lights/traffic_light_publisher.cpp @@ -12,11 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. +// This message will be deleted in the future +#if __has_include() #include +#endif + +#if __has_include() +#include +#endif + #include namespace traffic_simulator { +#if __has_include() template <> auto TrafficLightPublisher::publish( const rclcpp::Time & current_ros_time, @@ -50,4 +59,40 @@ auto TrafficLightPublisher::p } traffic_light_state_array_publisher_->publish(message); } +#endif + +#if __has_include() +template <> +auto TrafficLightPublisher::publish( + const rclcpp::Time & current_ros_time, + const simulation_api_schema::UpdateTrafficLightsRequest & request) -> void +{ + assert(hdmap_utils_ != nullptr); + + autoware_perception_msgs::msg::TrafficLightGroupArray message; + message.stamp = current_ros_time; + for (const auto & traffic_light : request.states()) { + auto relation_ids = + hdmap_utils_->getTrafficLightRegulatoryElementIDsFromTrafficLight(traffic_light.id()); + + for (auto relation_id : relation_ids) { + // skip if the traffic light has no bulbs + if (not traffic_light.traffic_light_status().empty()) { + using TrafficLightGroupType = autoware_perception_msgs::msg::TrafficLightGroup; + TrafficLightGroupType traffic_light_group_message; + traffic_light_group_message.traffic_light_group_id = relation_id; + + for (auto bulb_status : traffic_light.traffic_light_status()) { + using TrafficLightBulbType = autoware_perception_msgs::msg::TrafficLightElement; + TrafficLightBulbType light_bulb_message; + simulation_interface::toMsg(bulb_status, light_bulb_message); + traffic_light_group_message.elements.push_back(light_bulb_message); + } + message.traffic_light_groups.push_back(traffic_light_group_message); + } + } + } + traffic_light_state_array_publisher_->publish(message); +} +#endif } // namespace traffic_simulator From 282817ae2462c101618420ecfe20f9e8fa43453a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 13:59:23 +0900 Subject: [PATCH 28/49] feat(traffic_simulator): support TrafficLightGroupArray in SimpleSensorSimulator --- .../sensor_simulation/sensor_simulation.hpp | 32 +++++++++++++++---- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 5e0e5c54536..458a0d1bdef 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -19,7 +19,6 @@ #include #include -#include #include #include #include @@ -29,6 +28,14 @@ #include #include +// This message will be deleted in the future +#if __has_include() +#include +#endif +#if __has_include() +#include +#endif + namespace simple_sensor_simulator { class SensorSimulation @@ -95,20 +102,31 @@ class SensorSimulation const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration, rclcpp::Node & node, std::shared_ptr hdmap_utils) -> void { - if (configuration.architecture_type() == "awf/universe") { + bool has_attached = false; +#if __has_include() + if (configuration.architecture_type() == "awf/universe/20230906") { using Message = autoware_perception_msgs::msg::TrafficSignalArray; traffic_lights_detectors_.push_back(std::make_unique( std::make_shared>( - "/perception/traffic_light_recognition/traffic_signals", &node, hdmap_utils))); - } else if (configuration.architecture_type() >= "awf/universe/20230906") { - using Message = autoware_perception_msgs::msg::TrafficSignalArray; + "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils))); + has_attached = true; + } +#endif + +#if __has_include() + if (configuration.architecture_type() == "awf/universe/20240605") { + using Message = autoware_perception_msgs::msg::TrafficLightGroupArray; traffic_lights_detectors_.push_back(std::make_unique( std::make_shared>( "/perception/traffic_light_recognition/internal/traffic_signals", &node, hdmap_utils))); - } else { + has_attached = true; + } +#endif + + if (not has_attached) { std::stringstream ss; ss << "Unexpected architecture_type " << std::quoted(configuration.architecture_type()) - << " given."; + << " given for traffic light."; throw std::runtime_error(ss.str()); } } From 8ad7666f1d3823c54da5917465044e3f13063339 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 14:48:52 +0900 Subject: [PATCH 29/49] chore: delete autoware_auto_msgs from dependencies --- dependency_humble.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/dependency_humble.repos b/dependency_humble.repos index 831241efebb..8e87c77daed 100644 --- a/dependency_humble.repos +++ b/dependency_humble.repos @@ -7,10 +7,6 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main - autoware_auto_msgs: - type: git - url: https://github.com/tier4/autoware_auto_msgs.git - version: tier4/main autoware_common: type: git url: https://github.com/autowarefoundation/autoware_common.git From a043751268339dbd71ed65f62f78514f6138de69 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 14:57:01 +0900 Subject: [PATCH 30/49] feat(traffic_simulator): support TrafficLightGroup for V2I traffic lights --- .../entity/entity_manager.hpp | 28 ++++++++++++++++--- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 9a568d40ff6..209861e3152 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -19,7 +19,13 @@ #include #include +#if __has_include() #include +#endif +#if __has_include() +#include +#endif + #include #include #include @@ -142,13 +148,27 @@ class EntityManager template auto makeV2ITrafficLightPublisher(Ts &&... xs) -> std::shared_ptr { - if (const auto architecture_type = - getParameter("architecture_type", "awf/universe"); - architecture_type.find("awf/universe") != std::string::npos) { + const auto architecture_type = getParameter("architecture_type"); + bool has_made_publisher = false; + +#if __has_include() + if (architecture_type == "awf/universe/20230906") { return std::make_shared< TrafficLightPublisher>( std::forward(xs)...); - } else { + has_made_publisher = true; + } +#endif +#if __has_include() + if (architecture_type == "awf/universe/20240605") { + return std::make_shared< + TrafficLightPublisher>( + std::forward(xs)...); + has_made_publisher = true; + } +#endif + + if (not has_made_publisher) { throw common::SemanticError( "Unexpected architecture_type ", std::quoted(architecture_type), " given for V2I traffic lights simulation."); From 9010b5d39f86a42dfbeb199789d45bf638b4ecdf Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 14:58:00 +0900 Subject: [PATCH 31/49] chore(scenario_test_runner): add new architecture_type and delete old one in launch file --- .../launch/scenario_test_runner.launch.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index cc5b6328f5d..6221377d4fc 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -33,7 +33,7 @@ def architecture_types(): - return ["awf/universe", "awf/universe/20230906"] + return ["awf/universe/20230906", "awf/universe/2023605"] def default_autoware_launch_package_of(architecture_type): @@ -42,8 +42,8 @@ def default_autoware_launch_package_of(architecture_type): f"architecture_type := {architecture_type} is not supported. Choose one of {architecture_types()}." ) return { - "awf/universe": "autoware_launch", "awf/universe/20230906": "autoware_launch", + "awf/universe/20230605": "autoware_launch", }[architecture_type] @@ -53,14 +53,14 @@ def default_autoware_launch_file_of(architecture_type): f"architecture_type := {architecture_type} is not supported. Choose one of {architecture_types()}." ) return { - "awf/universe": "planning_simulator.launch.xml", "awf/universe/20230906": "planning_simulator.launch.xml", + "awf/universe/202605": "planning_simulator.launch.xml", }[architecture_type] def launch_setup(context, *args, **kwargs): # fmt: off - architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20230906") + architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20240605") autoware_launch_file = LaunchConfiguration("autoware_launch_file", default=default_autoware_launch_file_of(architecture_type.perform(context))) autoware_launch_package = LaunchConfiguration("autoware_launch_package", default=default_autoware_launch_package_of(architecture_type.perform(context))) consider_acceleration_by_road_slope = LaunchConfiguration("consider_acceleration_by_road_slope", default=False) From d9ac32596c8df2adf882414fd6a523822e36387d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 15:28:33 +0900 Subject: [PATCH 32/49] fix: correct new architecture_type name --- .../launch/scenario_test_runner.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 6221377d4fc..f57a6027998 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -33,7 +33,7 @@ def architecture_types(): - return ["awf/universe/20230906", "awf/universe/2023605"] + return ["awf/universe/20230906", "awf/universe/20240605"] def default_autoware_launch_package_of(architecture_type): @@ -43,7 +43,7 @@ def default_autoware_launch_package_of(architecture_type): ) return { "awf/universe/20230906": "autoware_launch", - "awf/universe/20230605": "autoware_launch", + "awf/universe/20240605": "autoware_launch", }[architecture_type] @@ -54,7 +54,7 @@ def default_autoware_launch_file_of(architecture_type): ) return { "awf/universe/20230906": "planning_simulator.launch.xml", - "awf/universe/202605": "planning_simulator.launch.xml", + "awf/universe/20240605": "planning_simulator.launch.xml", }[architecture_type] From c8f86b7693f62dd2016cb4511c0cb870043f0136 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 15:29:29 +0900 Subject: [PATCH 33/49] doc: update Communication.md for new architecture_type --- docs/developer_guide/Communication.md | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 94d473aedaf..4ce88f8f983 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -27,19 +27,21 @@ | `/localization/acceleration` | [`geometry_msgs::msg::AccelWithCovarianceStamped`](https://github.com/ros2/common_interfaces/blob/rolling/geometry_msgs/msg/AccelWithCovarianceStamped.msg) | | | `/planning/mission_planning/checkpoint` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | | `/planning/mission_planning/goal` | [`geometry_msgs/msg/PoseStamped`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/PoseStamped.msg) | | -| `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | -| `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | -| `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | -| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | -| `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | -| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | -| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/vehicle/status/control_mode` | [`autoware_vehicle_msgs/msg/ControlModeReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/ControlModeReport.msg) | | +| `/vehicle/status/gear_status` | [`autoware_vehicle_msgs/msg/GearReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/GearReport.msg) | | +| `/vehicle/status/steering_status` | [`autoware_vehicle_msgs/msg/SteeringReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/SteeringReport.msg) | | +| `/vehicle/status/turn_indicators_status` | [`autoware_vehicle_msgs/msg/TurnIndicatorsReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/TurnIndicatorsReport.msg) | | +| `/vehicle/status/velocity_status` | [`autoware_vehicle_msgs/msg/VelocityReport`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_vehicle_msgs/msg/VelocityReport.msg) | | +| `/perception/object_recognition/detection/objects` | [`autoware_perception_msgs/msg/DetectedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/DetectedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | +| `/perception/object_recognition/ground_truth/objects` | [`autoware_perception_msgs/msg/TrackedObjects`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrackedObjects.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#object-detection-results-simulation) | | `/perception/obstacle_segmentation/pointcloud` | [`sensor_msgs/msg/PointCloud2`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/PointCloud2.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#lidar-simulation) | | `/perception/occupancy_grid_map/map` | [`nav_msgs/msg/OccupancyGrid`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/OccupancyGrid.msg) | [Simulated by simple_sensor_simulator](https://tier4.github.io/scenario_simulator_v2-docs/developer_guide/SimpleSensorSimulator/#occupancy-grid-sensor-simulation) | -| `/perception/traffic_light_recognition/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | for `architecture_type` equal to `awf/universe` | -| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | for `architecture_type` greater or equal to `awf/universe/20230906` | -| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | | -| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/perception/traffic_light_recognition/internal/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Optical traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/perception/traffic_light_recognition/external/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficSignalArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficSignalArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20230906` | +| `/v2x/traffic_signals` | [`autoware_perception_msgs/msg/TrafficLightGroupArray`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_perception_msgs/msg/TrafficLightGroupArray.msg) | Additional V2I traffic light interface for `architecture_type` equal to `awf/universe/20240605` | [//]: # (| /rosout | rcl_interfaces/msg/Log | | |) [//]: # (| /tf | tf2_msgs/msg/TFMessage | | |) From 6b1a6a3bbb01c14eab7dd6aa0ac535f0e72e339e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 10 Jun 2024 15:35:50 +0900 Subject: [PATCH 34/49] doc: update Communication.md for unupdated link --- docs/developer_guide/Communication.md | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/docs/developer_guide/Communication.md b/docs/developer_guide/Communication.md index 4ce88f8f983..1ce73c3a150 100644 --- a/docs/developer_guide/Communication.md +++ b/docs/developer_guide/Communication.md @@ -3,19 +3,19 @@ ### Subscribers -| topic | type | note | -|--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| -| `/autoware/state` | [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | -| `/control/command/control_cmd` | [`autoware_control_msgs/msg/Control`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_control_msgs/msg/Control.msg) | | -| `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/GearCommand.msg) | | -| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | -| `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | -| `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`autoware_planning_msgs/msg/PathWithLaneId`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_planning_msgs/msg/PathWithLaneId.msg) | | -| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | -| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition : `currentEmergencyState` | -| `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | -| `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in /simulation/openscenario_interpreter | -| `/api/external/get/rtc_status` | [`tier4_rtc_msgs::msg::CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | +| topic | type | note | +|--------------------------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------|-----------------------------------------------------------------------| +| `/autoware/state` | [`autoware_system_msgs/msg/AutowareState`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_system_msgs/msg/AutowareState.msg) | Used in UserDefinedValueCondition : `currentAutowareState` | +| `/control/command/control_cmd` | [`autoware_control_msgs/msg/Control`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_control_msgs/msg/Control.msg) | | +| `/control/command/gear_cmd` | [`autoware_vehicle_msgs/msg/GearCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/GearCommand.msg) | | +| `/control/command/turn_indicators_cmd` | [`autoware_vehicle_msgs/msg/TurnIndicatorsCommand`](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_vehicle_msgs/msg/TurnIndicatorsCommand.msg) | Used in UserDefinedValueCondition : `currentTurnIndicatorsState` | +| `/parameter_events` | [`rcl_interfaces/msg/ParameterEvent`](https://github.com/ros2/rcl_interfaces/blob/master/rcl_interfaces/msg/ParameterEvent.msg) | | +| `/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id` | [`tier4_planning_msgs/msg/PathWithLaneId`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/PathWithLaneId.msg) | | +| `/api/iv_msgs/planning/scenario_planning/trajectory` | [`tier4_planning_msgs/msg/Trajectory`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_planning_msgs/msg/Trajectory.msg) | | +| `/api/external/get/emergency` | [`tier4_external_api_msgs/msg/Emergency`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_external_api_msgs/msg/Emergency.msg) | Used in UserDefinedValueCondition : `currentEmergencyState` | +| `/api/fail_safe/mrm_state` | [`autoware_adapi_v1_msgs/msg/MrmState`](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/system/msg/MrmState.msg) | Used in UserDefinedValueCondition : `currentMinimumRiskManeuverState` | +| `/planning/scenario_planning/motion_velocity_optimizer/closest_jerk` | [`tier4_debug_msgs/msg/Float32Stamped`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_debug_msgs/msg/Float32Stamped.msg) | Used in /simulation/openscenario_interpreter | +| `/api/external/get/rtc_status` | [`tier4_rtc_msgs::msg::CooperateStatusArray`](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_rtc_msgs/msg/CooperateStatusArray.msg) | | ### Publishers From 4e53be5a23654cdaa5694534617e9d6c9e7a84eb Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 11 Jun 2024 17:06:46 +0900 Subject: [PATCH 35/49] chore: stop using autoware_auto_msgs --- external/concealer/include/concealer/autoware_universe.hpp | 2 +- external/concealer/src/autoware_universe.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 28d67be01eb..adb92e0c3fd 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -60,7 +60,7 @@ class AutowareUniverse : public Autoware std::atomic is_stop_requested = false; std::atomic current_control_mode = - autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; + autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS; std::atomic is_thrown = false; diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index a791a2fdd1c..fe5bb70781e 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -182,11 +182,11 @@ auto AutowareUniverse::getRouteLanelets() const -> std::vector auto AutowareUniverse::setManualMode() -> void { - current_control_mode.store(autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL); + current_control_mode.store(autoware_vehicle_msgs::msg::ControlModeReport::MANUAL); } auto AutowareUniverse::setAutonomousMode() -> void { - current_control_mode.store(autoware_auto_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS); + current_control_mode.store(autoware_vehicle_msgs::msg::ControlModeReport::AUTONOMOUS); } } // namespace concealer From 5dec62b0a24eee6e7beb0db51c1e75bcb38d6e3c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 19 Jun 2024 18:16:50 +0900 Subject: [PATCH 36/49] feat: use ADAPI for change autoware control --- .../concealer/field_operator_application.hpp | 4 +++- ..._operator_application_for_autoware_universe.hpp | 11 ++++++++--- ..._operator_application_for_autoware_universe.cpp | 14 ++++++++++---- .../traffic_simulator/src/entity/ego_entity.cpp | 4 ++-- 4 files changed, 23 insertions(+), 10 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 51f8aa3f3c1..b3aa58e2338 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -155,7 +155,9 @@ class FieldOperatorApplication : public rclcpp::Node virtual auto setVelocityLimit(double) -> void = 0; - virtual auto requestAutowareControl(const bool) -> void = 0; + virtual auto enableAutowareControl() -> void = 0; + + virtual auto disableAutowareControl() -> void = 0; }; } // namespace concealer diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index fbc1693d104..b01d847eac6 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -20,6 +20,7 @@ #endif #include +#include #include #include #include @@ -73,7 +74,8 @@ class FieldOperatorApplicationFor ServiceWithValidation requestSetRoutePoints; ServiceWithValidation requestSetRtcAutoMode; ServiceWithValidation requestSetVelocityLimit; - ServiceWithValidation requestChangeAutowareControl; + ServiceWithValidation requestEnableAutowareControl; + ServiceWithValidation requestDisableAutowareControl; // clang-format on tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; @@ -135,7 +137,8 @@ class FieldOperatorApplicationFor requestSetRoutePoints("/api/routing/set_route_points", *this, std::chrono::seconds(10)), requestSetRtcAutoMode("/api/external/set/rtc_auto_mode", *this), requestSetVelocityLimit("/api/autoware/set/velocity_limit", *this), - requestChangeAutowareControl("/system/operation_mode/change_autoware_control", *this) + requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this), + requestDisableAutowareControl("/api/operation_mode/disable_autoware_control", *this) // clang-format on { } @@ -175,7 +178,9 @@ class FieldOperatorApplicationFor auto setVelocityLimit(double) -> void override; - auto requestAutowareControl(const bool autoware_control) -> void override; + auto enableAutowareControl() -> void override; + + auto disableAutowareControl() -> void override; }; } // namespace concealer diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 0ee5b0c8785..fd51846f31e 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -481,13 +481,19 @@ auto FieldOperatorApplicationFor::requestAutoModeForCooperatio } } -auto FieldOperatorApplicationFor::requestAutowareControl( - const bool autoware_control) -> void +auto FieldOperatorApplicationFor::enableAutowareControl() -> void { task_queue.delay([this, autoware_control]() { auto request = std::make_shared(); - request->autoware_control = autoware_control; - requestChangeAutowareControl(request); + requestEnableAutowareControl(request); + }); +} + +auto FieldOperatorApplicationFor::disableAutowareControl() -> void +{ + task_queue.delay([this, autoware_control]() { + auto request = std::make_shared(); + requestDisableAutowareControl(request); }); } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 291c5fff4cd..937dff24420 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -150,7 +150,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) target_speed_ ? target_speed_.value() : status_.getTwist().linear.x)) { setStatus(CanonicalizedEntityStatus(*updated_status, hdmap_utils_ptr_)); } else { - field_operator_application->requestAutowareControl(true); + field_operator_application->enableAutowareControl(); is_controlled_by_simulator_ = false; } } @@ -216,7 +216,7 @@ auto EgoEntity::requestFollowTrajectory( { polyline_trajectory_ = parameter; VehicleEntity::requestFollowTrajectory(parameter); - field_operator_application->requestAutowareControl(false); + field_operator_application->disableAutowareControl(); is_controlled_by_simulator_ = true; } From 91a981207076c11bfc9213b035bdcae4bc2626dd Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 19 Jun 2024 18:19:39 +0900 Subject: [PATCH 37/49] feat: enrich error message of ServiceWithValidation for ResponseStatus of ADAPI --- .../include/concealer/service_with_validation.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index f083724d722..62421299c85 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -116,11 +116,12 @@ class ServiceWithValidation return; } else { RCLCPP_ERROR_STREAM( - logger, service_name - << " service request was accepted, but ResponseStatus::success is false " - << (service_call_status.message.empty() - ? "" - : " (" + service_call_status.message + ")")); + logger, service_name << " service request was accepted, but " + "ResponseStatus::success is false with error code: " + << service_call_status.code << ", and message: " + << (service_call_status.message.empty() + ? "" + : " (" + service_call_status.message + ")")); } } else { RCLCPP_INFO_STREAM(logger, service_name << " service request has been accepted."); From 67b036686ab6cfc3a770390392221884c86d7f5a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 21 Jun 2024 15:28:26 +0900 Subject: [PATCH 38/49] refactor: delete unused include --- .../field_operator_application_for_autoware_universe.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index 88caee0bd35..d9e70bd4fa2 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -43,7 +43,6 @@ #include #include #include -#include namespace concealer { From 4708462bc6bf0b9777f4d73dc8e64405a7de28a2 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 21 Jun 2024 17:00:57 +0900 Subject: [PATCH 39/49] fix: fix build errors --- .../field_operator_application_for_autoware_universe.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 176157d0d88..8b909f8a457 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -483,16 +483,16 @@ auto FieldOperatorApplicationFor::requestAutoModeForCooperatio auto FieldOperatorApplicationFor::enableAutowareControl() -> void { - task_queue.delay([this, autoware_control]() { - auto request = std::make_shared(); + task_queue.delay([this]() { + auto request = std::make_shared(); requestEnableAutowareControl(request); }); } auto FieldOperatorApplicationFor::disableAutowareControl() -> void { - task_queue.delay([this, autoware_control]() { - auto request = std::make_shared(); + task_queue.delay([this]() { + auto request = std::make_shared(); requestDisableAutowareControl(request); }); } From 904cf45771d7027b9395be22bcbdeef2d67d4683 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 24 Jun 2024 14:48:26 +0900 Subject: [PATCH 40/49] chore: apply formatter --- external/concealer/include/concealer/autoware.hpp | 5 ++--- .../concealer/include/concealer/autoware_universe.hpp | 5 ++--- ...ield_operator_application_for_autoware_universe.hpp | 2 +- external/concealer/src/autoware.cpp | 6 ++---- external/concealer/src/autoware_universe.cpp | 10 +++------- external/concealer/src/field_operator_application.cpp | 7 +++---- 6 files changed, 13 insertions(+), 22 deletions(-) diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp index 6bbbf6060cd..4d9c6bb5449 100644 --- a/external/concealer/include/concealer/autoware.hpp +++ b/external/concealer/include/concealer/autoware.hpp @@ -60,9 +60,8 @@ class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster autoware_vehicle_msgs::msg::TurnIndicatorsCommand; - virtual auto getVehicleCommand() const -> std::tuple< - autoware_control_msgs::msg::Control, - autoware_vehicle_msgs::msg::GearCommand> = 0; + virtual auto getVehicleCommand() const + -> std::tuple = 0; virtual auto getRouteLanelets() const -> std::vector = 0; diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index adb92e0c3fd..39d4b3f576b 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -16,7 +16,6 @@ #define CONCEALER__AUTOWARE_UNIVERSE_HPP_ #include -#include #include #include #include @@ -27,6 +26,7 @@ #include #include #include +#include namespace concealer { @@ -90,8 +90,7 @@ class AutowareUniverse : public Autoware auto getGearSign() const -> double override; auto getVehicleCommand() const -> std::tuple< - autoware_control_msgs::msg::Control, - autoware_vehicle_msgs::msg::GearCommand> override; + autoware_control_msgs::msg::Control, autoware_vehicle_msgs::msg::GearCommand> override; auto getRouteLanelets() const -> std::vector; diff --git a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp index d9e70bd4fa2..07b45058f30 100644 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ b/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp @@ -26,7 +26,6 @@ #include #include #include -#include #include #include #include @@ -38,6 +37,7 @@ #include #include #include +#include #include #include #include diff --git a/external/concealer/src/autoware.cpp b/external/concealer/src/autoware.cpp index 203e8a259c5..f29cbd93e2d 100644 --- a/external/concealer/src/autoware.cpp +++ b/external/concealer/src/autoware.cpp @@ -44,13 +44,11 @@ auto Autoware::set(const geometry_msgs::msg::Twist & twist) -> void { current_tw auto Autoware::set(const geometry_msgs::msg::Pose & pose) -> void { current_pose.store(pose); } -auto Autoware::getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand +auto Autoware::getTurnIndicatorsCommand() const -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand { static auto turn_indicators_command = []() { autoware_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; - turn_indicators_command.command = - autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; + turn_indicators_command.command = autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; return turn_indicators_command; }(); turn_indicators_command.stamp = now(); diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index fe5bb70781e..3afcdf6b22f 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -68,10 +68,7 @@ auto AutowareUniverse::getAcceleration() const -> double return getCommand().longitudinal.acceleration; } -auto AutowareUniverse::getVelocity() const -> double -{ - return getCommand().longitudinal.velocity; -} +auto AutowareUniverse::getVelocity() const -> double { return getCommand().longitudinal.velocity; } auto AutowareUniverse::getSteeringAngle() const -> double { @@ -164,9 +161,8 @@ auto AutowareUniverse::getGearSign() const -> double : 1.0; } -auto AutowareUniverse::getVehicleCommand() const -> std::tuple< - autoware_control_msgs::msg::Control, - autoware_vehicle_msgs::msg::GearCommand> +auto AutowareUniverse::getVehicleCommand() const + -> std::tuple { return std::make_tuple(getCommand(), getGearCommand()); } diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 4f75f6e6fe7..ba6ae6c16b9 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -151,8 +151,7 @@ auto FieldOperatorApplication::getTurnIndicatorsCommand() const { static auto turn_indicators_command = []() { autoware_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; - turn_indicators_command.command = - autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; + turn_indicators_command.command = autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; return turn_indicators_command; }(); turn_indicators_command.stamp = now(); @@ -168,9 +167,9 @@ auto operator<<( std::ostream & out, const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) -> std::ostream & { -#define CASE(IDENTIFIER) \ +#define CASE(IDENTIFIER) \ case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER: \ - out << #IDENTIFIER; \ + out << #IDENTIFIER; \ break switch (message.command) { From a3591f038b30ca35f9188d552556a482a2ffbe17 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 25 Jun 2024 13:57:03 +0900 Subject: [PATCH 41/49] add autoware_ prefix Signed-off-by: Yutaka Kondo --- .../traffic_simulator/hdmap_utils/hdmap_utils.hpp | 6 +++--- simulation/traffic_simulator/package.xml | 2 +- .../src/hdmap_utils/hdmap_utils.cpp | 12 ++++++------ test_runner/random_test_runner/src/lanelet_utils.cpp | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 4ad96b44ca8..bce0dfa4deb 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -35,9 +35,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index e4d4d6d6e88..ae6771a6c4a 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -18,7 +18,7 @@ geographic_msgs geometry_msgs lanelet2_core - lanelet2_extension + autoware_lanelet2_extension lanelet2_io lanelet2_matching lanelet2_projection diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 0c692bcd14c..439c988c391 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -36,12 +36,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include diff --git a/test_runner/random_test_runner/src/lanelet_utils.cpp b/test_runner/random_test_runner/src/lanelet_utils.cpp index aeaf2e85e07..feb2e9d83e0 100644 --- a/test_runner/random_test_runner/src/lanelet_utils.cpp +++ b/test_runner/random_test_runner/src/lanelet_utils.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include From 6501e633809728222ca9fc515c5647d3d98859e2 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 25 Jun 2024 13:59:44 +0900 Subject: [PATCH 42/49] update repos Signed-off-by: Yutaka Kondo --- dependency_humble.repos | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/dependency_humble.repos b/dependency_humble.repos index 8e87c77daed..018661ea2b7 100644 --- a/dependency_humble.repos +++ b/dependency_humble.repos @@ -7,9 +7,17 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main - autoware_common: + autoware_cmake: type: git - url: https://github.com/autowarefoundation/autoware_common.git + url: https://github.com/autowarefoundation/autoware_cmake.git + version: main + autoware_utils: + type: git + url: https://github.com/autowarefoundation/autoware_utils.git + version: main + autoware_lanelet2_extension: + type: git + url: https://github.com/autowarefoundation/autoware_lanelet2_extension.git version: main color_names: type: git From a7c04682f82648dfa36e74e5ea75558fe60f1db0 Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 25 Jun 2024 16:31:05 +0900 Subject: [PATCH 43/49] re-add autoware_common Signed-off-by: Yutaka Kondo --- dependency_humble.repos | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/dependency_humble.repos b/dependency_humble.repos index 018661ea2b7..055e9c614e6 100644 --- a/dependency_humble.repos +++ b/dependency_humble.repos @@ -7,6 +7,11 @@ repositories: type: git url: https://github.com/autowarefoundation/autoware_msgs.git version: main + # TODO(youtalk): Remove autoware_common when https://github.com/autowarefoundation/autoware/issues/4911 is closed + autoware_common: + type: git + url: https://github.com/autowarefoundation/autoware_common.git + version: remove-autoware-cmake-utils autoware_cmake: type: git url: https://github.com/autowarefoundation/autoware_cmake.git From 17f873bf561d59c43db14a6c2bb6f552e03d51aa Mon Sep 17 00:00:00 2001 From: Yutaka Kondo Date: Tue, 25 Jun 2024 17:05:26 +0900 Subject: [PATCH 44/49] Revert "add autoware_ prefix" This reverts commit a3591f038b30ca35f9188d552556a482a2ffbe17. --- .../traffic_simulator/hdmap_utils/hdmap_utils.hpp | 6 +++--- simulation/traffic_simulator/package.xml | 2 +- .../src/hdmap_utils/hdmap_utils.cpp | 12 ++++++------ test_runner/random_test_runner/src/lanelet_utils.cpp | 2 +- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index bce0dfa4deb..4ad96b44ca8 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -35,9 +35,9 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include diff --git a/simulation/traffic_simulator/package.xml b/simulation/traffic_simulator/package.xml index ae6771a6c4a..e4d4d6d6e88 100644 --- a/simulation/traffic_simulator/package.xml +++ b/simulation/traffic_simulator/package.xml @@ -18,7 +18,7 @@ geographic_msgs geometry_msgs lanelet2_core - autoware_lanelet2_extension + lanelet2_extension lanelet2_io lanelet2_matching lanelet2_projection diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 439c988c391..0c692bcd14c 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -36,12 +36,12 @@ #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include #include diff --git a/test_runner/random_test_runner/src/lanelet_utils.cpp b/test_runner/random_test_runner/src/lanelet_utils.cpp index feb2e9d83e0..aeaf2e85e07 100644 --- a/test_runner/random_test_runner/src/lanelet_utils.cpp +++ b/test_runner/random_test_runner/src/lanelet_utils.cpp @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include From 4776822ffa8f6e15bbf71af6cf77b4c82b170632 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Thu, 4 Jul 2024 10:49:52 +0200 Subject: [PATCH 45/49] Fix SpellCheck GitHub Action --- .../simulation_interface/proto/autoware_control_msgs.proto | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/simulation/simulation_interface/proto/autoware_control_msgs.proto b/simulation/simulation_interface/proto/autoware_control_msgs.proto index 4ae0543c96c..1a760f54da4 100644 --- a/simulation/simulation_interface/proto/autoware_control_msgs.proto +++ b/simulation/simulation_interface/proto/autoware_control_msgs.proto @@ -6,7 +6,7 @@ package autoware_control_msgs; message Lateral { builtin_interfaces.Time stamp = 1; - builtin_interfaces.Time cotrol_time = 2; + builtin_interfaces.Time control_time = 2; float steering_tire_angle = 3; float steering_tire_rotation_rate = 4; bool is_defined_steering_tire_rotation_rate = 5; @@ -14,7 +14,7 @@ message Lateral { message Longitudinal { builtin_interfaces.Time stamp = 1; - builtin_interfaces.Time cotrol_time = 2; + builtin_interfaces.Time control_time = 2; float velocity = 3; float acceleration = 4; float jerk = 5; @@ -24,7 +24,7 @@ message Longitudinal { message Control { builtin_interfaces.Time stamp = 1; - builtin_interfaces.Time cotrol_time = 2; + builtin_interfaces.Time control_time = 2; Lateral lateral = 3; Longitudinal longitudinal = 4; } From 0d2a86345757ca242e2feb1e364d515ae2dd9f2e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Thu, 4 Jul 2024 13:11:34 +0200 Subject: [PATCH 46/49] Apply clang format --- .../src/timeout.cpp | 4 +--- .../detection_sensor/detection_sensor.cpp | 6 ++---- .../simulation_interface/conversions.hpp | 13 ++++--------- .../simulation_interface/src/conversions.cpp | 19 +++++++------------ .../test/expect_equal_macros.hpp | 4 ++-- 5 files changed, 16 insertions(+), 30 deletions(-) diff --git a/openscenario/openscenario_interpreter_example/src/timeout.cpp b/openscenario/openscenario_interpreter_example/src/timeout.cpp index e2bf4571e15..55738bc2415 100644 --- a/openscenario/openscenario_interpreter_example/src/timeout.cpp +++ b/openscenario/openscenario_interpreter_example/src/timeout.cpp @@ -89,9 +89,7 @@ int main(const int argc, char const * const * const argv) auto subscription = node->create_subscription( "/autoware/state", rclcpp::QoS(1).reliable(), - [&](const autoware_system_msgs::msg::AutowareState::SharedPtr message) { - status = *message; - }); + [&](const autoware_system_msgs::msg::AutowareState::SharedPtr message) { status = *message; }); auto publisher = node->create_publisher("/timeout", rclcpp::QoS(1).reliable()); diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp index 15e54a088b9..dc6c1837f0d 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/detection_sensor/detection_sensor.cpp @@ -250,8 +250,7 @@ struct DefaultNoiseApplicator auto operator=(DefaultNoiseApplicator &&) = delete; - auto operator()(autoware_perception_msgs::msg::DetectedObjects detected_objects) - -> decltype(auto) + auto operator()(autoware_perception_msgs::msg::DetectedObjects detected_objects) -> decltype(auto) { auto position_noise_distribution = std::normal_distribution<>(0.0, detection_sensor_configuration.pos_noise_stddev()); @@ -330,8 +329,7 @@ auto DetectionSensor::update( for (const auto & status : statuses) { if (is_in_range(status)) { - const auto detected_object = - make(status); + const auto detected_object = make(status); detected_objects.objects.push_back(detected_object); ground_truth_objects.objects.push_back( make(status, detected_object)); diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 5f8b041fc28..7668d846c9e 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -180,9 +180,7 @@ DEFINE_CONVERSION(autoware_vehicle_msgs, GearCommand); #undef DEFINE_CONVERSION auto toProto( - const std::tuple< - autoware_control_msgs::msg::Control, - autoware_vehicle_msgs::msg::GearCommand> &, + const std::tuple &, traffic_simulator_msgs::VehicleCommand &) -> void; template @@ -192,8 +190,7 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr - { + auto convert_color = [](auto color) constexpr { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -208,8 +205,7 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr - { + auto convert_shape = [](auto shape) constexpr { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -237,8 +233,7 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr - { + auto convert_status = [](auto status) constexpr { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; diff --git a/simulation/simulation_interface/src/conversions.cpp b/simulation/simulation_interface/src/conversions.cpp index b124363e67e..5d006d319e9 100644 --- a/simulation/simulation_interface/src/conversions.cpp +++ b/simulation/simulation_interface/src/conversions.cpp @@ -490,8 +490,7 @@ void toMsg(const std_msgs::Header & proto, std_msgs::msg::Header & header) } void toProto( - const autoware_control_msgs::msg::Lateral & message, - autoware_control_msgs::Lateral & proto) + const autoware_control_msgs::msg::Lateral & message, autoware_control_msgs::Lateral & proto) { toProto(message.stamp, *proto.mutable_stamp()); proto.set_steering_tire_angle(message.steering_tire_angle); @@ -499,8 +498,7 @@ void toProto( } void toMsg( - const autoware_control_msgs::Lateral & proto, - autoware_control_msgs::msg::Lateral & message) + const autoware_control_msgs::Lateral & proto, autoware_control_msgs::msg::Lateral & message) { toMsg(proto.stamp(), message.stamp); message.steering_tire_angle = proto.steering_tire_angle(); @@ -528,8 +526,7 @@ void toMsg( } void toProto( - const autoware_control_msgs::msg::Control & message, - autoware_control_msgs::Control & proto) + const autoware_control_msgs::msg::Control & message, autoware_control_msgs::Control & proto) { toProto(message.stamp, *proto.mutable_stamp()); toProto(message.lateral, *proto.mutable_lateral()); @@ -537,8 +534,7 @@ void toProto( } void toMsg( - const autoware_control_msgs::Control & proto, - autoware_control_msgs::msg::Control & message) + const autoware_control_msgs::Control & proto, autoware_control_msgs::msg::Control & message) { toMsg(proto.stamp(), message.stamp); toMsg(proto.lateral(), message.lateral); @@ -551,7 +547,7 @@ auto toProto( { toProto(message.stamp, *proto.mutable_stamp()); -#define CASE(NAME) \ +#define CASE(NAME) \ case autoware_vehicle_msgs::msg::GearCommand::NAME: \ proto.set_command(autoware_vehicle_msgs::GearCommand_Constants::NAME); \ break @@ -594,9 +590,8 @@ auto toMsg( } auto toProto( - const std::tuple< - autoware_control_msgs::msg::Control, - autoware_vehicle_msgs::msg::GearCommand> & message, + const std::tuple & + message, traffic_simulator_msgs::VehicleCommand & proto) -> void { toProto(std::get<0>(message), *proto.mutable_control()); diff --git a/simulation/simulation_interface/test/expect_equal_macros.hpp b/simulation/simulation_interface/test/expect_equal_macros.hpp index 2cfe37ec892..f3862a9bb6b 100644 --- a/simulation/simulation_interface/test/expect_equal_macros.hpp +++ b/simulation/simulation_interface/test/expect_equal_macros.hpp @@ -156,14 +156,14 @@ * @brief Expect equal macros for autoware related messages. */ #define EXPECT_CONTROL_COMMAND_EQ(MSG, PROTO) \ - EXPECT_DOUBLE_EQ(MSG.longitudinal.velocity, PROTO.longitudinal().velocity()); \ + EXPECT_DOUBLE_EQ(MSG.longitudinal.velocity, PROTO.longitudinal().velocity()); \ EXPECT_DOUBLE_EQ(MSG.longitudinal.acceleration, PROTO.longitudinal().acceleration()); \ EXPECT_DOUBLE_EQ(MSG.lateral.steering_tire_angle, PROTO.lateral().steering_tire_angle()); \ EXPECT_DOUBLE_EQ( \ MSG.lateral.steering_tire_rotation_rate, PROTO.lateral().steering_tire_rotation_rate()); #define EXPECT_VEHICLE_COMMAND_EQ(CONTROL_MSG, GEAR_MSG, EMERGENCY_MSG, PROTO) \ - EXPECT_CONTROL_COMMAND_EQ(CONTROL_MSG, PROTO.control()); \ + EXPECT_CONTROL_COMMAND_EQ(CONTROL_MSG, PROTO.control()); \ EXPECT_DOUBLE_EQ(GEAR_MSG.command, PROTO.gear_command().data()); \ EXPECT_DOUBLE_EQ(EMERGENCY_MSG.emergency, PROTO.vehicle_emergency_stamped().emergency()); From 659f57e6feeb81be72285c9b3a966823fa8e55be Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Tue, 9 Jul 2024 14:34:48 +0200 Subject: [PATCH 47/49] Fix cpp_mock_scenarios launch parameters --- mock/cpp_mock_scenarios/launch/mock_test.launch.py | 2 +- mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 4b4b4573caf..2c2a3c339e5 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -89,7 +89,7 @@ def generate_launch_description(): { "junit_path": junit_path, "timeout": timeout, - "architecture_type": "awf/universe", + "architecture_type": "awf/universe/20240605", "autoware_launch_file": "planning_simulator.launch.xml", "autoware_launch_package": "autoware_launch", "launch_autoware": True, diff --git a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp index c7d1b5fc030..10bbc5e3899 100644 --- a/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp +++ b/mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp @@ -101,7 +101,7 @@ void CppScenarioNode::spawnEgoEntity( api_.attachOccupancyGridSensor([this] { simulation_api_schema::OccupancyGridSensorConfiguration configuration; // clang-format off - configuration.set_architecture_type(api_.getROS2Parameter("architecture_type", "awf/universe")); + configuration.set_architecture_type(api_.getROS2Parameter("architecture_type", "awf/universe/20240605")); configuration.set_entity("ego"); configuration.set_filter_by_range(true); configuration.set_height(200); From 7b102e5c88f534147c0604fabb8b8b139fa6f146 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Wed, 10 Jul 2024 12:22:10 +0200 Subject: [PATCH 48/49] Fix GitHub CI test job --- .../include/simulation_interface/conversions.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/simulation/simulation_interface/include/simulation_interface/conversions.hpp b/simulation/simulation_interface/include/simulation_interface/conversions.hpp index 7668d846c9e..0bd1f09e53f 100644 --- a/simulation/simulation_interface/include/simulation_interface/conversions.hpp +++ b/simulation/simulation_interface/include/simulation_interface/conversions.hpp @@ -190,7 +190,8 @@ auto toMsg( { using namespace simulation_api_schema; - auto convert_color = [](auto color) constexpr { + auto convert_color = [](auto color) constexpr + { switch (color) { case TrafficLight_Color_RED: return TrafficLightBulbMessageType::RED; @@ -205,7 +206,8 @@ auto toMsg( } }; - auto convert_shape = [](auto shape) constexpr { + auto convert_shape = [](auto shape) constexpr + { switch (shape) { case TrafficLight_Shape_CIRCLE: return TrafficLightBulbMessageType::CIRCLE; @@ -233,7 +235,8 @@ auto toMsg( } }; - auto convert_status = [](auto status) constexpr { + auto convert_status = [](auto status) constexpr + { switch (status) { case TrafficLight_Status_SOLID_OFF: return TrafficLightBulbMessageType::SOLID_OFF; From 0aba9dcf5151158f0f8dd47bf577797dc68b245f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Kie=C5=82czykowski?= Date: Wed, 24 Jul 2024 12:18:01 +0200 Subject: [PATCH 49/49] Fix conflicts in includes --- .../include/traffic_simulator/hdmap_utils/hdmap_utils.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 79610675227..a39966055dd 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -29,6 +29,9 @@ #include #include +#include +#include +#include #include #include #include