From 68cfb44084898b7faf9104bf60748bdeb4ddf37f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 14 Mar 2025 11:55:48 +0900 Subject: [PATCH 01/10] Add new member function `FieldOperatorApplication::state` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 18 +++++ .../src/field_operator_application.cpp | 69 +++++++++++++++++++ 2 files changed, 87 insertions(+) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 020154f176f..1dba35c4eed 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -21,6 +21,14 @@ #include #endif +#if __has_include() +#include +#endif + +#if __has_include() +#include +#endif + #include #include #include @@ -76,6 +84,8 @@ struct FieldOperatorApplication : public rclcpp::Node using Emergency = tier4_external_api_msgs::msg::Emergency; using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; using MrmState = autoware_adapi_v1_msgs::msg::MrmState; + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; using Trajectory = tier4_planning_msgs::msg::Trajectory; using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; @@ -96,7 +106,13 @@ struct FieldOperatorApplication : public rclcpp::Node Subscriber getLocalizationState; #endif Subscriber getMrmState; +#if __has_include() + Subscriber getOperationModeState; +#endif Subscriber getPathWithLaneId; +#if __has_include() + Subscriber getRouteState; +#endif Subscriber getTrajectory; Subscriber getTurnIndicatorsCommand; @@ -162,6 +178,8 @@ struct FieldOperatorApplication : public rclcpp::Node auto setVelocityLimit(double) -> void; + auto state() const -> std::string; + auto enableAutowareControl() -> void; }; } // namespace concealer diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index fa1a5cc5169..555eb6f8dcb 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -167,7 +167,13 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) minimum_risk_maneuver_state = state_name_of(message.state); minimum_risk_maneuver_behavior = behavior_name_of(message.behavior); }), +#if __has_include() + getOperationModeState("/api/operation_mode/state", rclcpp::QoS(1), *this), +#endif getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), +#if __has_include() + getRouteState("/api/routing/state", rclcpp::QoS(1), *this), +#endif getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this), getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), requestClearRoute("/api/routing/clear_route", *this), @@ -525,6 +531,69 @@ auto FieldOperatorApplication::setVelocityLimit(double velocity_limit) -> void }); } +auto FieldOperatorApplication::state() const -> std::string +{ +#if __has_include() and \ + __has_include() and \ + __has_include() + /* + See https://github.com/autowarefoundation/autoware.universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/document/autoware-state.md + */ + switch (const auto localization_state = getLocalizationState(); localization_state.state) { + case LocalizationInitializationState::UNKNOWN: + case LocalizationInitializationState::UNINITIALIZED: + case LocalizationInitializationState::INITIALIZING: + return "INITIALIZING"; + + case LocalizationInitializationState::INITIALIZED: + switch (const auto route_state = getRouteState(); route_state.state) { + case RouteState::UNKNOWN: + return "INITIALIZING"; + + case RouteState::UNSET: + return "WAITING_FOR_ROUTE"; + + case RouteState::ARRIVED: + return "ARRIVED_GOAL"; + + case RouteState::SET: + case RouteState::CHANGING: + switch (const auto operation_mode_state = getOperationModeState(); + operation_mode_state.mode) { + case OperationModeState::UNKNOWN: + return "INITIALIZING"; + + case OperationModeState::AUTONOMOUS: + case OperationModeState::LOCAL: + case OperationModeState::REMOTE: + if (operation_mode_state.is_autoware_control_enabled) { + return "DRIVING"; + } + [[fallthrough]]; + + case OperationModeState::STOP: + return operation_mode_state.is_autonomous_mode_available ? "WAITING_FOR_ENGAGE" + : "PLANNING"; + + default: + return ""; + } + + default: + return ""; + } + + default: + return ""; + } +#else + static_assert(__has_include()); + static_assert(false); + + return autoware_state; +#endif +} + auto FieldOperatorApplication::spinSome() -> void { task_queue.rethrow(); From 1ff1bc17a68ea7ca77f2ba4059e865560f6df845 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 14 Mar 2025 14:19:28 +0900 Subject: [PATCH 02/10] Remove data member `concealer::FieldOperatorApplication::autoware_state` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 10 ++-- .../src/field_operator_application.cpp | 51 ++++++++----------- .../src/entity/ego_entity.cpp | 2 +- 3 files changed, 26 insertions(+), 37 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 1dba35c4eed..0dde5647c1f 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -71,8 +71,6 @@ struct FieldOperatorApplication : public rclcpp::Node std::chrono::steady_clock::time_point time_limit; - std::string autoware_state = "LAUNCHING"; - std::string minimum_risk_maneuver_state; std::string minimum_risk_maneuver_behavior; @@ -135,16 +133,16 @@ struct FieldOperatorApplication : public rclcpp::Node template auto waitForAutowareStateToBe( - const std::string & state, Thunk thunk = [] {}) + const std::string & expect, Thunk thunk = [] {}) { thunk(); - while (not finalized.load() and autoware_state != state) { + while (not finalized.load() and state() != expect) { if (time_limit <= std::chrono::steady_clock::now()) { throw common::AutowareError( - "Simulator waited for the Autoware state to transition to ", state, + "Simulator waited for the Autoware state to transition to ", expect, ", but time is up. The current Autoware state is ", - (autoware_state.empty() ? "not published yet" : autoware_state)); + (state().empty() ? "unknown" : state())); } else { thunk(); rclcpp::GenericRate(std::chrono::seconds(1)).sleep(); diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 555eb6f8dcb..0a86e0b45ae 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -81,30 +81,7 @@ FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) : rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), process_id(pid), time_limit(std::chrono::steady_clock::now() + std::chrono::seconds(getParameter("initialize_duration"))), - getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & message) { - auto state_name_of = [](auto state) constexpr { - switch (state) { - case AutowareState::INITIALIZING: - return "INITIALIZING"; - case AutowareState::WAITING_FOR_ROUTE: - return "WAITING_FOR_ROUTE"; - case AutowareState::PLANNING: - return "PLANNING"; - case AutowareState::WAITING_FOR_ENGAGE: - return "WAITING_FOR_ENGAGE"; - case AutowareState::DRIVING: - return "DRIVING"; - case AutowareState::ARRIVED_GOAL: - return "ARRIVED_GOAL"; - case AutowareState::FINALIZING: - return "FINALIZING"; - default: - return ""; - } - }; - - autoware_state = state_name_of(message.state); - }), + getAutowareState("/autoware/state", rclcpp::QoS(1), *this), getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this), getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & message) { @@ -310,13 +287,13 @@ auto FieldOperatorApplication::engage() -> void auto FieldOperatorApplication::engageable() const -> bool { task_queue.rethrow(); - return task_queue.empty() and autoware_state == "WAITING_FOR_ENGAGE"; + return task_queue.empty() and state() == "WAITING_FOR_ENGAGE"; } auto FieldOperatorApplication::engaged() const -> bool { task_queue.rethrow(); - return task_queue.empty() and autoware_state == "DRIVING"; + return task_queue.empty() and state() == "DRIVING"; } auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray @@ -587,10 +564,24 @@ auto FieldOperatorApplication::state() const -> std::string return ""; } #else - static_assert(__has_include()); - static_assert(false); - - return autoware_state; + switch (const auto autoware_state = getAutowareState().state) { + case AutowareState::INITIALIZING: + return "INITIALIZING"; + case AutowareState::WAITING_FOR_ROUTE: + return "WAITING_FOR_ROUTE"; + case AutowareState::PLANNING: + return "PLANNING"; + case AutowareState::WAITING_FOR_ENGAGE: + return "WAITING_FOR_ENGAGE"; + case AutowareState::DRIVING: + return "DRIVING"; + case AutowareState::ARRIVED_GOAL: + return "ARRIVED_GOAL"; + case AutowareState::FINALIZING: + return "FINALIZING"; + default: + return ""; + } #endif } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 1d3f943fd49..c02a550a277 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -117,7 +117,7 @@ auto EgoEntity::getTurnIndicatorsCommandName() const -> std::string } } -auto EgoEntity::getCurrentAction() const -> std::string { return autoware_state; } +auto EgoEntity::getCurrentAction() const -> std::string { return state(); } auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter { From 949fd16fc89885b322421b581b0ee7706a4b2124 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 14 Mar 2025 17:30:07 +0900 Subject: [PATCH 03/10] Update interpreter to call `engage` only once immediately after evaluating `Storyboard.Init` Signed-off-by: yamacir-kit --- .../src/field_operator_application.cpp | 8 +- .../src/openscenario_interpreter.cpp | 74 +++++++------------ 2 files changed, 29 insertions(+), 53 deletions(-) diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 0a86e0b45ae..56ca36a472b 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -273,7 +273,7 @@ auto FieldOperatorApplication::engage() -> void try { auto request = std::make_shared(); request->engage = true; - return requestEngage(request, 1); + return requestEngage(request, 30); } catch (const common::AutowareError &) { return; // Ignore error because this service is validated by Autoware state transition. } @@ -326,7 +326,7 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi initial_pose_stamped.pose.pose = initial_pose; return initial_pose_stamped; }()); - return requestInitialPose(request, 1); + return requestInitialPose(request, 30); } catch (const common::AutowareError &) { return; // Ignore error because this service is validated by Autoware state transition. } @@ -374,7 +374,7 @@ auto FieldOperatorApplication::plan(const std::vectorwaypoints.push_back(each.pose); } - requestSetRoutePoints(request, 1); + requestSetRoutePoints(request, 30); waitForAutowareStateToBe("WAITING_FOR_ENGAGE"); }); @@ -489,7 +489,7 @@ auto FieldOperatorApplication::sendCooperateCommand( request->stamp = cooperate_status_array.stamp; request->commands.push_back(cooperate_command); - task_queue.delay([this, request]() { requestCooperateCommands(request, 1); }); + task_queue.delay([this, request]() { requestCooperateCommands(request, 30); }); used_cooperate_statuses.push_back(*cooperate_status); } diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index ae815c5f52b..35545eadec6 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -157,55 +157,22 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result }, [this]() { const auto evaluate_time = execution_timer.invoke("evaluate", [this]() { - if (std::isnan(evaluateSimulationTime())) { - auto engaged = [this]() { - return std::all_of( - currentScenarioDefinition()->entities.begin(), - currentScenarioDefinition()->entities.end(), [this](const auto & each) { - return std::apply( - [this](const auto & name, const auto & object) { - return not object.template is() or - not object.template as().is_added or - not object.template as() - .object_controller.isAutoware() or - NonStandardOperation::isEngaged(name); - }, - each); - }); - }; - - auto engageable = [this]() { - return std::all_of( - currentScenarioDefinition()->entities.begin(), - currentScenarioDefinition()->entities.end(), [this](const auto & each) { - return std::apply( - [this](const auto & name, const auto & object) { - return not object.template is() or - not object.template as().is_added or - not object.template as() - .object_controller.isAutoware() or - NonStandardOperation::isEngageable(name); - }, - each); - }); - }; - - auto engage = [this]() { - for (const auto & [name, object] : currentScenarioDefinition()->entities) { - if ( - object.template is() and - object.template as().is_added and - object.template as().object_controller.isAutoware()) { - NonStandardOperation::engage(name); - } - } - }; - - if (engaged()) { - return activateNonUserDefinedControllers(); - } else if (engageable()) { - return engage(); - } + if ( + std::isnan(evaluateSimulationTime()) and + std::all_of( + currentScenarioDefinition()->entities.begin(), + currentScenarioDefinition()->entities.end(), [this](const auto & each) { + return std::apply( + [this](const auto & name, const auto & object) { + return not object.template is() or + not object.template as().is_added or + not object.template as() + .object_controller.isAutoware() or + NonStandardOperation::isEngaged(name); + }, + each); + })) { + return activateNonUserDefinedControllers(); } else if (currentScenarioDefinition()) { currentScenarioDefinition()->evaluate(); } else { @@ -299,6 +266,15 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result if (currentScenarioDefinition()) { currentScenarioDefinition()->storyboard.init.evaluateInstantaneousActions(); + + for (const auto & [name, object] : currentScenarioDefinition()->entities) { + if ( + object.template is() and + object.template as().is_added and + object.template as().object_controller.isAutoware()) { + NonStandardOperation::engage(name); + } + } } else { throw Error("No script evaluable."); } From d1db5e7ca3b8cf7d26ad78ff4ecedaa7dd0af665 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Fri, 14 Mar 2025 18:01:47 +0900 Subject: [PATCH 04/10] Cleanup Signed-off-by: yamacir-kit --- .../src/openscenario_interpreter.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index 35545eadec6..29af89ec88e 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -163,11 +163,10 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result currentScenarioDefinition()->entities.begin(), currentScenarioDefinition()->entities.end(), [this](const auto & each) { return std::apply( - [this](const auto & name, const auto & object) { - return not object.template is() or - not object.template as().is_added or - not object.template as() - .object_controller.isAutoware() or + [this](const auto & name, const Object & object) { + return not object.is() or + not object.as().is_added or + not object.as().object_controller.isAutoware() or NonStandardOperation::isEngaged(name); }, each); From 4afd9f85d0e6d514bea1f101490e51c33dc58210 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 17 Mar 2025 13:55:21 +0900 Subject: [PATCH 05/10] Add new struct `LegacyAutowareState` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 15 ++- .../concealer/legacy_autoware_state.hpp | 96 +++++++++++++++++++ .../src/field_operator_application.cpp | 55 ++++------- .../src/entity/ego_entity.cpp | 2 +- 4 files changed, 124 insertions(+), 44 deletions(-) create mode 100644 external/concealer/include/concealer/legacy_autoware_state.hpp diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 0dde5647c1f..b4819ab50f7 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -35,10 +35,10 @@ #include #include #include -#include #include #include #include +#include #include #include #include @@ -133,16 +133,15 @@ struct FieldOperatorApplication : public rclcpp::Node template auto waitForAutowareStateToBe( - const std::string & expect, Thunk thunk = [] {}) + const LegacyAutowareState & state, Thunk thunk = [] {}) { thunk(); - while (not finalized.load() and state() != expect) { + while (not finalized.load() and getLegacyAutowareState().value != state.value) { if (time_limit <= std::chrono::steady_clock::now()) { throw common::AutowareError( - "Simulator waited for the Autoware state to transition to ", expect, - ", but time is up. The current Autoware state is ", - (state().empty() ? "unknown" : state())); + "Simulator waited for the Autoware state to transition to ", state, + ", but time is up. The current Autoware state is ", getLegacyAutowareState()); } else { thunk(); rclcpp::GenericRate(std::chrono::seconds(1)).sleep(); @@ -168,6 +167,8 @@ struct FieldOperatorApplication : public rclcpp::Node auto clearRoute() -> void; + auto getLegacyAutowareState() const -> LegacyAutowareState; + auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray; auto requestAutoModeForCooperation(const std::string &, bool) -> void; @@ -176,8 +177,6 @@ struct FieldOperatorApplication : public rclcpp::Node auto setVelocityLimit(double) -> void; - auto state() const -> std::string; - auto enableAutowareControl() -> void; }; } // namespace concealer diff --git a/external/concealer/include/concealer/legacy_autoware_state.hpp b/external/concealer/include/concealer/legacy_autoware_state.hpp new file mode 100644 index 00000000000..02ce332b63b --- /dev/null +++ b/external/concealer/include/concealer/legacy_autoware_state.hpp @@ -0,0 +1,96 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONCEALER__LEGACY_AUTOWARE_STATE_HPP_ +#define CONCEALER__LEGACY_AUTOWARE_STATE_HPP_ + +#if __has_include() +#include +#endif + +namespace concealer +{ +struct LegacyAutowareState +{ + enum value_type { + undefined, + initializing, + waiting_for_route, + planning, + waiting_for_engage, + driving, + arrived_goal, + finalizing, + }; + + value_type value; + + constexpr LegacyAutowareState(value_type value) : value(value) {} + +#if __has_include() + LegacyAutowareState(const autoware_system_msgs::msg::AutowareState & autoware_state) + : value([&]() { + switch (autoware_state.state) { + case autoware_system_msgs::msg::AutowareState::INITIALIZING: + return LegacyAutowareState::initializing; + case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE: + return LegacyAutowareState::waiting_for_route; + case autoware_system_msgs::msg::AutowareState::PLANNING: + return LegacyAutowareState::planning; + case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE: + return LegacyAutowareState::waiting_for_engage; + case autoware_system_msgs::msg::AutowareState::DRIVING: + return LegacyAutowareState::driving; + case autoware_system_msgs::msg::AutowareState::ARRIVED_GOAL: + return LegacyAutowareState::arrived_goal; + case autoware_system_msgs::msg::AutowareState::FINALIZING: + return LegacyAutowareState::finalizing; + } + }()) + { + } +#endif + + constexpr operator const char *() const + { + switch (value) { + case initializing: + return "INITIALIZING"; + case waiting_for_route: + return "WAITING_FOR_ROUTE"; + case planning: + return "PLANNING"; + case waiting_for_engage: + return "WAITING_FOR_ENGAGE"; + case driving: + return "DRIVING"; + case arrived_goal: + return "ARRIVED_GOAL"; + case finalizing: + return "FINALIZING"; + default: + return ""; + } + } + + operator std::string() const { return operator const char *(); } + + friend auto operator<<(std::ostream & os, const LegacyAutowareState & state) -> std::ostream & + { + return os << static_cast(state); + } +}; +} // namespace concealer + +#endif // CONCEALER__LEGACY_AUTOWARE_STATE_HPP_ diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 36b51b693e2..0614ab9a573 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -268,7 +268,7 @@ auto FieldOperatorApplication::engage() -> void request->engage = true; requestEngage(request, 30); - waitForAutowareStateToBe("DRIVING"); + waitForAutowareStateToBe(LegacyAutowareState::driving); time_limit = std::decay_t::max(); }); @@ -277,13 +277,14 @@ auto FieldOperatorApplication::engage() -> void auto FieldOperatorApplication::engageable() const -> bool { task_queue.rethrow(); - return task_queue.empty() and state() == "WAITING_FOR_ENGAGE"; + return task_queue.empty() and + getLegacyAutowareState().value == LegacyAutowareState::waiting_for_engage; } auto FieldOperatorApplication::engaged() const -> bool { task_queue.rethrow(); - return task_queue.empty() and state() == "DRIVING"; + return task_queue.empty() and getLegacyAutowareState().value == LegacyAutowareState::driving; } auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray @@ -317,7 +318,7 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi }()); requestInitialPose(request, 30); - waitForAutowareStateToBe("WAITING_FOR_ROUTE"); + waitForAutowareStateToBe(LegacyAutowareState::waiting_for_route); }); } } @@ -360,7 +361,7 @@ auto FieldOperatorApplication::plan(const std::vector void }); } -auto FieldOperatorApplication::state() const -> std::string +auto FieldOperatorApplication::getLegacyAutowareState() const -> LegacyAutowareState { #if __has_include() and \ __has_include() and \ @@ -504,68 +505,52 @@ auto FieldOperatorApplication::state() const -> std::string case LocalizationInitializationState::UNKNOWN: case LocalizationInitializationState::UNINITIALIZED: case LocalizationInitializationState::INITIALIZING: - return "INITIALIZING"; + return LegacyAutowareState::initializing; case LocalizationInitializationState::INITIALIZED: switch (const auto route_state = getRouteState(); route_state.state) { case RouteState::UNKNOWN: - return "INITIALIZING"; + return LegacyAutowareState::initializing; case RouteState::UNSET: - return "WAITING_FOR_ROUTE"; + return LegacyAutowareState::waiting_for_route; case RouteState::ARRIVED: - return "ARRIVED_GOAL"; + return LegacyAutowareState::arrived_goal; case RouteState::SET: case RouteState::CHANGING: switch (const auto operation_mode_state = getOperationModeState(); operation_mode_state.mode) { case OperationModeState::UNKNOWN: - return "INITIALIZING"; + return LegacyAutowareState::initializing; case OperationModeState::AUTONOMOUS: case OperationModeState::LOCAL: case OperationModeState::REMOTE: if (operation_mode_state.is_autoware_control_enabled) { - return "DRIVING"; + return LegacyAutowareState::driving; } [[fallthrough]]; case OperationModeState::STOP: - return operation_mode_state.is_autonomous_mode_available ? "WAITING_FOR_ENGAGE" - : "PLANNING"; + return operation_mode_state.is_autonomous_mode_available + ? LegacyAutowareState::waiting_for_engage + : LegacyAutowareState::planning; default: - return ""; + return LegacyAutowareState::undefined; } default: - return ""; + return LegacyAutowareState::undefined; } default: - return ""; + return LegacyAutowareState::undefined; } #else - switch (const auto autoware_state = getAutowareState().state) { - case AutowareState::INITIALIZING: - return "INITIALIZING"; - case AutowareState::WAITING_FOR_ROUTE: - return "WAITING_FOR_ROUTE"; - case AutowareState::PLANNING: - return "PLANNING"; - case AutowareState::WAITING_FOR_ENGAGE: - return "WAITING_FOR_ENGAGE"; - case AutowareState::DRIVING: - return "DRIVING"; - case AutowareState::ARRIVED_GOAL: - return "ARRIVED_GOAL"; - case AutowareState::FINALIZING: - return "FINALIZING"; - default: - return ""; - } + return getAutowareState(); #endif } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index c0fa921278d..a6850d04866 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -119,7 +119,7 @@ auto EgoEntity::getTurnIndicatorsCommandName() const -> std::string } } -auto EgoEntity::getCurrentAction() const -> std::string { return state(); } +auto EgoEntity::getCurrentAction() const -> std::string { return getLegacyAutowareState(); } auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter { From b675d72f49d55544c632c0f2930778ca0919ccae Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 17 Mar 2025 14:23:49 +0900 Subject: [PATCH 06/10] Add message conversion constructor to `LegacyAutowareState` Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 12 --- .../concealer/legacy_autoware_state.hpp | 90 +++++++++++++++++-- .../src/field_operator_application.cpp | 54 +---------- 3 files changed, 84 insertions(+), 72 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index b4819ab50f7..7f02d69dabe 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -17,18 +17,6 @@ #include -#if __has_include() -#include -#endif - -#if __has_include() -#include -#endif - -#if __has_include() -#include -#endif - #include #include #include diff --git a/external/concealer/include/concealer/legacy_autoware_state.hpp b/external/concealer/include/concealer/legacy_autoware_state.hpp index 02ce332b63b..cd715ebcc7a 100644 --- a/external/concealer/include/concealer/legacy_autoware_state.hpp +++ b/external/concealer/include/concealer/legacy_autoware_state.hpp @@ -19,6 +19,18 @@ #include #endif +#if __has_include() +#include +#endif + +#if __has_include() +#include +#endif + +#if __has_include() +#include +#endif + namespace concealer { struct LegacyAutowareState @@ -39,23 +51,85 @@ struct LegacyAutowareState constexpr LegacyAutowareState(value_type value) : value(value) {} #if __has_include() - LegacyAutowareState(const autoware_system_msgs::msg::AutowareState & autoware_state) + explicit LegacyAutowareState(const autoware_system_msgs::msg::AutowareState & autoware_state) : value([&]() { switch (autoware_state.state) { case autoware_system_msgs::msg::AutowareState::INITIALIZING: - return LegacyAutowareState::initializing; + return initializing; case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE: - return LegacyAutowareState::waiting_for_route; + return waiting_for_route; case autoware_system_msgs::msg::AutowareState::PLANNING: - return LegacyAutowareState::planning; + return planning; case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE: - return LegacyAutowareState::waiting_for_engage; + return waiting_for_engage; case autoware_system_msgs::msg::AutowareState::DRIVING: - return LegacyAutowareState::driving; + return driving; case autoware_system_msgs::msg::AutowareState::ARRIVED_GOAL: - return LegacyAutowareState::arrived_goal; + return arrived_goal; case autoware_system_msgs::msg::AutowareState::FINALIZING: - return LegacyAutowareState::finalizing; + return finalizing; + } + }()) + { + } +#endif + +#if __has_include() and \ + __has_include() and \ + __has_include() + explicit LegacyAutowareState( + const autoware_adapi_v1_msgs::msg::LocalizationInitializationState & localization_state, + const autoware_adapi_v1_msgs::msg::RouteState & route_state, + const autoware_adapi_v1_msgs::msg::OperationModeState & operation_mode_state) + : value([&]() { + /* + See https://github.com/autowarefoundation/autoware.universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/document/autoware-state.md + */ + switch (localization_state.state) { + case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNKNOWN: + case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNINITIALIZED: + case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::INITIALIZING: + return initializing; + + case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::INITIALIZED: + switch (route_state.state) { + case autoware_adapi_v1_msgs::msg::RouteState::UNKNOWN: + return initializing; + + case autoware_adapi_v1_msgs::msg::RouteState::UNSET: + return waiting_for_route; + + case autoware_adapi_v1_msgs::msg::RouteState::ARRIVED: + return arrived_goal; + + case autoware_adapi_v1_msgs::msg::RouteState::SET: + case autoware_adapi_v1_msgs::msg::RouteState::CHANGING: + switch (operation_mode_state.mode) { + case autoware_adapi_v1_msgs::msg::OperationModeState::UNKNOWN: + return initializing; + + case autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS: + case autoware_adapi_v1_msgs::msg::OperationModeState::LOCAL: + case autoware_adapi_v1_msgs::msg::OperationModeState::REMOTE: + if (operation_mode_state.is_autoware_control_enabled) { + return driving; + } + [[fallthrough]]; + + case autoware_adapi_v1_msgs::msg::OperationModeState::STOP: + return operation_mode_state.is_autonomous_mode_available ? waiting_for_engage + : planning; + + default: + return undefined; + } + + default: + return undefined; + } + + default: + return undefined; } }()) { diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 0614ab9a573..96d5e420908 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -498,59 +498,9 @@ auto FieldOperatorApplication::getLegacyAutowareState() const -> LegacyAutowareS #if __has_include() and \ __has_include() and \ __has_include() - /* - See https://github.com/autowarefoundation/autoware.universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/document/autoware-state.md - */ - switch (const auto localization_state = getLocalizationState(); localization_state.state) { - case LocalizationInitializationState::UNKNOWN: - case LocalizationInitializationState::UNINITIALIZED: - case LocalizationInitializationState::INITIALIZING: - return LegacyAutowareState::initializing; - - case LocalizationInitializationState::INITIALIZED: - switch (const auto route_state = getRouteState(); route_state.state) { - case RouteState::UNKNOWN: - return LegacyAutowareState::initializing; - - case RouteState::UNSET: - return LegacyAutowareState::waiting_for_route; - - case RouteState::ARRIVED: - return LegacyAutowareState::arrived_goal; - - case RouteState::SET: - case RouteState::CHANGING: - switch (const auto operation_mode_state = getOperationModeState(); - operation_mode_state.mode) { - case OperationModeState::UNKNOWN: - return LegacyAutowareState::initializing; - - case OperationModeState::AUTONOMOUS: - case OperationModeState::LOCAL: - case OperationModeState::REMOTE: - if (operation_mode_state.is_autoware_control_enabled) { - return LegacyAutowareState::driving; - } - [[fallthrough]]; - - case OperationModeState::STOP: - return operation_mode_state.is_autonomous_mode_available - ? LegacyAutowareState::waiting_for_engage - : LegacyAutowareState::planning; - - default: - return LegacyAutowareState::undefined; - } - - default: - return LegacyAutowareState::undefined; - } - - default: - return LegacyAutowareState::undefined; - } + return LegacyAutowareState(getLocalizationState(), getRouteState(), getOperationModeState()); #else - return getAutowareState(); + return LegacyAutowareState(getAutowareState()); #endif } From f1683619b7c51aac308fe5909c1e9f7733e40489 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 17 Mar 2025 16:44:30 +0900 Subject: [PATCH 07/10] Update `initialize`, `plan`, and `engage` to take into account AutowareState Signed-off-by: yamacir-kit --- .../concealer/field_operator_application.hpp | 12 +- .../src/field_operator_application.cpp | 143 +++++++++++------- 2 files changed, 94 insertions(+), 61 deletions(-) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 7f02d69dabe..1763c8c62cc 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -91,16 +91,16 @@ struct FieldOperatorApplication : public rclcpp::Node #if __has_include() Subscriber getLocalizationState; #endif - Subscriber getMrmState; + Subscriber getMrmState; #if __has_include() - Subscriber getOperationModeState; + Subscriber getOperationModeState; #endif - Subscriber getPathWithLaneId; + Subscriber getPathWithLaneId; #if __has_include() - Subscriber getRouteState; + Subscriber getRouteState; #endif - Subscriber getTrajectory; - Subscriber getTurnIndicatorsCommand; + Subscriber getTrajectory; + Subscriber getTurnIndicatorsCommand; Service requestClearRoute; Service requestCooperateCommands; diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 96d5e420908..ae6e978f87a 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -264,13 +264,26 @@ auto FieldOperatorApplication::enableAutowareControl() -> void auto FieldOperatorApplication::engage() -> void { task_queue.delay([this]() { - auto request = std::make_shared(); - request->engage = true; - requestEngage(request, 30); - - waitForAutowareStateToBe(LegacyAutowareState::driving); - - time_limit = std::decay_t::max(); + switch (const auto state = getLegacyAutowareState(); state.value) { + default: + throw common::AutowareError( + "The simulator attempted to request Autoware to engage, but was aborted because " + "Autoware's current state is ", + state, "."); + case LegacyAutowareState::planning: + waitForAutowareStateToBe(LegacyAutowareState::waiting_for_engage); + [[fallthrough]]; + case LegacyAutowareState::waiting_for_engage: + requestEngage( + [&]() { + auto request = std::make_shared(); + request->engage = true; + return request; + }(), + 30); + time_limit = std::decay_t::max(); + break; + } }); } @@ -302,23 +315,29 @@ auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initi { if (not std::exchange(initialized, true)) { task_queue.delay([this, initial_pose]() { -#if __has_include() - if (getLocalizationState().state != LocalizationInitializationState::UNINITIALIZED) { - return; + switch (const auto state = getLegacyAutowareState(); state.value) { + default: + throw common::AutowareError( + "The simulator attempted to initialize Autoware, but aborted because Autoware's " + "current state is ", + state, "."); + case LegacyAutowareState::initializing: + requestInitialPose( + [&]() { + auto request = + std::make_shared(); + request->pose.push_back([&]() { + auto initial_pose_stamped = geometry_msgs::msg::PoseWithCovarianceStamped(); + initial_pose_stamped.header.stamp = get_clock()->now(); + initial_pose_stamped.header.frame_id = "map"; + initial_pose_stamped.pose.pose = initial_pose; + return initial_pose_stamped; + }()); + return request; + }(), + 30); + break; } -#endif - auto request = - std::make_shared(); - request->pose.push_back([&]() { - auto initial_pose_stamped = geometry_msgs::msg::PoseWithCovarianceStamped(); - initial_pose_stamped.header.stamp = get_clock()->now(); - initial_pose_stamped.header.frame_id = "map"; - initial_pose_stamped.pose.pose = initial_pose; - return initial_pose_stamped; - }()); - requestInitialPose(request, 30); - - waitForAutowareStateToBe(LegacyAutowareState::waiting_for_route); }); } } @@ -329,39 +348,53 @@ auto FieldOperatorApplication::plan(const std::vector(); - - request->header = route.back().header; - - /* - NOTE: The autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type was - created on 2022/09/05 [1], and the autoware_adapi_v1_msgs::msg::Option - type data member was added to the - autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type on 2023/04/12 - [2]. Therefore, we cannot expect - autoware_adapi_v1_msgs::srv::SetRoutePoints::Request to always have a - data member `option`. - - [1] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/805f8ebd3ca24564844df9889feeaf183101fbef - [2] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/cf310bd038673b6cbef3ae3b61dfe607212de419 - */ - if constexpr ( - DetectMember_option::value and - DetectMember_allow_goal_modification< - decltype(std::declval().option)>::value) { - request->option.allow_goal_modification = - common::getParameter(get_node_parameters_interface(), "allow_goal_modification"); - } - - request->goal = route.back().pose; - - for (const auto & each : route | boost::adaptors::sliced(0, route.size() - 1)) { - request->waypoints.push_back(each.pose); + switch (const auto state = getLegacyAutowareState(); state.value) { + default: + throw common::AutowareError( + "The simulator attempted to send a goal to Autoware, but was aborted because Autoware's " + "current state is ", + state, "."); + case LegacyAutowareState::initializing: + case LegacyAutowareState::arrived_goal: + waitForAutowareStateToBe(LegacyAutowareState::waiting_for_route); + [[fallthrough]]; + case LegacyAutowareState::waiting_for_route: + requestSetRoutePoints( + [&]() { + auto request = std::make_shared(); + + request->header = route.back().header; + request->goal = route.back().pose; + + for (const auto & each : route | boost::adaptors::sliced(0, route.size() - 1)) { + request->waypoints.push_back(each.pose); + } + + /* + NOTE: The autoware_adapi_v1_msgs::srv::SetRoutePoints::Request + type was created on 2022/09/05 [1], and the + autoware_adapi_v1_msgs::msg::Option type data member was added + to the autoware_adapi_v1_msgs::srv::SetRoutePoints::Request type + on 2023/04/12 [2]. Therefore, we cannot expect + autoware_adapi_v1_msgs::srv::SetRoutePoints::Request to always + have a data member `option`. + + [1] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/805f8ebd3ca24564844df9889feeaf183101fbef + [2] https://github.com/autowarefoundation/autoware_adapi_msgs/commit/cf310bd038673b6cbef3ae3b61dfe607212de419 + */ + if constexpr ( + DetectMember_option::value and + DetectMember_allow_goal_modification< + decltype(std::declval().option)>::value) { + request->option.allow_goal_modification = common::getParameter( + get_node_parameters_interface(), "allow_goal_modification"); + } + + return request; + }(), + 30); + break; } - - requestSetRoutePoints(request, 30); - - waitForAutowareStateToBe(LegacyAutowareState::waiting_for_engage); }); } From 21c167f08d35ea485719084f7cdbdac8841641ed Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 17 Mar 2025 17:11:55 +0900 Subject: [PATCH 08/10] Fix `LegacyAutowareState` to correctly transition from `arrived_goal` to `waiting_for_route` Signed-off-by: yamacir-kit --- .../include/concealer/legacy_autoware_state.hpp | 13 +++++++++---- .../concealer/src/field_operator_application.cpp | 3 ++- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/external/concealer/include/concealer/legacy_autoware_state.hpp b/external/concealer/include/concealer/legacy_autoware_state.hpp index cd715ebcc7a..62a3e13acb6 100644 --- a/external/concealer/include/concealer/legacy_autoware_state.hpp +++ b/external/concealer/include/concealer/legacy_autoware_state.hpp @@ -80,7 +80,8 @@ struct LegacyAutowareState explicit LegacyAutowareState( const autoware_adapi_v1_msgs::msg::LocalizationInitializationState & localization_state, const autoware_adapi_v1_msgs::msg::RouteState & route_state, - const autoware_adapi_v1_msgs::msg::OperationModeState & operation_mode_state) + const autoware_adapi_v1_msgs::msg::OperationModeState & operation_mode_state, + const rclcpp::Time & now) : value([&]() { /* See https://github.com/autowarefoundation/autoware.universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/document/autoware-state.md @@ -96,12 +97,16 @@ struct LegacyAutowareState case autoware_adapi_v1_msgs::msg::RouteState::UNKNOWN: return initializing; + case autoware_adapi_v1_msgs::msg::RouteState::ARRIVED: + if (const auto duration = now - rclcpp::Time(route_state.stamp); + duration.seconds() < 2.0) { + return arrived_goal; + } + [[fallthrough]]; + case autoware_adapi_v1_msgs::msg::RouteState::UNSET: return waiting_for_route; - case autoware_adapi_v1_msgs::msg::RouteState::ARRIVED: - return arrived_goal; - case autoware_adapi_v1_msgs::msg::RouteState::SET: case autoware_adapi_v1_msgs::msg::RouteState::CHANGING: switch (operation_mode_state.mode) { diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index ae6e978f87a..bab01f7a503 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -531,7 +531,8 @@ auto FieldOperatorApplication::getLegacyAutowareState() const -> LegacyAutowareS #if __has_include() and \ __has_include() and \ __has_include() - return LegacyAutowareState(getLocalizationState(), getRouteState(), getOperationModeState()); + return LegacyAutowareState( + getLocalizationState(), getRouteState(), getOperationModeState(), now()); #else return LegacyAutowareState(getAutowareState()); #endif From 7b7c133481273ef8a76866bab45c209789c5e00f Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Mon, 24 Mar 2025 18:00:45 +0900 Subject: [PATCH 09/10] Add missing `__has_include` Signed-off-by: yamacir-kit --- .../include/concealer/field_operator_application.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 1763c8c62cc..eee473ee7d0 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -68,10 +68,16 @@ struct FieldOperatorApplication : public rclcpp::Node using Control = autoware_control_msgs::msg::Control; using CooperateStatusArray = tier4_rtc_msgs::msg::CooperateStatusArray; using Emergency = tier4_external_api_msgs::msg::Emergency; +#if __has_include() using LocalizationInitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; +#endif using MrmState = autoware_adapi_v1_msgs::msg::MrmState; +#if __has_include() using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; +#endif +#if __has_include() using RouteState = autoware_adapi_v1_msgs::msg::RouteState; +#endif using Trajectory = tier4_planning_msgs::msg::Trajectory; using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; From 02f88a72c428cad433fc7bc9d033299555813462 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 27 Mar 2025 14:27:04 +0900 Subject: [PATCH 10/10] Add a link to the code referenced for `LegacyAutowareState` construction in comment Signed-off-by: yamacir-kit --- .../concealer/include/concealer/legacy_autoware_state.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/external/concealer/include/concealer/legacy_autoware_state.hpp b/external/concealer/include/concealer/legacy_autoware_state.hpp index 62a3e13acb6..de2a95eb1f7 100644 --- a/external/concealer/include/concealer/legacy_autoware_state.hpp +++ b/external/concealer/include/concealer/legacy_autoware_state.hpp @@ -84,7 +84,9 @@ struct LegacyAutowareState const rclcpp::Time & now) : value([&]() { /* - See https://github.com/autowarefoundation/autoware.universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/document/autoware-state.md + See + - https://github.com/autowarefoundation/autoware.universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/document/autoware-state.md + - https://github.com/autowarefoundation/autoware_universe/blob/e60daf7d1c85208eaac083b90c181e224c2ac513/system/autoware_default_adapi/src/compatibility/autoware_state.cpp#L80-L141 */ switch (localization_state.state) { case autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNKNOWN: