diff --git a/external/concealer/CMakeLists.txt b/external/concealer/CMakeLists.txt index 5a0ffb32056..0c05bfd2e30 100644 --- a/external/concealer/CMakeLists.txt +++ b/external/concealer/CMakeLists.txt @@ -14,11 +14,9 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME} SHARED - src/autoware.cpp src/autoware_universe.cpp src/execute.cpp src/field_operator_application.cpp - src/field_operator_application_for_autoware_universe.cpp src/is_package_exists.cpp src/task_queue.cpp) diff --git a/external/concealer/include/concealer/autoware.hpp b/external/concealer/include/concealer/autoware.hpp deleted file mode 100644 index fcda771cf9f..00000000000 --- a/external/concealer/include/concealer/autoware.hpp +++ /dev/null @@ -1,83 +0,0 @@ -// 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__AUTOWARE_HPP_ -#define CONCEALER__AUTOWARE_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace concealer -{ -/** - * Provides an abstraction to communicate with Autoware in order to: - * - receive vehicle commands to simulate vehicle kinematics - * - provide vehicle state reports on an appropriate topics - * NOTE: This class is intended to be move to simple_sensor_simulator - */ -class Autoware : public rclcpp::Node, public ContinuousTransformBroadcaster -{ -protected: - std::atomic current_acceleration; - - std::atomic current_twist; - - std::atomic current_pose; - -public: - CONCEALER_PUBLIC explicit Autoware(); - - virtual auto getAcceleration() const -> double = 0; - - virtual auto getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand; - - virtual auto getSteeringAngle() const -> double = 0; - - virtual auto getVelocity() const -> double = 0; - - // returns -1.0 when gear is reverse and 1.0 otherwise - virtual auto getGearSign() const -> double = 0; - - virtual auto getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand; - - virtual auto getVehicleCommand() const - -> std::tuple = 0; - - virtual auto getRouteLanelets() const -> std::vector = 0; - - virtual auto getControlModeReport() const -> autoware_vehicle_msgs::msg::ControlModeReport = 0; - - auto set(const geometry_msgs::msg::Accel &) -> void; - - auto set(const geometry_msgs::msg::Twist &) -> void; - - auto set(const geometry_msgs::msg::Pose &) -> void; - - virtual auto setManualMode() -> void = 0; - - virtual auto rethrow() -> void; -}; -} // namespace concealer - -#endif // CONCEALER__AUTOWARE_HPP_ diff --git a/external/concealer/include/concealer/autoware_stream.hpp b/external/concealer/include/concealer/autoware_stream.hpp deleted file mode 100644 index 5a13a61b276..00000000000 --- a/external/concealer/include/concealer/autoware_stream.hpp +++ /dev/null @@ -1,31 +0,0 @@ -// 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__AUTOWARE_STREAM_HPP_ -#define CONCEALER__AUTOWARE_STREAM_HPP_ - -#define AUTOWARE_INFO_STREAM(...) \ - RCLCPP_INFO_STREAM(get_logger(), "\x1b[32m" << __VA_ARGS__ << "\x1b[0m") - -#define AUTOWARE_WARN_STREAM(...) \ - RCLCPP_WARN_STREAM(get_logger(), "\x1b[33m" << __VA_ARGS__ << "\x1b[0m") - -#define AUTOWARE_ERROR_STREAM(...) \ - RCLCPP_ERROR_STREAM(get_logger(), "\x1b[1;31m" << __VA_ARGS__ << "\x1b[0m") - -#define AUTOWARE_SYSTEM_ERROR(FROM) \ - AUTOWARE_ERROR_STREAM( \ - "Error on calling " FROM ": " << std::system_error(errno, std::system_category()).what()) - -#endif // CONCEALER__AUTOWARE_STREAM_HPP_ diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 0692c7717c0..34c1bda7afd 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -15,28 +15,33 @@ #ifndef CONCEALER__AUTOWARE_UNIVERSE_HPP_ #define CONCEALER__AUTOWARE_UNIVERSE_HPP_ +#include #include #include +#include #include #include +#include #include #include #include -#include +#include #include #include +#include #include +#include +#include #include +#include #include namespace concealer { -/* - * Implements Autoware interface for Autoware Universe - * NOTE: This class is intended to be move to simple_sensor_simulator - */ -class AutowareUniverse : public Autoware +class AutowareUniverse : public rclcpp::Node, + public ContinuousTransformBroadcaster { +public: // clang-format off using AccelWithCovarianceStamped = geometry_msgs::msg::AccelWithCovarianceStamped; using Control = autoware_control_msgs::msg::Control; @@ -44,6 +49,7 @@ class AutowareUniverse : public Autoware using ControlModeReport = autoware_vehicle_msgs::msg::ControlModeReport; using GearCommand = autoware_vehicle_msgs::msg::GearCommand; using GearReport = autoware_vehicle_msgs::msg::GearReport; + using Odometry = nav_msgs::msg::Odometry; using PathWithLaneId = tier4_planning_msgs::msg::PathWithLaneId; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; using SteeringReport = autoware_vehicle_msgs::msg::SteeringReport; @@ -51,21 +57,26 @@ class AutowareUniverse : public Autoware using TurnIndicatorsReport = autoware_vehicle_msgs::msg::TurnIndicatorsReport; using VelocityReport = autoware_vehicle_msgs::msg::VelocityReport; - SubscriberWrapper getCommand; - SubscriberWrapper getGearCommandImpl; - SubscriberWrapper getTurnIndicatorsCommand; - SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getCommand; + SubscriberWrapper getGearCommand; + SubscriberWrapper getTurnIndicatorsCommand; + SubscriberWrapper getPathWithLaneId; PublisherWrapper setAcceleration; - PublisherWrapper setOdometry; + PublisherWrapper setOdometry; PublisherWrapper setPose; PublisherWrapper setSteeringReport; PublisherWrapper setGearReport; PublisherWrapper setControlModeReport; PublisherWrapper setVelocityReport; PublisherWrapper setTurnIndicatorsReport; + + std::atomic current_acceleration; + std::atomic current_pose; + std::atomic current_twist; // clang-format on +private: rclcpp::Service::SharedPtr control_mode_request_server; const rclcpp::TimerBase::SharedPtr localization_update_timer; @@ -82,38 +93,25 @@ class AutowareUniverse : public Autoware std::exception_ptr thrown; - auto stopAndJoin() -> void; - public: CONCEALER_PUBLIC explicit AutowareUniverse(bool); ~AutowareUniverse(); - auto rethrow() -> void override; - - auto getAcceleration() const -> double override; - - auto getSteeringAngle() const -> double override; - - auto getVelocity() const -> double override; + auto rethrow() -> void; auto updateLocalization() -> void; auto updateVehicleState() -> void; - auto getGearCommand() const -> GearCommand override; - - auto getGearSign() const -> double override; + auto getVehicleCommand() const -> std::tuple; - auto getVehicleCommand() const -> std::tuple override; + auto getRouteLanelets() const -> std::vector; - auto getRouteLanelets() const -> std::vector override; + auto getControlModeReport() const -> ControlModeReport; - auto getControlModeReport() const -> ControlModeReport override; - - auto setManualMode() -> void override; + auto setManualMode() -> void; }; - } // namespace concealer #endif // CONCEALER__AUTOWARE_UNIVERSE_HPP_ diff --git a/external/concealer/include/concealer/execute.hpp b/external/concealer/include/concealer/execute.hpp index b9414370ed7..b8569ee7d5a 100644 --- a/external/concealer/include/concealer/execute.hpp +++ b/external/concealer/include/concealer/execute.hpp @@ -24,8 +24,6 @@ auto execute(const std::vector &) -> int; // Emulates shell's $(...) expression. auto dollar(const std::string & command) -> std::string; - -void sudokill(const pid_t process_id); } // namespace concealer #endif // CONCEALER__EXECUTE_HPP_ diff --git a/external/concealer/include/concealer/field_operator_application.hpp b/external/concealer/include/concealer/field_operator_application.hpp index 2e35afb5ef6..db11a70de50 100644 --- a/external/concealer/include/concealer/field_operator_application.hpp +++ b/external/concealer/include/concealer/field_operator_application.hpp @@ -15,32 +15,46 @@ #ifndef CONCEALER__AUTOWARE_USER_HPP_ #define CONCEALER__AUTOWARE_USER_HPP_ -// #define CONCEALER_ISOLATE_STANDARD_OUTPUT - #include +#if __has_include() +#include +#endif + #include +#include +#include +#include +#include #include +#include +#include #include -#include -#include +#include #include +#include +#include +#include #include #include #include -#include #include #include -#include +#include #include +#include +#include +#include +#include +#include +#include +#include +#include #include #include namespace concealer { -template -class FieldOperatorApplicationFor; - /* ---- NOTE ------------------------------------------------------------------- * * The magic class 'FieldOperatorApplication' is a class that makes it easy to work with @@ -54,34 +68,74 @@ class FieldOperatorApplicationFor; * initialize, plan, and engage. * * -------------------------------------------------------------------------- */ -class FieldOperatorApplication : public rclcpp::Node +struct FieldOperatorApplication : public rclcpp::Node, + public TransitionAssertion { std::atomic is_stop_requested = false; bool is_autoware_exited = false; - auto checkAutowareProcess() -> void; - -protected: const pid_t process_id = 0; - TaskQueue task_queue; - bool initialize_was_called = false; - auto stopRequest() noexcept -> void; - - auto isStopRequested() const noexcept -> bool; - - // this method is purely virtual because different Autoware types are killed differently - // currently, we are not sure why this is the case so detailed investigation is needed - virtual auto sendSIGINT() -> void = 0; - - // method called in destructor of a derived class - // because it is difficult to differentiate shutting down behavior in destructor of a base class - auto shutdownAutoware() -> void; + std::string autoware_state; + + tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; + + std::string minimum_risk_maneuver_state; + + std::string minimum_risk_maneuver_behavior; + + // clang-format off + using AutowareState = autoware_system_msgs::msg::AutowareState; + using Control = autoware_control_msgs::msg::Control; + using CooperateStatusArray = tier4_rtc_msgs::msg::CooperateStatusArray; + 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 PathWithLaneId = tier4_planning_msgs::msg::PathWithLaneId; + using Trajectory = tier4_planning_msgs::msg::Trajectory; + using TurnIndicatorsCommand = autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + using CooperateCommands = tier4_rtc_msgs::srv::CooperateCommands; + using Engage = tier4_external_api_msgs::srv::Engage; + using InitializeLocalization = autoware_adapi_v1_msgs::srv::InitializeLocalization; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using AutoModeWithModule = tier4_rtc_msgs::srv::AutoModeWithModule; + using SetVelocityLimit = tier4_external_api_msgs::srv::SetVelocityLimit; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + + SubscriberWrapper getAutowareState; + SubscriberWrapper getCommand; + SubscriberWrapper getCooperateStatusArray; + SubscriberWrapper getEmergencyState; +#if __has_include() + SubscriberWrapper getLocalizationState; +#endif + SubscriberWrapper getMrmState; + SubscriberWrapper getPathWithLaneId; + SubscriberWrapper getTrajectory; + SubscriberWrapper getTurnIndicatorsCommand; + + ServiceWithValidation requestClearRoute; + ServiceWithValidation requestCooperateCommands; + ServiceWithValidation requestEngage; + ServiceWithValidation requestInitialPose; + ServiceWithValidation requestSetRoutePoints; + ServiceWithValidation requestSetRtcAutoMode; + ServiceWithValidation requestSetVelocityLimit; + ServiceWithValidation requestEnableAutowareControl; + // clang-format on + + /* + The task queue must be deconstructed before any services, so it must be + the last class data member. (Class data members are constructed in + declaration order and deconstructed in reverse order.) + */ + TaskQueue task_queue; -public: CONCEALER_PUBLIC explicit FieldOperatorApplication(const pid_t = 0); template @@ -90,78 +144,44 @@ class FieldOperatorApplication : public rclcpp::Node { } - ~FieldOperatorApplication() override = default; + ~FieldOperatorApplication(); auto spinSome() -> void; - /* ---- NOTE ------------------------------------------------------------------- - * - * Send an engagement request to Autoware. If Autoware does not have an - * engagement equivalent, this operation can be nop (No operation). - * - * -------------------------------------------------------------------------- */ - virtual auto engage() -> void = 0; - - virtual auto engageable() const -> bool = 0; - - virtual auto engaged() const -> bool = 0; + auto engage() -> void; - /* ---- NOTE ------------------------------------------------------------------- - * - * Send initial_pose to Autoware. - * - * -------------------------------------------------------------------------- */ - virtual auto initialize(const geometry_msgs::msg::Pose &) -> void = 0; + auto engageable() const -> bool; - /* ---- NOTE ------------------------------------------------------------------- - * - * Send the destination and route constraint points to Autoware. The argument - * route is guaranteed to be size 1 or greater, and its last element is the - * destination. When the size of a route is 2 or greater, the non-last element - * is the route constraint. That is, Autoware must go through the element - * points on the given'route' starting at index 0 and stop at index - * route.size() - 1. - * - * -------------------------------------------------------------------------- */ - virtual auto plan(const std::vector &) -> void = 0; + auto engaged() const -> bool; - virtual auto clearRoute() -> void = 0; + auto initialize(const geometry_msgs::msg::Pose &) -> void; - virtual auto getAutowareStateName() const -> std::string = 0; + auto plan(const std::vector &) -> void; - virtual auto getMinimumRiskManeuverBehaviorName() const -> std::string = 0; + auto clearRoute() -> void; - virtual auto getMinimumRiskManeuverStateName() const -> std::string = 0; + auto getAutowareStateName() const { return autoware_state; } - virtual auto getEmergencyStateName() const -> std::string = 0; + auto getMinimumRiskManeuverBehaviorName() const { return minimum_risk_maneuver_behavior; } - virtual auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray = 0; + auto getMinimumRiskManeuverStateName() const { return minimum_risk_maneuver_state; } - /* */ auto initialized() const noexcept { return initialize_was_called; } + auto getEmergencyStateName() const { return minimum_risk_maneuver_state; } - virtual auto requestAutoModeForCooperation(const std::string &, bool) -> void = 0; + auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray; - // different autowares accept different initial target speed - virtual auto restrictTargetSpeed(double) const -> double = 0; + auto initialized() const noexcept { return initialize_was_called; } - virtual auto getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand; + auto requestAutoModeForCooperation(const std::string &, bool) -> void; - virtual auto rethrow() const noexcept(false) -> void; + auto rethrow() const { task_queue.rethrow(); } - virtual auto sendCooperateCommand(const std::string &, const std::string &) -> void = 0; + auto sendCooperateCommand(const std::string &, const std::string &) -> void; - virtual auto setVelocityLimit(double) -> void = 0; + auto setVelocityLimit(double) -> void; - virtual auto enableAutowareControl() -> void = 0; + auto enableAutowareControl() -> void; }; } // namespace concealer -namespace autoware_vehicle_msgs::msg -{ -auto operator<<(std::ostream &, const TurnIndicatorsCommand &) -> std::ostream &; - -auto operator>>(std::istream &, TurnIndicatorsCommand &) -> std::istream &; -} // 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 deleted file mode 100644 index cd6e156170c..00000000000 --- a/external/concealer/include/concealer/field_operator_application_for_autoware_universe.hpp +++ /dev/null @@ -1,205 +0,0 @@ -// 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__AUTOWARE_UNIVERSE_USER_HPP_ -#define CONCEALER__AUTOWARE_UNIVERSE_USER_HPP_ - -#if __has_include() -#include -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace concealer -{ -template <> -class FieldOperatorApplicationFor -: public FieldOperatorApplication, - public TransitionAssertion> -{ - friend struct TransitionAssertion>; - - // clang-format off - SubscriberWrapper getCommand; - SubscriberWrapper getAutowareState; - SubscriberWrapper getCooperateStatusArray; - SubscriberWrapper getEmergencyState; -#if __has_include() - SubscriberWrapper getLocalizationState; -#endif - SubscriberWrapper getMrmState; - SubscriberWrapper getTrajectory; - SubscriberWrapper getTurnIndicatorsCommandImpl; - - ServiceWithValidation requestClearRoute; - ServiceWithValidation requestCooperateCommands; - ServiceWithValidation requestEngage; - ServiceWithValidation requestInitialPose; - ServiceWithValidation requestSetRoutePoints; - ServiceWithValidation requestSetRtcAutoMode; - ServiceWithValidation requestSetVelocityLimit; - ServiceWithValidation requestEnableAutowareControl; - // clang-format on - - tier4_rtc_msgs::msg::CooperateStatusArray latest_cooperate_status_array; - - std::string autoware_state; - - std::string minimum_risk_maneuver_state; - - std::string minimum_risk_maneuver_behavior; - - auto receiveMrmState(const autoware_adapi_v1_msgs::msg::MrmState & msg) -> void; - - auto receiveEmergencyState(const tier4_external_api_msgs::msg::Emergency & msg) -> void; - - /* - NOTE: This predicate should not take the state being compared as an - argument or template parameter. Otherwise, code using this class would - need to have knowledge of the Autoware state type. - */ -#define DEFINE_STATE_PREDICATE(NAME, VALUE) \ - auto is##NAME() const noexcept { return autoware_state == #VALUE; } \ - static_assert(true, "") - - DEFINE_STATE_PREDICATE(Initializing, INITIALIZING_VEHICLE); - DEFINE_STATE_PREDICATE(WaitingForRoute, WAITING_FOR_ROUTE); - DEFINE_STATE_PREDICATE(Planning, PLANNING); - DEFINE_STATE_PREDICATE(WaitingForEngage, WAITING_FOR_ENGAGE); - DEFINE_STATE_PREDICATE(Driving, DRIVING); - DEFINE_STATE_PREDICATE(ArrivedGoal, ARRIVAL_GOAL); - DEFINE_STATE_PREDICATE(Finalizing, FINALIZING); - -#undef DEFINE_STATE_PREDICATE - -protected: - template - auto getAutowareStateString(std::uint8_t state) const -> char const * - { -#define CASE(IDENTIFIER) \ - case T::IDENTIFIER: \ - return #IDENTIFIER - - switch (state) { - CASE(INITIALIZING); - CASE(WAITING_FOR_ROUTE); - CASE(PLANNING); - CASE(WAITING_FOR_ENGAGE); - CASE(DRIVING); - CASE(ARRIVED_GOAL); - CASE(FINALIZING); - - default: - return ""; - } - -#undef CASE - } - auto sendSIGINT() -> void override; - -public: - SubscriberWrapper getPathWithLaneId; - -public: - template - CONCEALER_PUBLIC explicit FieldOperatorApplicationFor(Ts &&... xs) - : FieldOperatorApplication(std::forward(xs)...), - // clang-format off - getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), - getAutowareState("/autoware/state", rclcpp::QoS(1), *this, [this](const auto & v) { - autoware_state = getAutowareStateString(v.state); }), - getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), - getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & v) { receiveEmergencyState(v); }), -#if __has_include() - getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), -#endif - getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & v) { receiveMrmState(v); }), - getTrajectory("/api/iv_msgs/planning/scenario_planning/trajectory", rclcpp::QoS(1), *this), - getTurnIndicatorsCommandImpl("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), - requestClearRoute("/api/routing/clear_route", *this), - requestCooperateCommands("/api/external/set/rtc_commands", *this), - requestEngage("/api/external/set/engage", *this), - requestInitialPose("/api/localization/initialize", *this), - // 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), - requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this), - getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this) - // clang-format on - { - } - - ~FieldOperatorApplicationFor() override; - - auto engage() -> void override; - - auto engageable() const -> bool override; - - auto engaged() const -> bool override; - - auto getAutowareStateName() const -> std::string override; - - auto getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray override; - - auto getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand override; - - auto getEmergencyStateName() const -> std::string override; - - auto getMinimumRiskManeuverBehaviorName() const -> std::string override; - - auto getMinimumRiskManeuverStateName() const -> std::string override; - - auto initialize(const geometry_msgs::msg::Pose &) -> void override; - - auto plan(const std::vector &) -> void override; - - auto clearRoute() -> void override; - - auto requestAutoModeForCooperation(const std::string &, bool) -> void override; - - auto restrictTargetSpeed(double) const -> double override; - - auto sendCooperateCommand(const std::string &, const std::string &) -> void override; - - auto setVelocityLimit(double) -> void override; - - auto enableAutowareControl() -> void override; -}; -} // namespace concealer - -#endif // CONCEALER__AUTOWARE_UNIVERSE_USER_HPP_ diff --git a/external/concealer/include/concealer/has_data_member_allow_goal_modification.hpp b/external/concealer/include/concealer/has_data_member_allow_goal_modification.hpp deleted file mode 100644 index 56de7a1f9f7..00000000000 --- a/external/concealer/include/concealer/has_data_member_allow_goal_modification.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// 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__HAS_DATA_MEMBER_ALLOW_GOAL_MODIFICATION_HPP_ -#define CONCEALER__HAS_DATA_MEMBER_ALLOW_GOAL_MODIFICATION_HPP_ - -#include - -namespace concealer -{ -template -struct has_data_member_allow_goal_modification : public std::false_type -{ -}; - -template -struct has_data_member_allow_goal_modification< - T, std::void_t().allow_goal_modification)>> : public std::true_type -{ -}; - -template -inline constexpr auto has_data_member_allow_goal_modification_v = - has_data_member_allow_goal_modification::value; -} // namespace concealer - -#endif // CONCEALER__HAS_DATA_MEMBER_ALLOW_GOAL_MODIFICATION_HPP_ diff --git a/external/concealer/include/concealer/has_data_member_option.hpp b/external/concealer/include/concealer/has_data_member_option.hpp deleted file mode 100644 index 959e5fcdf44..00000000000 --- a/external/concealer/include/concealer/has_data_member_option.hpp +++ /dev/null @@ -1,37 +0,0 @@ -// 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__HAS_DATA_MEMBER_OPTION_HPP_ -#define CONCEALER__HAS_DATA_MEMBER_OPTION_HPP_ - -#include - -namespace concealer -{ -template -struct has_data_member_option : public std::false_type -{ -}; - -template -struct has_data_member_option().option)>> -: public std::true_type -{ -}; - -template -inline constexpr auto has_data_member_option_v = has_data_member_option::value; -} // namespace concealer - -#endif // CONCEALER__HAS_DATA_MEMBER_OPTION_HPP_ diff --git a/external/concealer/include/concealer/launch.hpp b/external/concealer/include/concealer/launch.hpp index 54f1459f1b3..55f4da068f3 100644 --- a/external/concealer/include/concealer/launch.hpp +++ b/external/concealer/include/concealer/launch.hpp @@ -15,13 +15,6 @@ #ifndef CONCEALER__LAUNCH_HPP_ #define CONCEALER__LAUNCH_HPP_ -#ifdef CONCEALER_ISOLATE_STANDARD_OUTPUT -#include -#include -#include -#include -#endif - #include #include #include @@ -37,14 +30,6 @@ template auto ros2_launch( const std::string & package, const std::string & file, const Parameters & parameters) { -#ifdef CONCEALER_ISOLATE_STANDARD_OUTPUT - const std::string log_filename = "/tmp/scenario_test_runner/autoware-output.txt"; - const auto fd = ::open(log_filename.c_str(), O_RDWR | O_CREAT, S_IRUSR | S_IWUSR); - ::dup2(fd, STDOUT_FILENO); - ::dup2(fd, STDERR_FILENO); - ::close(fd); -#endif - const auto argv = [&]() { auto argv = std::vector(); diff --git a/external/concealer/include/concealer/member_detector.hpp b/external/concealer/include/concealer/member_detector.hpp new file mode 100644 index 00000000000..d618347b0e9 --- /dev/null +++ b/external/concealer/include/concealer/member_detector.hpp @@ -0,0 +1,83 @@ +// 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__MEMBER_DETECTOR_HPP_ +#define CONCEALER__MEMBER_DETECTOR_HPP_ + +#include + +namespace concealer +{ +#define DEFINE_MEMBER_DETECTOR(IDENTIFIER) \ + template \ + struct DetectMember_##IDENTIFIER : public std::false_type \ + { \ + }; \ + \ + template \ + struct DetectMember_##IDENTIFIER().IDENTIFIER)>> \ + : public std::true_type \ + { \ + } + +DEFINE_MEMBER_DETECTOR(allow_goal_modification); +DEFINE_MEMBER_DETECTOR(distance); +DEFINE_MEMBER_DETECTOR(option); +DEFINE_MEMBER_DETECTOR(status); +DEFINE_MEMBER_DETECTOR(success); + +#undef DEFINE_MEMBER_DETECTOR + +#define DEFINE_STATIC_MEMBER_DETECTOR(IDENTIFIER) \ + template \ + struct DetectStaticMember_##IDENTIFIER : public std::false_type \ + { \ + }; \ + \ + template \ + struct DetectStaticMember_##IDENTIFIER> \ + : public std::true_type \ + { \ + } + +DEFINE_STATIC_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); +DEFINE_STATIC_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(AVOIDANCE_LEFT); +DEFINE_STATIC_MEMBER_DETECTOR(AVOIDANCE_RIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(BLIND_SPOT); +DEFINE_STATIC_MEMBER_DETECTOR(COMFORTABLE_STOP); +DEFINE_STATIC_MEMBER_DETECTOR(CROSSWALK); +DEFINE_STATIC_MEMBER_DETECTOR(DETECTION_AREA); +DEFINE_STATIC_MEMBER_DETECTOR(EMERGENCY_STOP); +DEFINE_STATIC_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_LEFT); +DEFINE_STATIC_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_RIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(GOAL_PLANNER); +DEFINE_STATIC_MEMBER_DETECTOR(INTERSECTION); +DEFINE_STATIC_MEMBER_DETECTOR(INTERSECTION_OCCLUSION); +DEFINE_STATIC_MEMBER_DETECTOR(LANE_CHANGE_LEFT); +DEFINE_STATIC_MEMBER_DETECTOR(LANE_CHANGE_RIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(NONE); +DEFINE_STATIC_MEMBER_DETECTOR(NO_DRIVABLE_LANE); +DEFINE_STATIC_MEMBER_DETECTOR(NO_STOPPING_AREA); +DEFINE_STATIC_MEMBER_DETECTOR(OCCLUSION_SPOT); +DEFINE_STATIC_MEMBER_DETECTOR(PULL_OUT); +DEFINE_STATIC_MEMBER_DETECTOR(PULL_OVER); +DEFINE_STATIC_MEMBER_DETECTOR(START_PLANNER); +DEFINE_STATIC_MEMBER_DETECTOR(TRAFFIC_LIGHT); +DEFINE_STATIC_MEMBER_DETECTOR(UNKNOWN); + +#undef DEFINE_STATIC_MEMBER_DETECTOR +} // namespace concealer + +#endif // CONCEALER__MEMBER_DETECTOR_HPP_ diff --git a/external/concealer/include/concealer/service_with_validation.hpp b/external/concealer/include/concealer/service_with_validation.hpp index 62421299c85..6b49df8d0f0 100644 --- a/external/concealer/include/concealer/service_with_validation.hpp +++ b/external/concealer/include/concealer/service_with_validation.hpp @@ -17,73 +17,60 @@ #include #include -#include +#include #include +#include #include #include #include #include -template -struct has_data_member_status : public std::false_type +namespace concealer { -}; - template -struct has_data_member_status().status)>> -: public std::true_type +class ServiceWithValidation { -}; - -template -constexpr auto has_data_member_status_v = has_data_member_status::value; + const std::string service_name; -template -struct has_data_member_success : public std::false_type -{ -}; + rclcpp::Logger logger; -template -struct has_data_member_success().success)>> -: public std::true_type -{ -}; + typename rclcpp::Client::SharedPtr client; -template -constexpr auto has_data_member_success_v = has_data_member_success::value; + rclcpp::WallRate validation_rate; -namespace concealer -{ -template -class ServiceWithValidation -{ public: + template explicit ServiceWithValidation( const std::string & service_name, FieldOperatorApplication & autoware, const std::chrono::nanoseconds validation_interval = std::chrono::seconds(1)) : service_name(service_name), logger(autoware.get_logger()), - client(autoware.create_client(service_name, rmw_qos_profile_default)), + client(autoware.template create_client(service_name, rmw_qos_profile_default)), validation_rate(validation_interval) { } - class TimeoutError : public common::Error + auto operator()(const typename T::Request::SharedPtr & request, std::size_t attempts_count) + -> void { - public: - template - explicit TimeoutError(Ts &&... xs) : common::Error(std::forward(xs)...) - { + while (!client->service_is_ready()) { + RCLCPP_INFO_STREAM(logger, service_name << " service is not ready."); + validation_rate.sleep(); } - }; - auto operator()(const typename T::Request::SharedPtr & request, std::size_t attempts_count = 1) - -> void - { - validateAvailability(); + auto send = [this](const auto & request) { + if (auto future = client->async_send_request(request); + future.wait_for(validation_rate.period()) == std::future_status::ready) { + return std::optional::SharedFuture>(future); + } else { + RCLCPP_ERROR_STREAM(logger, service_name << " service request has timed out."); + return std::optional::SharedFuture>(); + } + }; + for (std::size_t attempt = 0; attempt < attempts_count; ++attempt, validation_rate.sleep()) { - if (const auto & service_call_result = callWithTimeoutValidation(request)) { - if constexpr (has_data_member_status_v) { + if (const auto & service_call_result = send(request)) { + if constexpr (DetectMember_status::value) { if constexpr (std::is_same_v< tier4_external_api_msgs::msg::ResponseStatus, decltype(T::Response::status)>) { @@ -127,7 +114,7 @@ class ServiceWithValidation RCLCPP_INFO_STREAM(logger, service_name << " service request has been accepted."); return; } - } else if constexpr (has_data_member_success_v) { + } else if constexpr (DetectMember_success::value) { if constexpr (std::is_same_v) { if (service_call_result->get()->success) { RCLCPP_INFO_STREAM(logger, service_name << " service request has been accepted."); @@ -147,39 +134,11 @@ class ServiceWithValidation } } } - throw TimeoutError( + + throw common::scenario_simulator_exception::AutowareError( "Requested the service ", std::quoted(service_name), " ", attempts_count, " times, but was not successful."); } - -private: - auto validateAvailability() -> void - { - while (!client->service_is_ready()) { - RCLCPP_INFO_STREAM(logger, service_name << " service is not ready."); - validation_rate.sleep(); - } - } - - auto callWithTimeoutValidation(const typename T::Request::SharedPtr & request) - -> std::optional::SharedFuture> - { - if (auto future = client->async_send_request(request); - future.wait_for(validation_rate.period()) == std::future_status::ready) { - return future; - } else { - RCLCPP_ERROR_STREAM(logger, service_name << " service request has timed out."); - return std::nullopt; - } - } - - const std::string service_name; - - rclcpp::Logger logger; - - typename rclcpp::Client::SharedPtr client; - - rclcpp::WallRate validation_rate; }; } // namespace concealer diff --git a/external/concealer/include/concealer/subscriber_wrapper.hpp b/external/concealer/include/concealer/subscriber_wrapper.hpp index 8247a872e3a..eb4fde75af0 100644 --- a/external/concealer/include/concealer/subscriber_wrapper.hpp +++ b/external/concealer/include/concealer/subscriber_wrapper.hpp @@ -20,46 +20,39 @@ namespace concealer { -enum class ThreadSafety : bool { unsafe, safe }; - -template +template class SubscriberWrapper { - typename MessageType::ConstSharedPtr current_value = std::make_shared(); + typename Message::ConstSharedPtr current_value = std::make_shared(); - typename rclcpp::Subscription::SharedPtr subscription; + typename rclcpp::Subscription::SharedPtr subscription; public: - auto operator()() const -> decltype(auto) - { - if constexpr (thread_safety == ThreadSafety::unsafe) { - return *current_value; - } else { - return *std::atomic_load(¤t_value); - } - } + auto operator()() const -> const auto & { return *std::atomic_load(¤t_value); } - template - SubscriberWrapper( - const std::string & topic, const rclcpp::QoS & quality_of_service, - NodeInterface & autoware_interface, - const std::function & callback = {}) - : subscription(autoware_interface.template create_subscription( + template + explicit SubscriberWrapper( + const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware, + const Callback & callback) + : subscription(autoware.template create_subscription( topic, quality_of_service, - [this, callback](const typename MessageType::ConstSharedPtr message) { - if constexpr (thread_safety == ThreadSafety::safe) { - std::atomic_store(¤t_value, message); - if (current_value and callback) { - callback(*std::atomic_load(¤t_value)); - } - } else { - if (current_value = message; current_value and callback) { - callback(*current_value); - } + [this, callback](const typename Message::ConstSharedPtr & message) { + if (std::atomic_store(¤t_value, message); current_value) { + callback(*std::atomic_load(¤t_value)); } })) { } + + template + explicit SubscriberWrapper( + const std::string & topic, const rclcpp::QoS & quality_of_service, Autoware & autoware) + : subscription(autoware.template create_subscription( + topic, quality_of_service, [this](const typename Message::ConstSharedPtr & message) { + std::atomic_store(¤t_value, message); + })) + { + } }; } // namespace concealer diff --git a/external/concealer/include/concealer/task_queue.hpp b/external/concealer/include/concealer/task_queue.hpp index 8f1b84f0f0f..4440e890268 100644 --- a/external/concealer/include/concealer/task_queue.hpp +++ b/external/concealer/include/concealer/task_queue.hpp @@ -45,8 +45,6 @@ class TaskQueue public: explicit TaskQueue(); - void stopAndJoin(); - ~TaskQueue(); template diff --git a/external/concealer/include/concealer/transition_assertion.hpp b/external/concealer/include/concealer/transition_assertion.hpp index 7850899362e..ce28408a7f1 100644 --- a/external/concealer/include/concealer/transition_assertion.hpp +++ b/external/concealer/include/concealer/transition_assertion.hpp @@ -25,6 +25,7 @@ namespace concealer template struct TransitionAssertion { +protected: const std::chrono::steady_clock::time_point start; const std::chrono::seconds initialize_duration; @@ -42,16 +43,16 @@ struct TransitionAssertion #define DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(STATE) \ template \ - auto waitForAutowareStateToBe##STATE( \ + auto waitForAutowareStateToBe_##STATE( \ Thunk && thunk = [] {}, Interval interval = std::chrono::seconds(1)) \ { \ - for (thunk(); not static_cast(*this).isStopRequested() and \ - not static_cast(*this).is##STATE(); \ + for (thunk(); not static_cast(*this).is_stop_requested.load() and \ + static_cast(*this).autoware_state != #STATE; \ rclcpp::GenericRate(interval).sleep()) { \ if ( \ have_never_been_engaged and \ start + initialize_duration <= std::chrono::steady_clock::now()) { \ - const auto state = static_cast(*this).getAutowareStateName(); \ + const auto state = static_cast(*this).autoware_state; \ throw common::AutowareError( \ "Simulator waited for the Autoware state to transition to " #STATE \ ", but time is up. The current Autoware state is ", \ @@ -60,19 +61,19 @@ struct TransitionAssertion thunk(); \ } \ } \ - if constexpr (std::string_view(#STATE) == std::string_view("Driving")) { \ + if constexpr (std::string_view(#STATE) == std::string_view("DRIVING")) { \ have_never_been_engaged = false; \ } \ } \ static_assert(true) - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Initializing); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WaitingForRoute); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Planning); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WaitingForEngage); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Driving); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(ArrivedGoal); - DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(Finalizing); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(INITIALIZING); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WAITING_FOR_ROUTE); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(PLANNING); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(WAITING_FOR_ENGAGE); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(DRIVING); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(ARRIVED_GOAL); + DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE(FINALIZING); #undef DEFINE_WAIT_FOR_AUTOWARE_STATE_TO_BE }; diff --git a/external/concealer/src/autoware.cpp b/external/concealer/src/autoware.cpp deleted file mode 100644 index f29cbd93e2d..00000000000 --- a/external/concealer/src/autoware.cpp +++ /dev/null @@ -1,59 +0,0 @@ -// 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. - -#include - -namespace concealer -{ -Autoware::Autoware() -: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), - current_acceleration(geometry_msgs::msg::Accel()), - current_twist(geometry_msgs::msg::Twist()), - current_pose(geometry_msgs::msg::Pose()) -{ -} - -auto Autoware::getGearCommand() const -> autoware_vehicle_msgs::msg::GearCommand -{ - static auto gear_command = []() { - autoware_vehicle_msgs::msg::GearCommand gear_command; - gear_command.command = autoware_vehicle_msgs::msg::GearCommand::DRIVE; - return gear_command; - }(); - gear_command.stamp = now(); - return gear_command; -} - -auto Autoware::set(const geometry_msgs::msg::Accel & acceleration) -> void -{ - current_acceleration.store(acceleration); -} - -auto Autoware::set(const geometry_msgs::msg::Twist & twist) -> void { current_twist.store(twist); } - -auto Autoware::set(const geometry_msgs::msg::Pose & pose) -> void { current_pose.store(pose); } - -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; - return turn_indicators_command; - }(); - turn_indicators_command.stamp = now(); - return turn_indicators_command; -} - -auto Autoware::rethrow() -> void {} -} // namespace concealer diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index da3d240d571..7467290ce20 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -17,8 +17,9 @@ namespace concealer { AutowareUniverse::AutowareUniverse(bool simulate_localization) -: getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), - getGearCommandImpl("/control/command/gear_cmd", rclcpp::QoS(1), *this), +: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), + getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), + getGearCommand("/control/command/gear_cmd", rclcpp::QoS(1), *this), getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), getPathWithLaneId( "/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), @@ -50,21 +51,20 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) response->success = true; } else if (request->mode == ControlModeCommand::Request::MANUAL) { /* - NOTE: - MANUAL request will come when a remote override is triggered. - But scenario_simulator_v2 don't support a remote override for now. + NOTE: MANUAL request will come when a remote override is triggered. + But scenario_simulator_v2 don't support a remote override for now. */ response->success = false; } else { response->success = false; } })), - // Autoware.Universe requires localization topics to send data at 50Hz - localization_update_timer(rclcpp::create_timer( - this, get_clock(), std::chrono::milliseconds(20), [this]() { updateLocalization(); })), - // Autoware.Universe requires vehicle state topics to send data at 30Hz - vehicle_state_update_timer(rclcpp::create_timer( - this, get_clock(), std::chrono::milliseconds(33), [this]() { updateVehicleState(); })), + localization_update_timer( + rclcpp::create_timer( // Autoware.Universe requires localization topics to send data at 50Hz + this, get_clock(), std::chrono::milliseconds(20), [this]() { updateLocalization(); })), + vehicle_state_update_timer( + rclcpp::create_timer( // Autoware.Universe requires vehicle state topics to send data at 30Hz + this, get_clock(), std::chrono::milliseconds(33), [this]() { updateVehicleState(); })), localization_and_vehicle_state_update_thread(std::thread([this]() { try { while (rclcpp::ok() and not is_stop_requested.load()) { @@ -78,31 +78,17 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) { } -AutowareUniverse::~AutowareUniverse() { stopAndJoin(); } - -auto AutowareUniverse::rethrow() -> void -{ - if (is_thrown.load()) { - throw thrown; - } -} - -auto AutowareUniverse::stopAndJoin() -> void +AutowareUniverse::~AutowareUniverse() { is_stop_requested.store(true); localization_and_vehicle_state_update_thread.join(); } -auto AutowareUniverse::getAcceleration() const -> double -{ - return getCommand().longitudinal.acceleration; -} - -auto AutowareUniverse::getVelocity() const -> double { return getCommand().longitudinal.velocity; } - -auto AutowareUniverse::getSteeringAngle() const -> double +auto AutowareUniverse::rethrow() -> void { - return getCommand().lateral.steering_tire_angle; + if (is_thrown.load()) { + throw thrown; + } } auto AutowareUniverse::updateLocalization() -> void @@ -122,7 +108,7 @@ auto AutowareUniverse::updateLocalization() -> void }()); setOdometry([this]() { - nav_msgs::msg::Odometry message; + Odometry message; message.header.stamp = get_clock()->now(); message.header.frame_id = "map"; message.pose.pose = current_pose.load(); @@ -133,7 +119,7 @@ auto AutowareUniverse::updateLocalization() -> void setPose([this]() { // See https://github.com/tier4/autoware.universe/blob/45ab20af979c5663e5a8d4dda787b1dea8d6e55b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L785-L803 - geometry_msgs::msg::PoseWithCovarianceStamped message; + PoseWithCovarianceStamped message; message.header.stamp = get_clock()->now(); message.header.frame_id = "map"; message.pose.pose = current_pose.load(); @@ -163,7 +149,7 @@ auto AutowareUniverse::updateVehicleState() -> void setSteeringReport([this]() { SteeringReport message; message.stamp = get_clock()->now(); - message.steering_tire_angle = getSteeringAngle(); + message.steering_tire_angle = getCommand().lateral.steering_tire_angle; return message; }()); @@ -186,21 +172,40 @@ auto AutowareUniverse::updateVehicleState() -> void }()); } -auto AutowareUniverse::getGearCommand() const -> GearCommand { return getGearCommandImpl(); } - -auto AutowareUniverse::getGearSign() const -> double +auto AutowareUniverse::getVehicleCommand() const -> std::tuple { - /// @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 - getGearCommand().command == GearCommand::REVERSE_2 - ? -1.0 - : 1.0; -} - -auto AutowareUniverse::getVehicleCommand() const -> std::tuple -{ - return std::make_tuple(getCommand(), getGearCommand()); + const auto control_command = getCommand(); + + const auto gear_command = getGearCommand(); + + auto sign_of = [](auto command) { + switch (command) { + case GearCommand::REVERSE: + case GearCommand::REVERSE_2: + return -1.0; + case GearCommand::NONE: + return 0.0; + default: + return 1.0; + } + }; + + /* + TODO Currently, acceleration is returned as an unsigned value + (`control_command.longitudinal.acceleration`) and a signed value + (`sign_of(gear_command.command)`), but this is for historical reasons and + there is no longer any reason to do so. + + return std::make_tuple( + control_command.longitudinal.velocity, + sign_of(gear_command.command) * control_command.longitudinal.acceleration, + control_command.lateral.steering_tire_angle, + gear_command.command); + */ + return std::make_tuple( + control_command.longitudinal.velocity, control_command.longitudinal.acceleration, + control_command.lateral.steering_tire_angle, sign_of(gear_command.command), + gear_command.command); } auto AutowareUniverse::getRouteLanelets() const -> std::vector diff --git a/external/concealer/src/execute.cpp b/external/concealer/src/execute.cpp index 301e48de81a..b6a3c668700 100644 --- a/external/concealer/src/execute.cpp +++ b/external/concealer/src/execute.cpp @@ -71,22 +71,4 @@ auto dollar(const std::string & command) -> std::string return result; } } - -void sudokill(const pid_t process_id) -{ - const auto process_str = std::to_string(process_id); - - const pid_t pid = fork(); - - switch (pid) { - case -1: - std::cout << std::system_error(errno, std::system_category()).what() << std::endl; - break; - case 0: - ::execlp("sudo", "sudo", "kill", "-2", process_str.c_str(), static_cast(nullptr)); - std::cout << std::system_error(errno, std::system_category()).what() << std::endl; - break; - } -} - } // namespace concealer diff --git a/external/concealer/src/field_operator_application.cpp b/external/concealer/src/field_operator_application.cpp index 747c204a68c..1c7cf0d04e7 100644 --- a/external/concealer/src/field_operator_application.cpp +++ b/external/concealer/src/field_operator_application.cpp @@ -12,7 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include +#include +#include #include #include #include @@ -20,58 +23,197 @@ namespace concealer { -FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) -: rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), - process_id(pid) +template +auto toModuleType(const std::string & module_name) { -} + static const std::unordered_map module_type_map = [&]() { + std::unordered_map module_type_map; -auto FieldOperatorApplication::stopRequest() noexcept -> void { is_stop_requested.store(true); } +#define EMPLACE(IDENTIFIER) \ + if constexpr (DetectStaticMember_##IDENTIFIER::value) { \ + module_type_map.emplace(#IDENTIFIER, T::IDENTIFIER); \ + } \ + static_assert(true) -auto FieldOperatorApplication::isStopRequested() const noexcept -> bool -{ - return is_stop_requested.load(); + /* + The following elements are in order of definition in the + tier4_rtc_msgs/msg/Module.msg file. Of course, unordered_map doesn't + preserve the insertion order, so the order itself doesn't matter. + */ + EMPLACE(NONE); + EMPLACE(LANE_CHANGE_LEFT); + EMPLACE(LANE_CHANGE_RIGHT); + EMPLACE(AVOIDANCE_LEFT); + EMPLACE(AVOIDANCE_RIGHT); + EMPLACE(GOAL_PLANNER); + EMPLACE(START_PLANNER); + EMPLACE(PULL_OUT); + EMPLACE(TRAFFIC_LIGHT); + EMPLACE(INTERSECTION); + EMPLACE(INTERSECTION_OCCLUSION); + EMPLACE(CROSSWALK); + EMPLACE(BLIND_SPOT); + EMPLACE(DETECTION_AREA); + EMPLACE(NO_STOPPING_AREA); + EMPLACE(OCCLUSION_SPOT); + EMPLACE(EXT_REQUEST_LANE_CHANGE_LEFT); + EMPLACE(EXT_REQUEST_LANE_CHANGE_RIGHT); + EMPLACE(AVOIDANCE_BY_LC_LEFT); + EMPLACE(AVOIDANCE_BY_LC_RIGHT); + EMPLACE(NO_DRIVABLE_LANE); + +#undef EMPLACE + + return module_type_map; + }(); + + if (const auto module_type = module_type_map.find(module_name); + module_type == module_type_map.end()) { + throw common::Error( + "Unexpected module name for tier4_rtc_msgs::msg::Module: ", module_name, "."); + } else { + return module_type->second; + } } -auto FieldOperatorApplication::spinSome() -> void +template +bool isValidCooperateStatus( + const CooperateStatusType & cooperate_status, std::uint8_t command_type, std::uint8_t module_type) { - if (rclcpp::ok() and not isStopRequested()) { - checkAutowareProcess(); - rclcpp::spin_some(get_node_base_interface()); + /** + * NOTE1: the finish_distance filter is set to over -20.0, + * because some valid rtc statuses has negative finish_distance due to the errors of + * localization or numerical calculation. This threshold is advised by a member of TIER IV + * planning and control team. + */ + + /** + * NOTE2: The difference in the variable referred as a distance is the impact of the + * message specification changes in the following URL. + * This was also decided after consulting with a member of TIER IV planning and control team. + * ref: https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef + */ + if constexpr (DetectMember_distance::value) { + return cooperate_status.module.type == module_type && + command_type != cooperate_status.command_status.type && + cooperate_status.distance >= -20.0; + } else { + return cooperate_status.module.type == module_type && + command_type != cooperate_status.command_status.type && + cooperate_status.finish_distance >= -20.0; } } -auto FieldOperatorApplication::checkAutowareProcess() -> void -{ - if (process_id != 0) { - int wstatus = 0; - int ret = waitpid(process_id, &wstatus, WNOHANG); - if (ret == 0) { - return; - } else if (ret < 0) { - if (errno == ECHILD) { - is_autoware_exited = true; - throw common::AutowareError("Autoware process is already terminated"); - } else { - AUTOWARE_SYSTEM_ERROR("waitpid"); - std::exit(EXIT_FAILURE); +// clang-format off +FieldOperatorApplication::FieldOperatorApplication(const pid_t pid) +: rclcpp::Node("concealer_user", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), + process_id(pid), + 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 ""; } - } + }; - if (WIFEXITED(wstatus)) { - is_autoware_exited = true; - throw common::AutowareError( - "Autoware process is unintentionally exited. exit code: ", WEXITSTATUS(wstatus)); - } else if (WIFSIGNALED(wstatus)) { - is_autoware_exited = true; - throw common::AutowareError("Autoware process is killed. signal is ", WTERMSIG(wstatus)); + autoware_state = state_name_of(message.state); + }), + getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), + getCooperateStatusArray("/api/external/get/rtc_status", rclcpp::QoS(1), *this, [this](const auto & v) { latest_cooperate_status_array = v; }), + getEmergencyState("/api/external/get/emergency", rclcpp::QoS(1), *this, [this](const auto & message) { + if (message.emergency) { + throw common::Error("Emergency state received"); } - } + }), +#if __has_include() + getLocalizationState("/api/localization/initialization_state", rclcpp::QoS(1).transient_local(), *this), +#endif + getMrmState("/api/fail_safe/mrm_state", rclcpp::QoS(1), *this, [this](const auto & message) { + auto state_name_of = [](auto state) constexpr { + switch (state) { + case MrmState::MRM_FAILED: + return "MRM_FAILED"; + case MrmState::MRM_OPERATING: + return "MRM_OPERATING"; + case MrmState::MRM_SUCCEEDED: + return "MRM_SUCCEEDED"; + case MrmState::NORMAL: + return "NORMAL"; + case MrmState::UNKNOWN: + return "UNKNOWN"; + default: + throw common::Error( + "Unexpected autoware_adapi_v1_msgs::msg::MrmState::state, number: ", state); + } + }; + + auto behavior_name_of = [](auto behavior) constexpr { + if constexpr (DetectStaticMember_COMFORTABLE_STOP::value) { + if (behavior == MrmState::COMFORTABLE_STOP) { + return "COMFORTABLE_STOP"; + } + } + if constexpr (DetectStaticMember_EMERGENCY_STOP::value) { + if (behavior == MrmState::EMERGENCY_STOP) { + return "EMERGENCY_STOP"; + } + } + if constexpr (DetectStaticMember_NONE::value) { + if (behavior == MrmState::NONE) { + return "NONE"; + } + } + if constexpr (DetectStaticMember_UNKNOWN::value) { + if (behavior == MrmState::UNKNOWN) { + return "UNKNOWN"; + } + } + if constexpr (DetectStaticMember_PULL_OVER::value) { + if (behavior == MrmState::PULL_OVER) { + return "PULL_OVER"; + } + } + throw common::Error( + "Unexpected autoware_adapi_v1_msgs::msg::MrmState::behavior, number: ", behavior); + }; + + minimum_risk_maneuver_state = state_name_of(message.state); + minimum_risk_maneuver_behavior = behavior_name_of(message.behavior); + }), + getPathWithLaneId("/planning/scenario_planning/lane_driving/behavior_planning/path_with_lane_id", rclcpp::QoS(1), *this), + 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), + requestCooperateCommands("/api/external/set/rtc_commands", *this), + requestEngage("/api/external/set/engage", *this), + requestInitialPose("/api/localization/initialize", *this), + // 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), + requestEnableAutowareControl("/api/operation_mode/enable_autoware_control", *this) +{ } +// clang-format on -auto FieldOperatorApplication::shutdownAutoware() -> void +FieldOperatorApplication::~FieldOperatorApplication() { - if (stopRequest(); process_id != 0 && not std::exchange(is_autoware_exited, true)) { + if (is_stop_requested.store(true); + process_id != 0 && not std::exchange(is_autoware_exited, true)) { const auto sigset = [this]() { if (auto signal_set = sigset_t(); sigemptyset(&signal_set) or sigaddset(&signal_set, SIGCHLD)) { @@ -98,7 +240,7 @@ auto FieldOperatorApplication::shutdownAutoware() -> void return timeout; }(); - if (sendSIGINT(); sigtimedwait(&sigset, nullptr, &timeout) < 0) { + if (::kill(process_id, SIGINT); sigtimedwait(&sigset, nullptr, &timeout) < 0) { switch (errno) { case EINTR: /* @@ -146,72 +288,271 @@ auto FieldOperatorApplication::shutdownAutoware() -> void } } -auto FieldOperatorApplication::getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand +auto FieldOperatorApplication::clearRoute() -> void { - static auto turn_indicators_command = []() { - autoware_vehicle_msgs::msg::TurnIndicatorsCommand turn_indicators_command; - turn_indicators_command.command = autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND; - return turn_indicators_command; - }(); - turn_indicators_command.stamp = now(); - return turn_indicators_command; + task_queue.delay([this] { + /* + Since this service tends to be available long after the launch of + Autoware, set the attempts_count to a high value. There is no technical + basis for the number 30. + */ + requestClearRoute(std::make_shared(), 30); + }); } -auto FieldOperatorApplication::rethrow() const -> void { task_queue.rethrow(); } -} // namespace concealer +auto FieldOperatorApplication::enableAutowareControl() -> void +{ + task_queue.delay([this]() { + auto request = std::make_shared(); + requestEnableAutowareControl(request, 30); + }); +} -namespace autoware_vehicle_msgs::msg +auto FieldOperatorApplication::engage() -> void { -auto operator<<( - std::ostream & out, const autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) - -> std::ostream & + task_queue.delay([this]() { + waitForAutowareStateToBe_DRIVING([this]() { + auto request = std::make_shared(); + request->engage = true; + try { + return requestEngage(request, 1); + } catch (const common::AutowareError &) { + return; // Ignore error because this service is validated by Autoware state transition. + } + }); + }); +} + +auto FieldOperatorApplication::engageable() const -> bool { -#define CASE(IDENTIFIER) \ - case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER: \ - out << #IDENTIFIER; \ - break + rethrow(); + return task_queue.exhausted() and autoware_state == "WAITING_FOR_ENGAGE"; +} - switch (message.command) { - CASE(DISABLE); - CASE(ENABLE_LEFT); - CASE(ENABLE_RIGHT); - CASE(NO_COMMAND); +auto FieldOperatorApplication::engaged() const -> bool +{ + rethrow(); + return task_queue.exhausted() and autoware_state == "DRIVING"; +} - default: - throw common::Error( - "Unsupported TurnIndicatorsCommand, state number : ", static_cast(message.command)); +auto FieldOperatorApplication::getWaypoints() const -> traffic_simulator_msgs::msg::WaypointsArray +{ + traffic_simulator_msgs::msg::WaypointsArray waypoints; + + for (const auto & point : getTrajectory().points) { + waypoints.waypoints.emplace_back(point.pose.position); } -#undef CASE + return waypoints; +} + +auto FieldOperatorApplication::initialize(const geometry_msgs::msg::Pose & initial_pose) -> void +{ + if (not std::exchange(initialize_was_called, true)) { + task_queue.delay([this, initial_pose]() { + waitForAutowareStateToBe_WAITING_FOR_ROUTE([&]() { +#if __has_include() + if (getLocalizationState().state != LocalizationInitializationState::UNINITIALIZED) { + return; + } +#endif + geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_msg; + initial_pose_msg.header.stamp = get_clock()->now(); + initial_pose_msg.header.frame_id = "map"; + initial_pose_msg.pose.pose = initial_pose; + + auto request = + std::make_shared(); + request->pose.push_back(initial_pose_msg); + try { + return requestInitialPose(request, 1); + } catch (const common::AutowareError &) { + return; // Ignore error because this service is validated by Autoware state transition. + } + }); + }); + } +} + +auto FieldOperatorApplication::plan(const std::vector & route) + -> void +{ + assert(not route.empty()); + + task_queue.delay([this, route] { + waitForAutowareStateToBe_WAITING_FOR_ROUTE(); // NOTE: This is assertion. + + auto request = std::make_shared(); + + 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 = + get_parameter("allow_goal_modification").get_value(); + } + + 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, 1); - return out; + waitForAutowareStateToBe_WAITING_FOR_ENGAGE(); + }); } -auto operator>>(std::istream & is, autoware_vehicle_msgs::msg::TurnIndicatorsCommand & message) - -> std::istream & +auto FieldOperatorApplication::requestAutoModeForCooperation( + const std::string & module_name, bool enable) -> void { -#define STATE(IDENTIFIER) \ - {#IDENTIFIER, autoware_vehicle_msgs::msg::TurnIndicatorsCommand::IDENTIFIER} + /* + The implementation of this function will not work properly if the + `rtc_auto_mode_manager` package is present. + */ + if (not isPackageExists("rtc_auto_mode_manager")) { + task_queue.delay([this, module_name, enable]() { + auto request = std::make_shared(); + request->module.type = toModuleType(module_name); + request->enable = enable; + /* + We attempt to resend the service up to 30 times, but this number of + times was determined by heuristics, not for any technical reason. + */ + requestSetRtcAutoMode(request, 30); + }); + } else { + throw common::Error( + "FieldOperatorApplication::requestAutoModeForCooperation is not supported in this " + "environment, because rtc_auto_mode_manager is present."); + } +} - std::unordered_map state_dictionary{ - STATE(DISABLE), - STATE(ENABLE_LEFT), - STATE(ENABLE_RIGHT), - STATE(NO_COMMAND), +auto FieldOperatorApplication::sendCooperateCommand( + const std::string & module_name, const std::string & command) -> void +{ + auto to_command_type = [](const auto & command) { + static const std::unordered_map command_type_map = { + {"ACTIVATE", tier4_rtc_msgs::msg::Command::ACTIVATE}, + {"DEACTIVATE", tier4_rtc_msgs::msg::Command::DEACTIVATE}, + }; + if (const auto command_type = command_type_map.find(command); + command_type == command_type_map.end()) { + throw common::Error("Unexpected command for tier4_rtc_msgs::msg::Command: ", command, "."); + } else { + return command_type->second; + } }; -#undef STATE + /* + NOTE: Used cooperate statuses will be deleted correctly in Autoware side + and provided via topic update. But, their update rate (typ. 10Hz) is lower + than the one of scenario_simulator_v2. So, we need to check cooperate + statuses if they are used or not in scenario_simulator_v2 side to avoid + sending the same cooperate command when sending multiple commands between + updates of cooperate statuses. + */ + static std::vector used_cooperate_statuses; - std::string command_string; - is >> command_string; + auto is_used_cooperate_status = [](const auto & cooperate_status) { + return std::find_if( + used_cooperate_statuses.begin(), used_cooperate_statuses.end(), + [&cooperate_status](const auto & used_cooperate_status) { + return used_cooperate_status.module == cooperate_status.module && + used_cooperate_status.uuid == cooperate_status.uuid && + used_cooperate_status.command_status.type == + cooperate_status.command_status.type; + }) != used_cooperate_statuses.end(); + }; - if (auto iter = state_dictionary.find(command_string); iter != state_dictionary.end()) { - message.set__command(iter->second); + if (const auto cooperate_status = std::find_if( + latest_cooperate_status_array.statuses.begin(), + latest_cooperate_status_array.statuses.end(), + [module_type = toModuleType(module_name), + command_type = to_command_type(command), + is_used_cooperate_status](const auto & cooperate_status) { + return isValidCooperateStatus( + cooperate_status, command_type, module_type) && + not is_used_cooperate_status(cooperate_status); + }); + cooperate_status == latest_cooperate_status_array.statuses.end()) { + std::stringstream what; + what + << "Failed to send a cooperate command: Cannot find a valid request to cooperate for module " + << std::quoted(module_name) << " and command " << std::quoted(command) << ". " + << "Please check if the situation is such that the request occurs when sending."; + throw common::Error(what.str()); } else { - throw common::Error("Unsupported TurnIndicatorsCommand::command : ", command_string.c_str()); + tier4_rtc_msgs::msg::CooperateCommand cooperate_command; + cooperate_command.module = cooperate_status->module; + cooperate_command.uuid = cooperate_status->uuid; + cooperate_command.command.type = to_command_type(command); + + auto request = std::make_shared(); + request->stamp = latest_cooperate_status_array.stamp; + request->commands.push_back(cooperate_command); + + task_queue.delay([this, request]() { requestCooperateCommands(request, 1); }); + + used_cooperate_statuses.push_back(*cooperate_status); } +} - return is; +auto FieldOperatorApplication::setVelocityLimit(double velocity_limit) -> void +{ + task_queue.delay([this, velocity_limit]() { + auto request = std::make_shared(); + request->velocity = velocity_limit; + /* + We attempt to resend the service up to 30 times, but this number of + times was determined by heuristics, not for any technical reason. + */ + requestSetVelocityLimit(request, 30); + }); +} + +auto FieldOperatorApplication::spinSome() -> void +{ + if (rclcpp::ok() and not is_stop_requested.load()) { + if (process_id != 0) { + auto status = 0; + if (const auto id = waitpid(process_id, &status, WNOHANG); id < 0) { + switch (errno) { + case ECHILD: + is_autoware_exited = true; + throw common::AutowareError("Autoware process is already terminated"); + default: + RCLCPP_ERROR_STREAM( + get_logger(), std::system_error(errno, std::system_category()).what()); + std::exit(EXIT_FAILURE); + } + } else if (0 < id) { + if (WIFEXITED(status)) { + is_autoware_exited = true; + throw common::AutowareError( + "Autoware process is unintentionally exited. exit code: ", WEXITSTATUS(status)); + } else if (WIFSIGNALED(status)) { + is_autoware_exited = true; + throw common::AutowareError("Autoware process is killed. signal is ", WTERMSIG(status)); + } + } + } + rclcpp::spin_some(get_node_base_interface()); + } } -} // namespace autoware_vehicle_msgs::msg +} // 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 deleted file mode 100644 index b3438d35a89..00000000000 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ /dev/null @@ -1,547 +0,0 @@ -// 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. - -#include -#include -#include -#include -#include - -namespace concealer -{ -FieldOperatorApplicationFor::~FieldOperatorApplicationFor() -{ - shutdownAutoware(); - // All tasks should be complete before the services used in them will be deinitialized. - task_queue.stopAndJoin(); -} - -template -struct lister -{ - std::reference_wrapper tuples; - - explicit lister(const Tuples & tuples) : tuples(std::cref(tuples)) {} -}; - -template -auto operator<<(std::ostream & ostream, const lister & lister) -> std::ostream & -{ - for (auto iterator = std::begin(lister.tuples.get()); iterator != std::end(lister.tuples.get()); - ++iterator) { - switch (std::distance(iterator, std::end(lister.tuples.get()))) { - case 1: - return ostream << std::get(*iterator); - - case 2: - ostream << std::get(*iterator) << " and "; - break; - - default: - ostream << std::get(*iterator) << ", "; - break; - } - } - - return ostream; -} - -template -auto listup(const Tuples & tuples) -> lister -{ - return lister(tuples); -} - -#define DEFINE_STATIC_DATA_MEMBER_DETECTOR(NAME) \ - template \ - struct HasStatic##NAME : public std::false_type \ - { \ - }; \ - \ - template \ - struct HasStatic##NAME> : public std::true_type \ - { \ - } - -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(LANE_CHANGE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(GOAL_PLANNER); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(START_PLANNER); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OUT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(TRAFFIC_LIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(INTERSECTION_OCCLUSION); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(CROSSWALK); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(BLIND_SPOT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(DETECTION_AREA); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_STOPPING_AREA); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(OCCLUSION_SPOT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EXT_REQUEST_LANE_CHANGE_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_DRIVABLE_LANE); - -// For MrmState::behavior -DEFINE_STATIC_DATA_MEMBER_DETECTOR(COMFORTABLE_STOP); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(EMERGENCY_STOP); -// DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); // NOTE: This is defined above. -DEFINE_STATIC_DATA_MEMBER_DETECTOR(UNKNOWN); -DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OVER); - -#undef DEFINE_STATIC_DATA_MEMBER_DETECTOR - -/** - * NOTE: for changes from `distance` to start/finish distance - * see https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef - */ -template -struct HasDistance : public std::false_type -{ -}; - -template -struct HasDistance().distance)>> : public std::true_type -{ -}; - -template -auto toModuleType(const std::string & module_name) -{ - static const std::unordered_map module_type_map = [&]() { - std::unordered_map module_type_map; - -#define EMPLACE(IDENTIFIER) \ - if constexpr (HasStatic##IDENTIFIER::value) { \ - module_type_map.emplace(#IDENTIFIER, T::IDENTIFIER); \ - } \ - static_assert(true) - - /* - The following elements are in order of definition in the - tier4_rtc_msgs/msg/Module.msg file. Of course, unordered_map doesn't - preserve the insertion order, so the order itself doesn't matter. - */ - EMPLACE(NONE); - EMPLACE(LANE_CHANGE_LEFT); - EMPLACE(LANE_CHANGE_RIGHT); - EMPLACE(AVOIDANCE_LEFT); - EMPLACE(AVOIDANCE_RIGHT); - EMPLACE(GOAL_PLANNER); - EMPLACE(START_PLANNER); - EMPLACE(PULL_OUT); - EMPLACE(TRAFFIC_LIGHT); - EMPLACE(INTERSECTION); - EMPLACE(INTERSECTION_OCCLUSION); - EMPLACE(CROSSWALK); - EMPLACE(BLIND_SPOT); - EMPLACE(DETECTION_AREA); - EMPLACE(NO_STOPPING_AREA); - EMPLACE(OCCLUSION_SPOT); - EMPLACE(EXT_REQUEST_LANE_CHANGE_LEFT); - EMPLACE(EXT_REQUEST_LANE_CHANGE_RIGHT); - EMPLACE(AVOIDANCE_BY_LC_LEFT); - EMPLACE(AVOIDANCE_BY_LC_RIGHT); - EMPLACE(NO_DRIVABLE_LANE); - -#undef EMPLACE - - return module_type_map; - }(); - - if (const auto module_type = module_type_map.find(module_name); - module_type == module_type_map.end()) { - throw common::Error( - "Unexpected module name for tier4_rtc_msgs::msg::Module: ", module_name, - ". One of the following module names is expected: ", listup<0>(module_type_map), "."); - } else { - return module_type->second; - } -} - -template -bool isValidCooperateStatus( - const CooperateStatusType & cooperate_status, std::uint8_t command_type, std::uint8_t module_type) -{ - /** - * NOTE1: the finish_distance filter is set to over -20.0, - * because some valid rtc statuses has negative finish_distance due to the errors of - * localization or numerical calculation. This threshold is advised by a member of TIER IV - * planning and control team. - */ - - /** - * NOTE2: The difference in the variable referred as a distance is the impact of the - * message specification changes in the following URL. - * This was also decided after consulting with a member of TIER IV planning and control team. - * ref: https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef - */ - if constexpr (HasDistance::value) { - return cooperate_status.module.type == module_type && - command_type != cooperate_status.command_status.type && - cooperate_status.distance >= -20.0; - } else { - return cooperate_status.module.type == module_type && - command_type != cooperate_status.command_status.type && - cooperate_status.finish_distance >= -20.0; - } -} - -auto FieldOperatorApplicationFor::sendCooperateCommand( - const std::string & module_name, const std::string & command) -> void -{ - auto to_command_type = [](const auto & command) { - static const std::unordered_map command_type_map = { - {"ACTIVATE", tier4_rtc_msgs::msg::Command::ACTIVATE}, - {"DEACTIVATE", tier4_rtc_msgs::msg::Command::DEACTIVATE}, - }; - if (const auto command_type = command_type_map.find(command); - command_type == command_type_map.end()) { - throw common::Error( - "Unexpected command for tier4_rtc_msgs::msg::Command: ", command, - ", One of the following commands is expected: ", listup<0>(command_type_map), "."); - } else { - return command_type->second; - } - }; - - /** - * NOTE: Used cooperate statuses will be deleted correctly in Autoware side and provided via topic update. - * But, their update rate (typ. 10Hz) is lower than the one of scenario_simulator_v2. - * So, we need to check cooperate statuses if they are used or not in scenario_simulator_v2 side - * to avoid sending the same cooperate command when sending multiple commands between updates of cooperate statuses. - */ - static std::vector used_cooperate_statuses; - auto is_used_cooperate_status = [](const auto & cooperate_status) { - return std::find_if( - used_cooperate_statuses.begin(), used_cooperate_statuses.end(), - [&cooperate_status](const auto & used_cooperate_status) { - return used_cooperate_status.module == cooperate_status.module && - used_cooperate_status.uuid == cooperate_status.uuid && - used_cooperate_status.command_status.type == - cooperate_status.command_status.type; - }) != used_cooperate_statuses.end(); - }; - - if (const auto cooperate_status = std::find_if( - latest_cooperate_status_array.statuses.begin(), - latest_cooperate_status_array.statuses.end(), - [module_type = toModuleType(module_name), - command_type = to_command_type(command), - is_used_cooperate_status](const auto & cooperate_status) { - return isValidCooperateStatus( - cooperate_status, command_type, module_type) && - not is_used_cooperate_status(cooperate_status); - }); - cooperate_status == latest_cooperate_status_array.statuses.end()) { - std::stringstream what; - what - << "Failed to send a cooperate command: Cannot find a valid request to cooperate for module " - << std::quoted(module_name) << " and command " << std::quoted(command) << "." - << "Please check if the situation is such that the request occurs when sending."; - throw common::Error(what.str()); - } else { - tier4_rtc_msgs::msg::CooperateCommand cooperate_command; - cooperate_command.module = cooperate_status->module; - cooperate_command.uuid = cooperate_status->uuid; - cooperate_command.command.type = to_command_type(command); - - auto request = std::make_shared(); - request->stamp = latest_cooperate_status_array.stamp; - request->commands.push_back(cooperate_command); - - task_queue.delay([this, request]() { requestCooperateCommands(request); }); - - used_cooperate_statuses.push_back(*cooperate_status); - } -} - -auto FieldOperatorApplicationFor::initialize( - const geometry_msgs::msg::Pose & initial_pose) -> void -{ - if (not std::exchange(initialize_was_called, true)) { - task_queue.delay([this, initial_pose]() { - waitForAutowareStateToBeWaitingForRoute([&]() { - -#if __has_include() - if ( - getLocalizationState().state != - autoware_adapi_v1_msgs::msg::LocalizationInitializationState::UNINITIALIZED) { - return; - } -#endif - geometry_msgs::msg::PoseWithCovarianceStamped initial_pose_msg; - initial_pose_msg.header.stamp = get_clock()->now(); - initial_pose_msg.header.frame_id = "map"; - initial_pose_msg.pose.pose = initial_pose; - - auto request = - std::make_shared(); - request->pose.push_back(initial_pose_msg); - try { - return requestInitialPose(request); - } catch (const decltype(requestInitialPose)::TimeoutError &) { - // ignore timeout error because this service is validated by Autoware state transition. - return; - } - }); - }); - } -} - -auto FieldOperatorApplicationFor::plan( - const std::vector & route) -> void -{ - assert(not route.empty()); - - task_queue.delay([this, route] { - waitForAutowareStateToBeWaitingForRoute(); // NOTE: This is assertion. - - auto request = std::make_shared(); - - 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 ( - has_data_member_option_v and - has_data_member_allow_goal_modification_v< - decltype(std::declval().option)>) { - request->option.allow_goal_modification = - get_parameter("allow_goal_modification").get_value(); - } - - 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); - - waitForAutowareStateToBeWaitingForEngage(); - }); -} - -auto FieldOperatorApplicationFor::clearRoute() -> void -{ - task_queue.delay([this] { - /* - Since this service tends to be available long after the launch of - Autoware, set the attempts_count to a high value. There is no technical - basis for the number 30. - */ - requestClearRoute(std::make_shared(), 30); - }); -} - -auto FieldOperatorApplicationFor::engage() -> void -{ - task_queue.delay([this]() { - waitForAutowareStateToBeDriving([this]() { - auto request = std::make_shared(); - request->engage = true; - try { - return requestEngage(request); - } catch (const decltype(requestEngage)::TimeoutError &) { - // ignore timeout error because this service is validated by Autoware state transition. - return; - } - }); - }); -} - -auto FieldOperatorApplicationFor::engageable() const -> bool -{ - rethrow(); - return task_queue.exhausted() and isWaitingForEngage(); -} - -auto FieldOperatorApplicationFor::engaged() const -> bool -{ - rethrow(); - return task_queue.exhausted() and isDriving(); -} - -auto FieldOperatorApplicationFor::getWaypoints() const - -> traffic_simulator_msgs::msg::WaypointsArray -{ - traffic_simulator_msgs::msg::WaypointsArray waypoints; - - for (const auto & point : getTrajectory().points) { - waypoints.waypoints.emplace_back(point.pose.position); - } - - return waypoints; -} - -auto FieldOperatorApplicationFor::getTurnIndicatorsCommand() const - -> autoware_vehicle_msgs::msg::TurnIndicatorsCommand -{ - return getTurnIndicatorsCommandImpl(); -} - -auto FieldOperatorApplicationFor::restrictTargetSpeed(double value) const - -> double -{ - // no restrictions here - return value; -} - -auto FieldOperatorApplicationFor::getAutowareStateName() const -> std::string -{ - return autoware_state; -} - -auto FieldOperatorApplicationFor::getEmergencyStateName() const -> std::string -{ - return minimum_risk_maneuver_state; -} - -auto FieldOperatorApplicationFor::getMinimumRiskManeuverBehaviorName() const - -> std::string -{ - return minimum_risk_maneuver_behavior; -} - -auto FieldOperatorApplicationFor::getMinimumRiskManeuverStateName() const - -> std::string -{ - return minimum_risk_maneuver_state; -} - -auto FieldOperatorApplicationFor::sendSIGINT() -> void // -{ - ::kill(process_id, SIGINT); -} - -auto FieldOperatorApplicationFor::setVelocityLimit(double velocity_limit) -> void -{ - task_queue.delay([this, velocity_limit]() { - auto request = std::make_shared(); - request->velocity = velocity_limit; - // We attempt to resend the service up to 30 times, but this number of times was determined by - // heuristics, not for any technical reason - requestSetVelocityLimit(request, 30); - }); -} - -auto FieldOperatorApplicationFor::requestAutoModeForCooperation( - const std::string & module_name, bool enable) -> void -{ - // Note: The implementation of this function will not work properly - // if the `rtc_auto_mode_manager` package is present. - if (not isPackageExists("rtc_auto_mode_manager")) { - task_queue.delay([this, module_name, enable]() { - auto request = std::make_shared(); - request->module.type = toModuleType(module_name); - request->enable = enable; - // We attempt to resend the service up to 30 times, but this number of times was determined by - // heuristics, not for any technical reason - requestSetRtcAutoMode(request, 30); - }); - } else { - throw common::Error( - "FieldOperatorApplicationFor::requestAutoModeForCooperation is not " - "supported in this environment, because rtc_auto_mode_manager is present."); - } -} - -auto FieldOperatorApplicationFor::enableAutowareControl() -> void -{ - task_queue.delay([this]() { - auto request = std::make_shared(); - requestEnableAutowareControl(request, 30); - }); -} - -auto FieldOperatorApplicationFor::receiveEmergencyState( - const tier4_external_api_msgs::msg::Emergency & message) -> void -{ - if (message.emergency) { - throw common::Error("Emergency state received"); - } -} - -template -auto toMinimumRiskManeuverBehaviorString(const std::uint8_t & behavior_number) -> std::string -{ - static const std::unordered_map behavior_string_map = [&]() { - std::unordered_map behavior_string_map; - -#define EMPLACE(IDENTIFIER) \ - if constexpr (HasStatic##IDENTIFIER::value) { \ - behavior_string_map.emplace(T::IDENTIFIER, #IDENTIFIER); \ - } \ - static_assert(true) - - EMPLACE(COMFORTABLE_STOP); - EMPLACE(EMERGENCY_STOP); - EMPLACE(NONE); - EMPLACE(UNKNOWN); - EMPLACE(PULL_OVER); - -#undef EMPLACE - return behavior_string_map; - }(); - - if (const auto behavior = behavior_string_map.find(behavior_number); - behavior == behavior_string_map.end()) { - throw common::Error( - "Unexpected autoware_adapi_v1_msgs::msg::MrmState::behavior, number: ", behavior_number); - } else { - return behavior->second; - } -} - -auto toMinimumRiskManeuverStateString(const std::uint8_t & state_number) -> std::string -{ - static const std::unordered_map state_string_map = { - {autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED, "MRM_FAILED"}, - {autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING, "MRM_OPERATING"}, - {autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED, "MRM_SUCCEEDED"}, - {autoware_adapi_v1_msgs::msg::MrmState::NORMAL, "NORMAL"}, - {autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN, "UNKNOWN"}, - }; - - if (const auto state = state_string_map.find(state_number); state == state_string_map.end()) { - throw common::Error( - "Unexpected autoware_adapi_v1_msgs::msg::MrmState::state, number: ", state_number); - } else { - return state->second; - } -} - -auto FieldOperatorApplicationFor::receiveMrmState( - const autoware_adapi_v1_msgs::msg::MrmState & message) -> void -{ - minimum_risk_maneuver_state = toMinimumRiskManeuverStateString(message.state); - - minimum_risk_maneuver_behavior = - toMinimumRiskManeuverBehaviorString(message.behavior); -} -} // namespace concealer diff --git a/external/concealer/src/task_queue.cpp b/external/concealer/src/task_queue.cpp index 32c7b003cc8..cc2298324d1 100644 --- a/external/concealer/src/task_queue.cpp +++ b/external/concealer/src/task_queue.cpp @@ -42,7 +42,7 @@ TaskQueue::TaskQueue() { } -void TaskQueue::stopAndJoin() +TaskQueue::~TaskQueue() { if (dispatcher.joinable()) { is_stop_requested.store(true, std::memory_order_release); @@ -50,8 +50,6 @@ void TaskQueue::stopAndJoin() } } -TaskQueue::~TaskQueue() { stopAndJoin(); } - bool TaskQueue::exhausted() const noexcept { return is_exhausted.load(); } void TaskQueue::rethrow() const diff --git a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp index f7c0bb10c68..b9cfae284bc 100644 --- a/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/user_defined_value_condition.cpp @@ -147,8 +147,18 @@ UserDefinedValueCondition::UserDefinedValueCondition(const pugi::xml_node & node std::make_pair( "currentTurnIndicatorsState", [result]() { - return make(boost::lexical_cast( - asFieldOperatorApplication(result.str(1)).getTurnIndicatorsCommand())); + switch (asFieldOperatorApplication(result.str(1)).getTurnIndicatorsCommand().command) { + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::DISABLE: + return make("DISABLE"); + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::ENABLE_LEFT: + return make("ENABLE_LEFT"); + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::ENABLE_RIGHT: + return make("ENABLE_RIGHT"); + case autoware_vehicle_msgs::msg::TurnIndicatorsCommand::NO_COMMAND: + return make("NO_COMMAND"); + default: + return make(); + } }), }; evaluate_value = dispatch.at(result.str(2)); // XXX catch diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index 9ba9aae8869..8e09148b912 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -15,7 +15,7 @@ #ifndef TRAFFIC_SIMULATOR__VEHICLE_SIMULATION__EGO_ENTITY_SIMULATION_HPP_ #define TRAFFIC_SIMULATOR__VEHICLE_SIMULATION__EGO_ENTITY_SIMULATION_HPP_ -#include +#include #include #include #include @@ -40,7 +40,7 @@ enum class VehicleModelType { class EgoEntitySimulation { public: - const std::unique_ptr autoware; + const std::unique_ptr autoware; private: const VehicleModelType vehicle_model_type_; diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index 99fb0380396..b8c0fe8362f 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -177,15 +177,15 @@ auto EgoEntitySimulation::makeSimulationModel( auto EgoEntitySimulation::setAutowareStatus() -> void { - autoware->set([this]() { + autoware->current_acceleration.store([this]() { geometry_msgs::msg::Accel message; message.linear.x = vehicle_model_ptr_->getAx(); return message; }()); - autoware->set(status_.getMapPose()); + autoware->current_pose.store(status_.getMapPose()); - autoware->set(getCurrentTwist()); + autoware->current_twist.store(getCurrentTwist()); } void EgoEntitySimulation::requestSpeedChange(double value) @@ -308,20 +308,42 @@ void EgoEntitySimulation::update( auto acceleration_by_slope = calculateAccelerationBySlope(); + const auto [speed, acceleration, tire_angle, gear_sign, gear_command] = + autoware->getVehicleCommand(); + switch (vehicle_model_type_) { case VehicleModelType::DELAY_STEER_ACC: case VehicleModelType::DELAY_STEER_ACC_GEARED: case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED: case VehicleModelType::IDEAL_STEER_ACC: case VehicleModelType::IDEAL_STEER_ACC_GEARED: - input(0) = autoware->getGearSign() * (autoware->getAcceleration() + acceleration_by_slope); - input(1) = autoware->getSteeringAngle(); + /* + TODO FIX THIS!!! + + THIS IS MAYBE INCORRECT. + + SHOULD BE + gear_sign * acceleration + acceleration_by_slope + OR + signed_acceleration + acceleration_by_slope + + Currently, acceleration is obtained as an unsigned value + (`acceleration`) and a signed value (`gear_sign`), but this is for + historical reasons and there is no longer any reason to do so. + + Therefore, when resolving this TODO comment, the assignee should + remove `gear_sign` from the tuple returned by + `AutowareUniverse::getVehicleCommand`, and at the same time change + `acceleration` to a signed value. + */ + input(0) = gear_sign * (acceleration + acceleration_by_slope); + input(1) = tire_angle; break; case VehicleModelType::DELAY_STEER_VEL: case VehicleModelType::IDEAL_STEER_VEL: - input(0) = autoware->getVelocity(); - input(1) = autoware->getSteeringAngle(); + input(0) = speed; + input(1) = tire_angle; break; default: @@ -329,7 +351,7 @@ void EgoEntitySimulation::update( "Unsupported vehicle_model_type ", toString(vehicle_model_type_), "specified"); } - vehicle_model_ptr_->setGear(autoware->getGearCommand().command); + vehicle_model_ptr_->setGear(gear_command); vehicle_model_ptr_->setInput(input); vehicle_model_ptr_->update(step_time); } 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 69fc2a2de1a..91cab0f7f97 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/ego_entity.hpp @@ -17,7 +17,6 @@ #include #include -#include #include #include #include diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index e5cce78c26c..e4f9560c180 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -13,8 +13,7 @@ // limitations under the License. #include -#include -#include +#include #include #include #include @@ -58,12 +57,10 @@ auto EgoEntity::makeFieldOperatorApplication( // clang-format on return getParameter(node_parameters, "launch_autoware", true) - ? std::make_unique< - concealer::FieldOperatorApplicationFor>( + ? std::make_unique( getParameter(node_parameters, "autoware_launch_package"), getParameter(node_parameters, "autoware_launch_file"), parameters) - : std::make_unique< - concealer::FieldOperatorApplicationFor>(); + : std::make_unique(); } else { throw common::SemanticError( "Unexpected architecture_type ", std::quoted(architecture_type), " was given."); @@ -117,8 +114,7 @@ auto EgoEntity::getRouteLanelets(double /*unused horizon*/) -> lanelet::Ids lanelet::Ids ids{}; if (const auto universe = - dynamic_cast *>( - field_operator_application.get()); + dynamic_cast(field_operator_application.get()); universe) { for (const auto & point : universe->getPathWithLaneId().points) { ids += point.lane_ids; @@ -278,7 +274,6 @@ auto EgoEntity::requestSpeedChange(double value, bool /* continuous */) -> void THROW_SEMANTIC_ERROR("You cannot set target speed to the ego vehicle after starting scenario."); } else { target_speed_ = value; - field_operator_application->restrictTargetSpeed(value); } }