diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 020154f176f..eee473ee7d0 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -17,20 +17,16 @@ #include -#if __has_include() -#include -#endif - #include #include #include #include #include #include -#include #include #include #include +#include #include #include #include @@ -63,8 +59,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; @@ -74,8 +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; @@ -95,10 +97,16 @@ struct FieldOperatorApplication : public rclcpp::Node #if __has_include() Subscriber getLocalizationState; #endif - Subscriber getMrmState; - Subscriber getPathWithLaneId; - Subscriber getTrajectory; - Subscriber getTurnIndicatorsCommand; + Subscriber getMrmState; +#if __has_include() + Subscriber getOperationModeState; +#endif + Subscriber getPathWithLaneId; +#if __has_include() + Subscriber getRouteState; +#endif + Subscriber getTrajectory; + Subscriber getTurnIndicatorsCommand; Service requestClearRoute; Service requestCooperateCommands; @@ -119,16 +127,15 @@ struct FieldOperatorApplication : public rclcpp::Node template auto waitForAutowareStateToBe( - const std::string & state, Thunk thunk = [] {}) + const LegacyAutowareState & state, Thunk thunk = [] {}) { thunk(); - while (not finalized.load() and autoware_state != state) { + 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 ", state, - ", but time is up. The current Autoware state is ", - (autoware_state.empty() ? "not published yet" : autoware_state)); + ", but time is up. The current Autoware state is ", getLegacyAutowareState()); } else { thunk(); rclcpp::GenericRate(std::chrono::seconds(1)).sleep(); @@ -154,6 +161,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; 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..de2a95eb1f7 --- /dev/null +++ b/external/concealer/include/concealer/legacy_autoware_state.hpp @@ -0,0 +1,177 @@ +// 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 + +#if __has_include() +#include +#endif + +#if __has_include() +#include +#endif + +#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() + explicit LegacyAutowareState(const autoware_system_msgs::msg::AutowareState & autoware_state) + : value([&]() { + switch (autoware_state.state) { + case autoware_system_msgs::msg::AutowareState::INITIALIZING: + return initializing; + case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ROUTE: + return waiting_for_route; + case autoware_system_msgs::msg::AutowareState::PLANNING: + return planning; + case autoware_system_msgs::msg::AutowareState::WAITING_FOR_ENGAGE: + return waiting_for_engage; + case autoware_system_msgs::msg::AutowareState::DRIVING: + return driving; + case autoware_system_msgs::msg::AutowareState::ARRIVED_GOAL: + return arrived_goal; + case autoware_system_msgs::msg::AutowareState::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, + const rclcpp::Time & now) + : value([&]() { + /* + 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: + 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::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::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; + } + }()) + { + } +#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 dbd09e24e4f..bab01f7a503 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(common::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) { @@ -167,7 +144,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), @@ -281,26 +264,40 @@ auto FieldOperatorApplication::enableAutowareControl() -> void auto FieldOperatorApplication::engage() -> void { task_queue.delay([this]() { - auto request = std::make_shared(); - request->engage = true; - requestEngage(request, 30); - - waitForAutowareStateToBe("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; + } }); } auto FieldOperatorApplication::engageable() const -> bool { task_queue.rethrow(); - return task_queue.empty() and autoware_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 autoware_state == "DRIVING"; + return task_queue.empty() and getLegacyAutowareState().value == LegacyAutowareState::driving; } auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray @@ -318,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("WAITING_FOR_ROUTE"); }); } } @@ -345,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"); + 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; } - - request->goal = route.back().pose; - - for (const auto & each : route | boost::adaptors::sliced(0, route.size() - 1)) { - request->waypoints.push_back(each.pose); - } - - requestSetRoutePoints(request, 30); - - waitForAutowareStateToBe("WAITING_FOR_ENGAGE"); }); } @@ -490,7 +507,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); } @@ -509,6 +526,18 @@ auto FieldOperatorApplication::setVelocityLimit(double velocity_limit) -> void }); } +auto FieldOperatorApplication::getLegacyAutowareState() const -> LegacyAutowareState +{ +#if __has_include() and \ + __has_include() and \ + __has_include() + return LegacyAutowareState( + getLocalizationState(), getRouteState(), getOperationModeState(), now()); +#else + return LegacyAutowareState(getAutowareState()); +#endif +} + auto FieldOperatorApplication::spinSome() -> void { task_queue.rethrow(); diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index 38d830a076d..ee46123a638 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -153,55 +153,21 @@ 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 Object & object) { + return not object.is() or + not object.as().is_added or + not object.as().object_controller.isAutoware() or + NonStandardOperation::isEngaged(name); + }, + each); + })) { + return activateNonUserDefinedControllers(); } else if (currentScenarioDefinition()) { currentScenarioDefinition()->evaluate(); } else { @@ -295,6 +261,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."); } diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index fe9e83ca836..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 autoware_state; } +auto EgoEntity::getCurrentAction() const -> std::string { return getLegacyAutowareState(); } auto EgoEntity::getBehaviorParameter() const -> traffic_simulator_msgs::msg::BehaviorParameter {