From c7c078a3e8b89f37876b91e373e5d931bdc3870f Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 10 Nov 2022 21:04:57 +0900 Subject: [PATCH] feat(operation_mode_transition_manager): support ad api (#1535) * feat(operation_mode_transition_manager): support ad api Signed-off-by: Takagi, Isamu * fix: merge operation mode state message Signed-off-by: Takagi, Isamu * feat(autoware_ad_api_msgs): define operation mode interface Signed-off-by: Takagi, Isamu * fix: add message Signed-off-by: Takagi, Isamu * Update common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * Update common/autoware_ad_api_msgs/operation_mode/msg/OperationModeState.msg Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * feat: apply field name change Signed-off-by: Takagi, Isamu * feat: move adapi message Signed-off-by: Takagi, Isamu * feat: change message type Signed-off-by: Takagi, Isamu * fix: fix build error Signed-off-by: Takagi, Isamu * fix: fix error message Signed-off-by: Takagi, Isamu * WIP Signed-off-by: Takagi, Isamu * feat: add compatibility Signed-off-by: Takagi, Isamu * fix: fix operation mode change when disable autoware control Signed-off-by: Takagi, Isamu * fix: fix operation mode change when autoware control is disabled Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .../rclcpp/exceptions.hpp | 39 +- .../CMakeLists.txt | 5 +- ...ration_mode_transition_manager.param.yaml} | 1 + .../data.hpp | 98 ----- .../operation_mode_transition_manager.hpp | 85 ----- .../state.hpp | 121 ------- ...eration_mode_transition_manager.launch.xml | 3 +- .../OperationModeTransitionManagerDebug.msg | 2 +- .../package.xml | 2 + .../src/compatibility.cpp | 140 ++++++++ .../src/compatibility.hpp | 59 +++ .../src/data.cpp | 103 ++---- .../src/data.hpp | 86 +++++ .../src/node.cpp | 268 ++++++++++++++ .../src/node.hpp | 80 +++++ .../src/operation_mode_transition_manager.cpp | 340 ------------------ .../src/state.cpp | 313 ++++++++-------- .../src/state.hpp | 104 ++++++ control/vehicle_cmd_gate/README.md | 2 +- control/vehicle_cmd_gate/package.xml | 1 + .../vehicle_cmd_gate/src/vehicle_cmd_gate.cpp | 16 +- .../vehicle_cmd_gate/src/vehicle_cmd_gate.hpp | 10 +- ...eration_mode_transition_manager.param.yaml | 1 + .../launch/control.launch.py | 4 +- 24 files changed, 1002 insertions(+), 881 deletions(-) rename control/operation_mode_transition_manager/config/{engage_transition_manager.param.yaml => operation_mode_transition_manager.param.yaml} (95%) delete mode 100644 control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp delete mode 100644 control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp delete mode 100644 control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp create mode 100644 control/operation_mode_transition_manager/src/compatibility.cpp create mode 100644 control/operation_mode_transition_manager/src/compatibility.hpp create mode 100644 control/operation_mode_transition_manager/src/data.hpp create mode 100644 control/operation_mode_transition_manager/src/node.cpp create mode 100644 control/operation_mode_transition_manager/src/node.hpp delete mode 100644 control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp create mode 100644 control/operation_mode_transition_manager/src/state.hpp diff --git a/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp b/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp index 87cb925c05ea4..a10b7d75602e6 100644 --- a/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp +++ b/common/component_interface_utils/include/component_interface_utils/rclcpp/exceptions.hpp @@ -17,22 +17,23 @@ #include -#include +#include #include namespace component_interface_utils { -class ServiceException : public std::runtime_error +class ServiceException : public std::exception { public: using ResponseStatus = autoware_adapi_v1_msgs::msg::ResponseStatus; using ResponseStatusCode = ResponseStatus::_code_type; - ServiceException(ResponseStatusCode code, const std::string & message) - : std::runtime_error(message) + ServiceException(ResponseStatusCode code, const std::string & message, bool success = false) { + success_ = success; code_ = code; + message_ = message; } template @@ -40,7 +41,7 @@ class ServiceException : public std::runtime_error { status.success = false; status.code = code_; - status.message = what(); + status.message = message_; } ResponseStatus status() const @@ -48,19 +49,21 @@ class ServiceException : public std::runtime_error ResponseStatus status; status.success = false; status.code = code_; - status.message = what(); + status.message = message_; return status; } private: + bool success_; ResponseStatusCode code_; + std::string message_; }; class ServiceUnready : public ServiceException { public: explicit ServiceUnready(const std::string & message) - : ServiceException(ResponseStatus::SERVICE_UNREADY, message) + : ServiceException(ResponseStatus::SERVICE_UNREADY, message, false) { } }; @@ -69,7 +72,7 @@ class ServiceTimeout : public ServiceException { public: explicit ServiceTimeout(const std::string & message) - : ServiceException(ResponseStatus::SERVICE_TIMEOUT, message) + : ServiceException(ResponseStatus::SERVICE_TIMEOUT, message, false) { } }; @@ -78,7 +81,25 @@ class TransformError : public ServiceException { public: explicit TransformError(const std::string & message) - : ServiceException(ResponseStatus::TRANSFORM_ERROR, message) + : ServiceException(ResponseStatus::TRANSFORM_ERROR, message, false) + { + } +}; + +class ParameterError : public ServiceException +{ +public: + explicit ParameterError(const std::string & message) + : ServiceException(ResponseStatus::PARAMETER_ERROR, message, false) + { + } +}; + +class NoEffectWarning : public ServiceException +{ +public: + explicit NoEffectWarning(const std::string & message) + : ServiceException(ResponseStatus::NO_EFFECT, message, true) { } }; diff --git a/control/operation_mode_transition_manager/CMakeLists.txt b/control/operation_mode_transition_manager/CMakeLists.txt index d281961b2c7e3..9c8ca29fdc277 100644 --- a/control/operation_mode_transition_manager/CMakeLists.txt +++ b/control/operation_mode_transition_manager/CMakeLists.txt @@ -7,9 +7,10 @@ find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() ament_auto_add_library(operation_mode_transition_manager_node SHARED - src/operation_mode_transition_manager.cpp - src/state.cpp + src/compatibility.cpp src/data.cpp + src/node.cpp + src/state.cpp ) rclcpp_components_register_node(operation_mode_transition_manager_node diff --git a/control/operation_mode_transition_manager/config/engage_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml similarity index 95% rename from control/operation_mode_transition_manager/config/engage_transition_manager.param.yaml rename to control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index d42e2392bea0f..be18812b59eba 100644 --- a/control/operation_mode_transition_manager/config/engage_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + transition_timeout: 3.0 frequency_hz: 10.0 engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. diff --git a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp deleted file mode 100644 index af78430618650..0000000000000 --- a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp +++ /dev/null @@ -1,98 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 OPERATION_MODE_TRANSITION_MANAGER__DATA_HPP_ -#define OPERATION_MODE_TRANSITION_MANAGER__DATA_HPP_ - -#include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace operation_mode_transition_manager -{ - -using nav_msgs::msg::Odometry; - -using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_vehicle_msgs::msg::ControlModeReport; -using autoware_auto_vehicle_msgs::srv::ControlModeCommand; -using operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; -using tier4_system_msgs::msg::IsAutonomousAvailable; -using tier4_system_msgs::msg::OperationMode; -using tier4_system_msgs::srv::OperationModeRequest; - -enum class State { - STOP = 0, - MANUAL_DIRECT, - REMOTE_OPERATOR, - LOCAL_OPERATOR, - TRANSITION_TO_AUTO, - AUTONOMOUS, -}; - -struct Data -{ - bool is_auto_available; - State requested_state; - Odometry kinematics; - Trajectory trajectory; - AckermannControlCommand control_cmd; - ControlModeReport current_control_mode; - OperationMode current_gate_operation_mode; - vehicle_info_util::VehicleInfo vehicle_info; -}; - -struct EngageAcceptableParam -{ - bool allow_autonomous_in_stopped = true; - double dist_threshold = 2.0; - double speed_upper_threshold = 10.0; - double speed_lower_threshold = -10.0; - double yaw_threshold = 0.785; - double acc_threshold = 2.0; - double lateral_acc_threshold = 2.0; - double lateral_acc_diff_threshold = 1.0; -}; - -struct StableCheckParam -{ - double duration = 3.0; - double dist_threshold = 0.5; - double speed_upper_threshold = 3.0; - double speed_lower_threshold = -3.0; - double yaw_threshold = M_PI / 10.0; -}; - -uint8_t toMsg(const State s); -State toEnum(const OperationMode s); -std::string toStr(const State s); -bool isManual(const State s); -bool isAuto(const State s); - -} // namespace operation_mode_transition_manager - -#endif // OPERATION_MODE_TRANSITION_MANAGER__DATA_HPP_ diff --git a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp deleted file mode 100644 index 64134a23f7297..0000000000000 --- a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.hpp +++ /dev/null @@ -1,85 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 OPERATION_MODE_TRANSITION_MANAGER__OPERATION_MODE_TRANSITION_MANAGER_HPP_ -#define OPERATION_MODE_TRANSITION_MANAGER__OPERATION_MODE_TRANSITION_MANAGER_HPP_ - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace operation_mode_transition_manager -{ - -class OperationModeTransitionManager : public rclcpp::Node -{ -public: - explicit OperationModeTransitionManager(const rclcpp::NodeOptions & options); - ~OperationModeTransitionManager() = default; - -private: - rclcpp::Publisher::SharedPtr pub_operation_mode_; - rclcpp::Publisher::SharedPtr pub_auto_available_; - rclcpp::Publisher::SharedPtr pub_debug_info_; - rclcpp::Subscription::SharedPtr sub_vehicle_kinematics_; - rclcpp::Subscription::SharedPtr sub_trajectory_; - rclcpp::Subscription::SharedPtr sub_control_cmd_; - rclcpp::Subscription::SharedPtr sub_control_mode_; - rclcpp::Subscription::SharedPtr sub_gate_operation_mode_; - rclcpp::Service::SharedPtr srv_mode_change_server_; - rclcpp::Client::SharedPtr srv_mode_change_client_; - rclcpp::TimerBase::SharedPtr timer_; - - std::unique_ptr operation_mode_transition_manager_; - - std::shared_ptr data_; - - State updateState(const std::shared_ptr data); - State getCurrentState() { return operation_mode_transition_manager_->getCurrentState(); } - - EngageAcceptableParam engage_acceptable_param_; - StableCheckParam stable_check_param_; - - bool hasDangerAcceleration(); - std::pair hasDangerLateralAcceleration(); - bool checkEngageAvailable(); - - void publishData(); - - // update information - void onTimer(); - - void onOperationModeRequest( - const OperationModeRequest::Request::SharedPtr request, - const OperationModeRequest::Response::SharedPtr response); - - mutable OperationModeTransitionManagerDebug debug_info_; -}; - -} // namespace operation_mode_transition_manager - -#endif // OPERATION_MODE_TRANSITION_MANAGER__OPERATION_MODE_TRANSITION_MANAGER_HPP_ diff --git a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp deleted file mode 100644 index 414d281dd5388..0000000000000 --- a/control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp +++ /dev/null @@ -1,121 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 OPERATION_MODE_TRANSITION_MANAGER__STATE_HPP_ -#define OPERATION_MODE_TRANSITION_MANAGER__STATE_HPP_ - -#include -#include - -#include -#include -#include -#include -#include - -#include - -namespace operation_mode_transition_manager -{ -class EngageStateBase -{ -public: - EngageStateBase(const State state, rclcpp::Node * node); - ~EngageStateBase() = default; - - virtual State update() = 0; - - State getCurrentState() { return state_; } - void setData(const std::shared_ptr data) { data_ = data; } - void setParam(const StableCheckParam & param) { stable_check_param_ = param; } - -protected: - rclcpp::Client::SharedPtr srv_mode_change_client_; - - rclcpp::Logger logger_; - rclcpp::Clock::SharedPtr clock_; - - State state_; - std::shared_ptr data_; - StableCheckParam stable_check_param_; - - State defaultUpdateOnManual(); - bool sendAutonomousModeRequest(); - bool sendManualModeRequest(); -}; - -class StopState : public EngageStateBase -{ -public: - explicit StopState(rclcpp::Node * node) : EngageStateBase(State::STOP, node) {} - State update() override { return defaultUpdateOnManual(); } -}; - -class RemoteOperatorState : public EngageStateBase -{ -public: - explicit RemoteOperatorState(rclcpp::Node * node) : EngageStateBase(State::REMOTE_OPERATOR, node) - { - } - State update() override { return defaultUpdateOnManual(); } -}; - -class ManualDirectState : public EngageStateBase -{ -public: - explicit ManualDirectState(rclcpp::Node * node) : EngageStateBase(State::MANUAL_DIRECT, node) {} - State update() override { return defaultUpdateOnManual(); } -}; - -class LocalOperatorState : public EngageStateBase -{ -public: - explicit LocalOperatorState(rclcpp::Node * node) : EngageStateBase(State::LOCAL_OPERATOR, node) {} - State update() override { return defaultUpdateOnManual(); } -}; - -class TransitionToAutoState : public EngageStateBase -{ -public: - explicit TransitionToAutoState(rclcpp::Node * node) - : EngageStateBase(State::TRANSITION_TO_AUTO, node) - { - transition_requested_time_ = clock_->now(); - }; - State update() override; - -private: - std::shared_ptr stable_start_time_; - bool checkSystemStable(); - - // return true when MANUAL mode is detected after AUTO transition is done. - bool checkVehicleOverride(); - - bool checkTransitionTimeout() const; - - bool is_vehicle_mode_change_done_ = false; // set to true when the mode changed to Auto. - bool is_control_mode_request_send_ = false; - rclcpp::Time transition_requested_time_; -}; - -class AutonomousState : public EngageStateBase -{ -public: - explicit AutonomousState(rclcpp::Node * node) : EngageStateBase(State::AUTONOMOUS, node) {} - State update() override; -}; - -} // namespace operation_mode_transition_manager - -#endif // OPERATION_MODE_TRANSITION_MANAGER__STATE_HPP_ diff --git a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml index efdfc1c6041e0..569d743ba2c4f 100644 --- a/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml +++ b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml @@ -15,11 +15,10 @@ - - + diff --git a/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg b/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg index 1dc13208b666a..57e26b075506c 100644 --- a/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg +++ b/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg @@ -1,7 +1,7 @@ builtin_interfaces/Time stamp # states -string requested_state +string previous_state string current_state # flags for engage permission diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml index 9db703b6ea7db..417f2e9c383d3 100644 --- a/control/operation_mode_transition_manager/package.xml +++ b/control/operation_mode_transition_manager/package.xml @@ -13,6 +13,8 @@ autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs + component_interface_specs + component_interface_utils geometry_msgs motion_utils rclcpp diff --git a/control/operation_mode_transition_manager/src/compatibility.cpp b/control/operation_mode_transition_manager/src/compatibility.cpp new file mode 100644 index 0000000000000..6a56f23612a6e --- /dev/null +++ b/control/operation_mode_transition_manager/src/compatibility.cpp @@ -0,0 +1,140 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "compatibility.hpp" + +#include + +namespace operation_mode_transition_manager +{ + +Compatibility::Compatibility(rclcpp::Node * node) : node_(node) +{ + sub_autoware_engage_ = node->create_subscription( + "/api/autoware/get/engage", 1, + std::bind(&Compatibility::on_autoware_engage, this, std::placeholders::_1)); + sub_gate_mode_ = node->create_subscription( + "/control/current_gate_mode", 1, + std::bind(&Compatibility::on_gate_mode, this, std::placeholders::_1)); + sub_selector_mode_ = node->create_subscription( + "/control/external_cmd_selector/current_selector_mode", 1, + std::bind(&Compatibility::on_selector_mode, this, std::placeholders::_1)); + + pub_autoware_engage_ = node->create_publisher("/autoware/engage", 1); + pub_gate_mode_ = node->create_publisher("/control/gate_mode_cmd", 1); + cli_selector_mode_ = + node->create_client("/control/external_cmd_selector/select_external_command"); +} + +void Compatibility::on_autoware_engage(const AutowareEngage::ConstSharedPtr msg) +{ + autoware_engage_ = msg; +} + +void Compatibility::on_gate_mode(const GateMode::ConstSharedPtr msg) { gate_mode_ = msg; } + +void Compatibility::on_selector_mode(const SelectorModeMsg::ConstSharedPtr msg) +{ + selector_mode_ = msg; +} + +std::optional Compatibility::get_mode() const +{ + if (!(autoware_engage_ && gate_mode_ && selector_mode_)) { + return std::nullopt; + } + + if (autoware_engage_->engage == false) { + return OperationMode::STOP; + } + if (gate_mode_->data == GateMode::AUTO) { + return OperationMode::AUTONOMOUS; + } + if (selector_mode_->data == SelectorModeMsg::REMOTE) { + return OperationMode::REMOTE; + } + if (selector_mode_->data == SelectorModeMsg::LOCAL) { + return OperationMode::LOCAL; + } + return std::nullopt; +} + +void Compatibility::set_mode(const OperationMode mode) +{ + // Set operation mode in order from upstream node + if (!(autoware_engage_ && gate_mode_ && selector_mode_)) { + return; + } + + // Convert mode for each node. + AutowareEngage::_engage_type autoware_engage; + GateMode::_data_type gate_mode; + SelectorModeMsg::_data_type selector_mode; + switch (mode) { + case OperationMode::STOP: + autoware_engage = false; + gate_mode = GateMode::AUTO; + selector_mode = SelectorModeMsg::NONE; + break; + case OperationMode::AUTONOMOUS: + autoware_engage = true; + gate_mode = GateMode::AUTO; + selector_mode = SelectorModeMsg::NONE; + break; + case OperationMode::LOCAL: + autoware_engage = true; + gate_mode = GateMode::EXTERNAL; + selector_mode = SelectorModeMsg::LOCAL; + break; + case OperationMode::REMOTE: + autoware_engage = true; + gate_mode = GateMode::EXTERNAL; + selector_mode = SelectorModeMsg::REMOTE; + break; + default: + RCLCPP_ERROR_STREAM(node_->get_logger(), "unknown mode"); + return; + } + + // Set selector mode. + if (selector_mode != SelectorModeMsg::NONE && selector_mode_->data != selector_mode) { + if (!is_calling_service_) { + auto req = std::make_shared(); + req->mode.data = selector_mode; + is_calling_service_ = true; + cli_selector_mode_->async_send_request( + req, + [this](rclcpp::Client::SharedFuture) { is_calling_service_ = false; }); + } + } + + // Set gate mode. + if (gate_mode_->data != gate_mode) { + GateMode msg; + msg.data = gate_mode; + pub_gate_mode_->publish(msg); + return; + } + + // Set autoware engage. + if (autoware_engage_->engage != autoware_engage) { + AutowareEngage msg; + msg.stamp = node_->now(); + msg.engage = autoware_engage; + pub_autoware_engage_->publish(msg); + return; + } +} + +} // namespace operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/compatibility.hpp b/control/operation_mode_transition_manager/src/compatibility.hpp new file mode 100644 index 0000000000000..a69b15e69960c --- /dev/null +++ b/control/operation_mode_transition_manager/src/compatibility.hpp @@ -0,0 +1,59 @@ +// Copyright 2022 Autoware Foundation +// +// 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 COMPATIBILITY_HPP_ +#define COMPATIBILITY_HPP_ + +#include "data.hpp" + +#include +#include +#include +#include + +namespace operation_mode_transition_manager +{ + +class Compatibility +{ +public: + explicit Compatibility(rclcpp::Node * node); + void set_mode(const OperationMode mode); + std::optional get_mode() const; + +private: + using AutowareEngage = autoware_auto_vehicle_msgs::msg::Engage; + using GateMode = tier4_control_msgs::msg::GateMode; + using SelectorModeMsg = tier4_control_msgs::msg::ExternalCommandSelectorMode; + using SelectorModeSrv = tier4_control_msgs::srv::ExternalCommandSelect; + rclcpp::Subscription::SharedPtr sub_autoware_engage_; + rclcpp::Subscription::SharedPtr sub_gate_mode_; + rclcpp::Subscription::SharedPtr sub_selector_mode_; + rclcpp::Publisher::SharedPtr pub_autoware_engage_; + rclcpp::Publisher::SharedPtr pub_gate_mode_; + rclcpp::Client::SharedPtr cli_selector_mode_; + void on_autoware_engage(const AutowareEngage::ConstSharedPtr msg); + void on_gate_mode(const GateMode::ConstSharedPtr msg); + void on_selector_mode(const SelectorModeMsg::ConstSharedPtr msg); + + bool is_calling_service_ = false; + rclcpp::Node * node_; + AutowareEngage::ConstSharedPtr autoware_engage_; + GateMode::ConstSharedPtr gate_mode_; + SelectorModeMsg::ConstSharedPtr selector_mode_; +}; + +} // namespace operation_mode_transition_manager + +#endif // COMPATIBILITY_HPP_ diff --git a/control/operation_mode_transition_manager/src/data.cpp b/control/operation_mode_transition_manager/src/data.cpp index 744c0c4983102..8734b6d1c8e86 100644 --- a/control/operation_mode_transition_manager/src/data.cpp +++ b/control/operation_mode_transition_manager/src/data.cpp @@ -12,88 +12,59 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "operation_mode_transition_manager/state.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - -#include -#include +#include "data.hpp" namespace operation_mode_transition_manager { -uint8_t toMsg(const State s) +std::string toString(const std::optional mode) { - if (s == State::STOP) { - return OperationMode::STOP; - } else if (s == State::REMOTE_OPERATOR) { - return OperationMode::REMOTE_OPERATOR; - } else if (s == State::MANUAL_DIRECT) { - return OperationMode::MANUAL_DIRECT; - } else if (s == State::LOCAL_OPERATOR) { - return OperationMode::LOCAL_OPERATOR; - } else if (s == State::TRANSITION_TO_AUTO) { - return OperationMode::TRANSITION_TO_AUTO; - } else if (s == State::AUTONOMOUS) { - return OperationMode::AUTONOMOUS; + if (!mode) { + return "DISENGAGE"; } - return OperationMode::STOP; -} - -State toEnum(const OperationMode m) -{ - if (m.mode == OperationMode::STOP) { - return State::STOP; - } else if (m.mode == OperationMode::REMOTE_OPERATOR) { - return State::REMOTE_OPERATOR; - } else if (m.mode == OperationMode::MANUAL_DIRECT) { - return State::MANUAL_DIRECT; - } else if (m.mode == OperationMode::LOCAL_OPERATOR) { - return State::LOCAL_OPERATOR; - } else if (m.mode == OperationMode::TRANSITION_TO_AUTO) { - return State::TRANSITION_TO_AUTO; - } else if (m.mode == OperationMode::AUTONOMOUS) { - return State::AUTONOMOUS; + switch (mode.value()) { + case OperationMode::STOP: + return "STOP"; + case OperationMode::AUTONOMOUS: + return "AUTONOMOUS"; + case OperationMode::LOCAL: + return "LOCAL"; + case OperationMode::REMOTE: + return "REMOTE"; } - return State::STOP; + return "UNKNOWN"; } -bool isManual(const State s) +std::optional toEnum(const ChangeOperationMode::Request & request) { - if ( - s == State::STOP || s == State::REMOTE_OPERATOR || s == State::MANUAL_DIRECT || - s == State::LOCAL_OPERATOR) { - return true; - } else { - return false; - } -} + using ServiceMode = ChangeOperationMode::Request; -bool isAuto(const State s) -{ - if (s == State::AUTONOMOUS) { - return true; - } else { - return false; + switch (request.mode) { + case ServiceMode::STOP: + return OperationMode::STOP; + case ServiceMode::AUTONOMOUS: + return OperationMode::AUTONOMOUS; + case ServiceMode::LOCAL: + return OperationMode::LOCAL; + case ServiceMode::REMOTE: + return OperationMode::REMOTE; } + return std::nullopt; } -std::string toStr(const State s) +OperationModeValue toMsg(const OperationMode mode) { - if (s == State::STOP) { - return "STOP"; - } else if (s == State::REMOTE_OPERATOR) { - return "REMOTE_OPERATOR"; - } else if (s == State::MANUAL_DIRECT) { - return "MANUAL_DIRECT"; - } else if (s == State::LOCAL_OPERATOR) { - return "LOCAL_OPERATOR"; - } else if (s == State::TRANSITION_TO_AUTO) { - return "TRANSITION_TO_AUTO"; - } else if (s == State::AUTONOMOUS) { - return "AUTONOMOUS"; - } else { - return "INVALID"; + switch (mode) { + case OperationMode::STOP: + return OperationModeState::STOP; + case OperationMode::AUTONOMOUS: + return OperationModeState::AUTONOMOUS; + case OperationMode::LOCAL: + return OperationModeState::LOCAL; + case OperationMode::REMOTE: + return OperationModeState::REMOTE; } + return OperationModeState::UNKNOWN; } } // namespace operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/data.hpp b/control/operation_mode_transition_manager/src/data.hpp new file mode 100644 index 0000000000000..9b822936a0252 --- /dev/null +++ b/control/operation_mode_transition_manager/src/data.hpp @@ -0,0 +1,86 @@ +// Copyright 2022 Autoware Foundation +// +// 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 DATA_HPP_ +#define DATA_HPP_ + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace operation_mode_transition_manager +{ + +using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response; +using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; +using OperationModeValue = OperationModeState::_mode_type; +using ChangeOperationMode = tier4_system_msgs::srv::ChangeOperationMode; +using ControlModeCommand = autoware_auto_vehicle_msgs::srv::ControlModeCommand; +using ControlModeReport = autoware_auto_vehicle_msgs::msg::ControlModeReport; + +enum class OperationMode { STOP, AUTONOMOUS, LOCAL, REMOTE }; + +struct Transition +{ + Transition(const rclcpp::Time & time, bool control, std::optional previous) + : time(time), previous(previous) + { + is_engage_requested = control; + is_engage_failed = false; + is_engage_completed = false; + } + + const rclcpp::Time time; + const std::optional previous; + bool is_engage_requested; + bool is_engage_failed; + bool is_engage_completed; +}; + +struct EngageAcceptableParam +{ + bool allow_autonomous_in_stopped = true; + double dist_threshold = 2.0; + double speed_upper_threshold = 10.0; + double speed_lower_threshold = -10.0; + double yaw_threshold = 0.785; + double acc_threshold = 2.0; + double lateral_acc_threshold = 2.0; + double lateral_acc_diff_threshold = 1.0; +}; + +struct StableCheckParam +{ + double duration = 3.0; + double dist_threshold = 0.5; + double speed_upper_threshold = 3.0; + double speed_lower_threshold = -3.0; + double yaw_threshold = M_PI / 10.0; +}; + +std::string toString(const std::optional mode); +std::optional toEnum(const ChangeOperationMode::Request & request); +OperationModeValue toMsg(const OperationMode mode); + +} // namespace operation_mode_transition_manager + +#endif // DATA_HPP_ diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp new file mode 100644 index 0000000000000..64a40b31289de --- /dev/null +++ b/control/operation_mode_transition_manager/src/node.cpp @@ -0,0 +1,268 @@ +// Copyright 2022 Autoware Foundation +// +// 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 "node.hpp" + +#include + +namespace operation_mode_transition_manager +{ + +OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::NodeOptions & options) +: Node("operation_mode_transition_manager", options), compatibility_(this) +{ + sub_control_mode_report_ = create_subscription( + "control_mode_report", 1, + [this](const ControlModeReport::SharedPtr msg) { control_mode_report_ = *msg; }); + + sub_gate_operation_mode_ = create_subscription( + "gate_operation_mode", 1, + [this](const OperationModeState::SharedPtr msg) { gate_operation_mode_ = *msg; }); + + cli_control_mode_ = create_client("control_mode_request"); + pub_debug_info_ = create_publisher("~/debug_info", 1); + + // component interface + { + const auto node = component_interface_utils::NodeAdaptor(this); + node.init_srv( + srv_autoware_control, this, &OperationModeTransitionManager::onChangeAutowareControl); + node.init_srv(srv_operation_mode, this, &OperationModeTransitionManager::onChangeOperationMode); + node.init_pub(pub_operation_mode_); + } + + // timer + { + const auto period_ns = rclcpp::Rate(declare_parameter("frequency_hz")).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&OperationModeTransitionManager::onTimer, this)); + } + + // initialize state + transition_timeout_ = declare_parameter("transition_timeout"); + current_mode_ = OperationMode::STOP; + transition_ = nullptr; + gate_operation_mode_.mode = OperationModeState::UNKNOWN; + gate_operation_mode_.is_in_transition = false; + control_mode_report_.mode = ControlModeReport::NO_COMMAND; + + // modes + modes_[OperationMode::STOP] = std::make_unique(); + modes_[OperationMode::AUTONOMOUS] = std::make_unique(this); + modes_[OperationMode::LOCAL] = std::make_unique(); + modes_[OperationMode::REMOTE] = std::make_unique(); +} + +void OperationModeTransitionManager::onChangeAutowareControl( + const ChangeAutowareControlAPI::Service::Request::SharedPtr request, + const ChangeAutowareControlAPI::Service::Response::SharedPtr response) +{ + if (request->autoware_control) { + // Treat as a transition to the current operation mode. + changeOperationMode(std::nullopt); + } else { + // Allow mode transition to complete without canceling. + compatibility_transition_ = std::nullopt; + transition_.reset(); + changeControlMode(ControlModeCommand::Request::MANUAL); + } + response->status.success = true; +} + +void OperationModeTransitionManager::onChangeOperationMode( + const ChangeOperationModeAPI::Service::Request::SharedPtr request, + const ChangeOperationModeAPI::Service::Response::SharedPtr response) +{ + const auto mode = toEnum(*request); + if (!mode) { + throw component_interface_utils::ParameterError("The operation mode is invalid."); + } + changeOperationMode(mode.value()); + response->status.success = true; +} + +void OperationModeTransitionManager::changeControlMode(ControlModeCommandType mode) +{ + const auto callback = [this](rclcpp::Client::SharedFuture future) { + if (!future.get()->success) { + RCLCPP_WARN(get_logger(), "Autonomous mode change was rejected."); + if (transition_) { + transition_->is_engage_failed = true; + } + } + }; + + const auto request = std::make_shared(); + request->stamp = now(); + request->mode = mode; + cli_control_mode_->async_send_request(request, callback); +} + +void OperationModeTransitionManager::changeOperationMode(std::optional request_mode) +{ + // NOTE: If request_mode is nullopt, indicates to enable autoware control + + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + const bool request_control = request_mode ? false : true; + + if (current_mode_ == request_mode) { + throw component_interface_utils::NoEffectWarning("The mode is the same as the current."); + } + + if (current_control && request_control) { + throw component_interface_utils::NoEffectWarning("Autoware already controls the vehicle."); + } + + // TODO(Takagi, Isamu): overwrite transition between operation modes + if (transition_) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_IN_TRANSITION, "The mode transition is in progress."); + } + + // Enter transition mode if the vehicle is being or will be controlled by Autoware. + if (current_control || request_control) { + if (!available_mode_change_[request_mode.value_or(current_mode_)]) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change condition is not satisfied."); + } + if (request_control) { + transition_ = std::make_unique(now(), request_control, std::nullopt); + } else { + transition_ = std::make_unique(now(), request_control, current_mode_); + } + } + compatibility_transition_ = now(); + current_mode_ = request_mode.value_or(current_mode_); +} + +void OperationModeTransitionManager::cancelTransition() +{ + const auto & previous = transition_->previous; + if (previous) { + current_mode_ = previous.value(); + } else { + changeControlMode(ControlModeCommand::Request::MANUAL); + } + transition_.reset(); + compatibility_transition_ = std::nullopt; +} + +void OperationModeTransitionManager::processTransition() +{ + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + + // Check timeout. + if (transition_timeout_ < (now() - transition_->time).seconds()) { + return cancelTransition(); + } + + // Check engage failure. + if (transition_->is_engage_failed) { + return cancelTransition(); + } + + // Check override. + if (current_control) { + transition_->is_engage_completed = true; + } else { + if (transition_->is_engage_completed) { + return cancelTransition(); + } + } + + // Check reflection of mode change to the compatible interface. + if (current_mode_ != compatibility_.get_mode()) { + return; + } + + // Check completion when engaged, otherwise engage after the gate reflects transition. + if (current_control) { + if (modes_.at(current_mode_)->isModeChangeCompleted()) { + return transition_.reset(); + } + } else { + if (transition_->is_engage_requested && gate_operation_mode_.is_in_transition) { + transition_->is_engage_requested = false; + return changeControlMode(ControlModeCommand::Request::AUTONOMOUS); + } + } +} + +void OperationModeTransitionManager::onTimer() +{ + for (const auto & [type, mode] : modes_) { + mode->update(current_mode_ == type && transition_); + } + + for (const auto & [type, mode] : modes_) { + available_mode_change_[type] = mode->isModeChangeAvailable(); + } + + // Check sync timeout to the compatible interface. + if (compatibility_transition_) { + if (transition_timeout_ < (now() - compatibility_transition_.value()).seconds()) { + compatibility_transition_ = std::nullopt; + } + } + + // Reflects the mode when changed by the compatible interface. + if (compatibility_transition_) { + compatibility_.set_mode(current_mode_); + } else { + current_mode_ = compatibility_.get_mode().value_or(current_mode_); + } + + // Reset sync timeout when it is completed. + if (current_mode_ == compatibility_.get_mode()) { + compatibility_transition_ = std::nullopt; + } + + if (transition_) { + processTransition(); + } + + publishData(); +} + +void OperationModeTransitionManager::publishData() +{ + const bool current_control = control_mode_report_.mode == ControlModeReport::AUTONOMOUS; + const auto time = now(); + + OperationModeStateAPI::Message state; + state.mode = toMsg(current_mode_); + state.is_autoware_control_enabled = current_control; + state.is_in_transition = transition_ ? true : false; + state.is_stop_mode_available = available_mode_change_[OperationMode::STOP]; + state.is_autonomous_mode_available = available_mode_change_[OperationMode::AUTONOMOUS]; + state.is_local_mode_available = available_mode_change_[OperationMode::LOCAL]; + state.is_remote_mode_available = available_mode_change_[OperationMode::REMOTE]; + + if (prev_state_ != state) { + prev_state_ = state; + state.stamp = time; + pub_operation_mode_->publish(state); + } + + ModeChangeBase::DebugInfo debug_info = modes_.at(OperationMode::AUTONOMOUS)->getDebugInfo(); + debug_info.stamp = time; + debug_info.current_state = toString(current_mode_); + debug_info.previous_state = transition_ ? toString(transition_->previous) : "NONE"; + pub_debug_info_->publish(debug_info); +} + +} // namespace operation_mode_transition_manager + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(operation_mode_transition_manager::OperationModeTransitionManager) diff --git a/control/operation_mode_transition_manager/src/node.hpp b/control/operation_mode_transition_manager/src/node.hpp new file mode 100644 index 0000000000000..71073902231a2 --- /dev/null +++ b/control/operation_mode_transition_manager/src/node.hpp @@ -0,0 +1,80 @@ +// Copyright 2022 Autoware Foundation +// +// 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 NODE_HPP_ +#define NODE_HPP_ + +#include "compatibility.hpp" +#include "state.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace operation_mode_transition_manager +{ + +class OperationModeTransitionManager : public rclcpp::Node +{ +public: + explicit OperationModeTransitionManager(const rclcpp::NodeOptions & options); + +private: + using ChangeAutowareControlAPI = system_interface::ChangeAutowareControl; + using ChangeOperationModeAPI = system_interface::ChangeOperationMode; + using OperationModeStateAPI = system_interface::OperationModeState; + component_interface_utils::Service::SharedPtr srv_autoware_control; + component_interface_utils::Service::SharedPtr srv_operation_mode; + component_interface_utils::Publisher::SharedPtr pub_operation_mode_; + void onChangeAutowareControl( + const ChangeAutowareControlAPI::Service::Request::SharedPtr request, + const ChangeAutowareControlAPI::Service::Response::SharedPtr response); + void onChangeOperationMode( + const ChangeOperationModeAPI::Service::Request::SharedPtr request, + const ChangeOperationModeAPI::Service::Response::SharedPtr response); + + using ControlModeCommandType = ControlModeCommand::Request::_mode_type; + rclcpp::Subscription::SharedPtr sub_control_mode_report_; + rclcpp::Subscription::SharedPtr sub_gate_operation_mode_; + rclcpp::Client::SharedPtr cli_control_mode_; + rclcpp::Publisher::SharedPtr pub_debug_info_; + rclcpp::TimerBase::SharedPtr timer_; + void onTimer(); + void publishData(); + void changeControlMode(ControlModeCommandType mode); + void changeOperationMode(std::optional mode); + void cancelTransition(); + void processTransition(); + + double transition_timeout_; + OperationMode current_mode_; + std::unique_ptr transition_; + std::unordered_map> modes_; + std::unordered_map available_mode_change_; + OperationModeState gate_operation_mode_; + ControlModeReport control_mode_report_; + + std::optional prev_state_; + + Compatibility compatibility_; + std::optional compatibility_transition_; +}; + +} // namespace operation_mode_transition_manager + +#endif // NODE_HPP_ diff --git a/control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp b/control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp deleted file mode 100644 index 5a941b8adab44..0000000000000 --- a/control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp +++ /dev/null @@ -1,340 +0,0 @@ -// Copyright 2022 Autoware Foundation -// -// 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 "operation_mode_transition_manager/operation_mode_transition_manager.hpp" - -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - -#include -#include - -namespace operation_mode_transition_manager -{ - -using motion_utils::findNearestIndex; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcYawDeviation; - -OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::NodeOptions & options) -: Node("operation_mode_transition_manager", options) -{ - using std::placeholders::_1; - using std::placeholders::_2; - operation_mode_transition_manager_ = std::make_unique(this); - data_ = std::make_shared(); - data_->requested_state = State::MANUAL_DIRECT; - data_->vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - - pub_operation_mode_ = create_publisher("operation_mode", 1); - pub_auto_available_ = create_publisher("is_autonomous_available", 1); - pub_debug_info_ = create_publisher("~/debug_info", 1); - - sub_vehicle_kinematics_ = create_subscription( - "kinematics", 1, [this](const Odometry::SharedPtr msg) { data_->kinematics = *msg; }); - - sub_trajectory_ = create_subscription( - "trajectory", 1, [this](const Trajectory::SharedPtr msg) { data_->trajectory = *msg; }); - - sub_control_cmd_ = create_subscription( - "control_cmd", 1, - [this](const AckermannControlCommand::SharedPtr msg) { data_->control_cmd = *msg; }); - - sub_control_mode_ = create_subscription( - "control_mode_report", 1, - [this](const ControlModeReport::SharedPtr msg) { data_->current_control_mode = *msg; }); - - sub_gate_operation_mode_ = create_subscription( - "gate_operation_mode", 1, - [this](const OperationMode::SharedPtr msg) { data_->current_gate_operation_mode = *msg; }); - - srv_mode_change_server_ = create_service( - "operation_mode_request", - std::bind(&OperationModeTransitionManager::onOperationModeRequest, this, _1, _2)); - - { - auto & p = engage_acceptable_param_; - p.allow_autonomous_in_stopped = - declare_parameter("engage_acceptable_limits.allow_autonomous_in_stopped"); - p.dist_threshold = declare_parameter("engage_acceptable_limits.dist_threshold"); - p.speed_upper_threshold = - declare_parameter("engage_acceptable_limits.speed_upper_threshold"); - p.speed_lower_threshold = - declare_parameter("engage_acceptable_limits.speed_lower_threshold"); - p.yaw_threshold = declare_parameter("engage_acceptable_limits.yaw_threshold"); - p.acc_threshold = declare_parameter("engage_acceptable_limits.acc_threshold"); - p.lateral_acc_threshold = - declare_parameter("engage_acceptable_limits.lateral_acc_threshold"); - p.lateral_acc_diff_threshold = - declare_parameter("engage_acceptable_limits.lateral_acc_diff_threshold"); - } - - { - auto & p = stable_check_param_; - p.duration = declare_parameter("stable_check.duration"); - p.dist_threshold = declare_parameter("stable_check.dist_threshold"); - p.speed_upper_threshold = declare_parameter("stable_check.speed_upper_threshold"); - p.speed_lower_threshold = declare_parameter("stable_check.speed_lower_threshold"); - p.yaw_threshold = declare_parameter("stable_check.yaw_threshold"); - } - - { - const auto hz = declare_parameter("frequency_hz"); - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(1.0 / hz)); - timer_ = rclcpp::create_timer( - this, get_clock(), period_ns, std::bind(&OperationModeTransitionManager::onTimer, this)); - } -} - -void OperationModeTransitionManager::onOperationModeRequest( - const OperationModeRequest::Request::SharedPtr request, - const OperationModeRequest::Response::SharedPtr response) -{ - const auto req_state = toEnum(request->mode); - - // invalid case - if (req_state == State::TRANSITION_TO_AUTO) { - RCLCPP_WARN( - get_logger(), "mode change to TRANSITION_TO_AUTO is not supported. Request ignored."); - response->success = false; - return; - } - - data_->requested_state = toEnum(request->mode); - - const auto state = updateState(data_); - - // write success/failure condition one by one for the state transition result. - if (req_state == state) { - response->success = true; - return; - } - if (isAuto(req_state) && state == State::TRANSITION_TO_AUTO) { - response->success = true; - return; - } - - // not satisfy any success conditions. - response->success = false; - data_->requested_state = operation_mode_transition_manager_->getCurrentState(); - RCLCPP_WARN(get_logger(), "mode change failed. Request was declined."); - return; -} - -void OperationModeTransitionManager::onTimer() -{ - data_->is_auto_available = checkEngageAvailable(); - - updateState(data_); - - publishData(); -} - -void OperationModeTransitionManager::publishData() -{ - const auto time = now(); - - OperationMode mode; - mode.stamp = time; - mode.mode = toMsg(operation_mode_transition_manager_->getCurrentState()); - pub_operation_mode_->publish(mode); - - IsAutonomousAvailable msg; - msg.stamp = time; - msg.is_autonomous_available = data_->is_auto_available; - pub_auto_available_->publish(msg); - - debug_info_.stamp = time; - debug_info_.requested_state = toStr(data_->requested_state); - debug_info_.current_state = toStr(operation_mode_transition_manager_->getCurrentState()); - pub_debug_info_->publish(debug_info_); -} - -bool OperationModeTransitionManager::hasDangerAcceleration() -{ - debug_info_.target_control_acceleration = data_->control_cmd.longitudinal.acceleration; - - const bool is_stopping = std::abs(data_->kinematics.twist.twist.linear.x) < 0.01; - if (is_stopping) { - return false; // any acceleration is ok when stopped - } - - const bool has_large_acc = - std::abs(data_->control_cmd.longitudinal.acceleration) > engage_acceptable_param_.acc_threshold; - return has_large_acc; -} - -std::pair OperationModeTransitionManager::hasDangerLateralAcceleration() -{ - const auto wheelbase = data_->vehicle_info.wheel_base_m; - const auto curr_vx = data_->kinematics.twist.twist.linear.x; - const auto curr_wz = data_->kinematics.twist.twist.angular.z; - - // Calculate angular velocity from kinematics model. - // Use current_vx to focus on the steering behavior. - const auto target_wz = - curr_vx * std::tan(data_->control_cmd.lateral.steering_tire_angle) / wheelbase; - - const auto curr_lat_acc = curr_vx * curr_wz; - const auto target_lat_acc = curr_vx * target_wz; - - const bool has_large_lat_acc = - std::abs(curr_lat_acc) > engage_acceptable_param_.lateral_acc_threshold; - const bool has_large_lat_acc_diff = - std::abs(curr_lat_acc - target_lat_acc) > engage_acceptable_param_.lateral_acc_diff_threshold; - - debug_info_.lateral_acceleration = curr_lat_acc; - debug_info_.lateral_acceleration_deviation = curr_lat_acc - target_lat_acc; - - return {has_large_lat_acc, has_large_lat_acc_diff}; -} - -bool OperationModeTransitionManager::checkEngageAvailable() -{ - constexpr auto dist_max = 100.0; - constexpr auto yaw_max = M_PI_4; - - const auto current_speed = data_->kinematics.twist.twist.linear.x; - const auto target_control_speed = data_->control_cmd.longitudinal.speed; - const auto & param = engage_acceptable_param_; - - if (data_->trajectory.points.size() < 2) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - get_logger(), *get_clock(), 5000, "Engage unavailable: trajectory size must be > 2"); - debug_info_ = OperationModeTransitionManagerDebug{}; // all false - return false; - } - - const auto closest_idx = - findNearestIndex(data_->trajectory.points, data_->kinematics.pose.pose, dist_max, yaw_max); - if (!closest_idx) { - RCLCPP_INFO(get_logger(), "Engage unavailable: closest point not found"); - debug_info_ = OperationModeTransitionManagerDebug{}; // all false - return false; // closest trajectory point not found. - } - const auto closest_point = data_->trajectory.points.at(*closest_idx); - const auto target_planning_speed = closest_point.longitudinal_velocity_mps; - debug_info_.trajectory_available_ok = true; - - // No engagement is lateral control error is large - const auto lateral_deviation = calcDistance2d(closest_point.pose, data_->kinematics.pose.pose); - const bool lateral_deviation_ok = lateral_deviation < param.dist_threshold; - - // No engagement is yaw control error is large - const auto yaw_deviation = calcYawDeviation(closest_point.pose, data_->kinematics.pose.pose); - const bool yaw_deviation_ok = yaw_deviation < param.yaw_threshold; - - // No engagement if speed control error is large - const auto speed_deviation = current_speed - target_planning_speed; - const bool speed_upper_deviation_ok = speed_deviation <= param.speed_upper_threshold; - const bool speed_lower_deviation_ok = speed_deviation >= param.speed_lower_threshold; - - // No engagement if the vehicle is moving but the target speed is zero. - const bool stop_ok = !(std::abs(current_speed) > 0.1 && std::abs(target_control_speed) < 0.01); - - // No engagement if the large acceleration is commanded. - const bool large_acceleration_ok = !hasDangerAcceleration(); - - // No engagement if the lateral acceleration is over threshold - const auto [has_large_lat_acc, has_large_lat_acc_diff] = hasDangerLateralAcceleration(); - const auto large_lateral_acceleration_ok = !has_large_lat_acc; - const auto large_lateral_acceleration_diff_ok = !has_large_lat_acc_diff; - - // No engagement if a stop is expected within a certain period of time - // TODO(Horibe): write me - // ... - - const bool is_all_ok = lateral_deviation_ok && yaw_deviation_ok && speed_upper_deviation_ok && - speed_lower_deviation_ok && stop_ok && large_acceleration_ok && - large_lateral_acceleration_ok && large_lateral_acceleration_diff_ok; - - // set for debug info - { - debug_info_.is_all_ok = is_all_ok; - debug_info_.lateral_deviation_ok = lateral_deviation_ok; - debug_info_.yaw_deviation_ok = yaw_deviation_ok; - debug_info_.speed_upper_deviation_ok = speed_upper_deviation_ok; - debug_info_.speed_lower_deviation_ok = speed_lower_deviation_ok; - debug_info_.stop_ok = stop_ok; - debug_info_.large_acceleration_ok = large_acceleration_ok; - debug_info_.large_lateral_acceleration_ok = large_lateral_acceleration_ok; - debug_info_.large_lateral_acceleration_diff_ok = large_lateral_acceleration_diff_ok; - - debug_info_.current_speed = current_speed; - debug_info_.target_control_speed = target_control_speed; - debug_info_.target_planning_speed = target_planning_speed; - - debug_info_.lateral_deviation = lateral_deviation; - debug_info_.yaw_deviation = yaw_deviation; - debug_info_.speed_deviation = speed_deviation; - } - - // Engagement is ready if the vehicle is stopped. - // (this is checked in the end to calculate some debug values.) - if (param.allow_autonomous_in_stopped && std::abs(current_speed) < 0.01) { - debug_info_.is_all_ok = true; - debug_info_.engage_allowed_for_stopped_vehicle = true; - return true; - } - - return is_all_ok; -} - -State OperationModeTransitionManager::updateState(const std::shared_ptr data) -{ - const auto current_state = operation_mode_transition_manager_->getCurrentState(); - - operation_mode_transition_manager_->setData(data); - const auto next_state = operation_mode_transition_manager_->update(); - - // no state change - if (next_state == current_state) { - return current_state; - } - - // transit state - switch (next_state) { - case State::STOP: - operation_mode_transition_manager_ = std::make_unique(this); - break; - case State::REMOTE_OPERATOR: - operation_mode_transition_manager_ = std::make_unique(this); - break; - case State::MANUAL_DIRECT: - operation_mode_transition_manager_ = std::make_unique(this); - break; - case State::LOCAL_OPERATOR: - operation_mode_transition_manager_ = std::make_unique(this); - break; - case State::TRANSITION_TO_AUTO: - operation_mode_transition_manager_ = std::make_unique(this); - break; - case State::AUTONOMOUS: - operation_mode_transition_manager_ = std::make_unique(this); - break; - } - operation_mode_transition_manager_->setParam(stable_check_param_); - - if (next_state != operation_mode_transition_manager_->getCurrentState()) { - throw std::runtime_error("operation_mode_transition_manager: unexpected state change!"); - } - - return operation_mode_transition_manager_->getCurrentState(); -} - -} // namespace operation_mode_transition_manager - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(operation_mode_transition_manager::OperationModeTransitionManager) diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index f98079636d491..11c4fd070343f 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "operation_mode_transition_manager/state.hpp" +#include "state.hpp" -#include "motion_utils/motion_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include +#include #include #include @@ -27,135 +27,58 @@ using motion_utils::findNearestIndex; using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcYawDeviation; -EngageStateBase::EngageStateBase(const State state, rclcpp::Node * node) -: logger_(node->get_logger()), clock_(node->get_clock()), state_(state) +AutonomousMode::AutonomousMode(rclcpp::Node * node) +: logger_(node->get_logger()), clock_(node->get_clock()) { - // TODO(Horibe): move to manager. - srv_mode_change_client_ = node->create_client("control_mode_request"); -} - -State EngageStateBase::defaultUpdateOnManual() -{ - const bool all_engage_requirements_are_satisfied = data_->is_auto_available; - const bool is_engage_requested = isAuto(data_->requested_state); - - // manual to manual: change state directly - if (!is_engage_requested) { - return isManual(data_->requested_state) ? data_->requested_state : getCurrentState(); - } - - // manual to auto: control_more_request will be sent in TRANSITION_TO_AUTO state. - if (all_engage_requirements_are_satisfied) { - return State::TRANSITION_TO_AUTO; - } else { - RCLCPP_WARN(logger_, "engage requirements are not satisfied. Engage prohibited."); - return getCurrentState(); - } -} - -bool EngageStateBase::sendAutonomousModeRequest() -{ - bool success = true; - - auto request = std::make_shared(); - request->stamp = clock_->now(); - request->mode = ControlModeCommand::Request::AUTONOMOUS; - - const auto callback = [&](rclcpp::Client::SharedFuture future) { - success = future.get()->success; - if (!success) { - RCLCPP_WARN(logger_, "Autonomous mode change was rejected."); - } - }; - - srv_mode_change_client_->async_send_request(request, callback); - - // TODO(Horibe): handle request failure. Now, only timeout check is running in Transition state. - // auto future = srv_mode_change_client_->async_send_request(request, callback); - // rclcpp::spin_until_future_complete(node_, future); - - return success; -} - -bool TransitionToAutoState::checkVehicleOverride() -{ - const auto mode = data_->current_control_mode.mode; - - if (mode == ControlModeReport::AUTONOMOUS) { - is_vehicle_mode_change_done_ = true; + vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*node).getVehicleInfo(); + + sub_control_cmd_ = node->create_subscription( + "control_cmd", 1, + [this](const AckermannControlCommand::SharedPtr msg) { control_cmd_ = *msg; }); + + sub_kinematics_ = node->create_subscription( + "kinematics", 1, [this](const Odometry::SharedPtr msg) { kinematics_ = *msg; }); + + sub_trajectory_ = node->create_subscription( + "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); + + // params for mode change available + { + auto & p = engage_acceptable_param_; + p.allow_autonomous_in_stopped = + node->declare_parameter("engage_acceptable_limits.allow_autonomous_in_stopped"); + p.dist_threshold = node->declare_parameter("engage_acceptable_limits.dist_threshold"); + p.speed_upper_threshold = + node->declare_parameter("engage_acceptable_limits.speed_upper_threshold"); + p.speed_lower_threshold = + node->declare_parameter("engage_acceptable_limits.speed_lower_threshold"); + p.yaw_threshold = node->declare_parameter("engage_acceptable_limits.yaw_threshold"); + p.acc_threshold = node->declare_parameter("engage_acceptable_limits.acc_threshold"); + p.lateral_acc_threshold = + node->declare_parameter("engage_acceptable_limits.lateral_acc_threshold"); + p.lateral_acc_diff_threshold = + node->declare_parameter("engage_acceptable_limits.lateral_acc_diff_threshold"); } - if (is_vehicle_mode_change_done_) { - if (mode != ControlModeReport::AUTONOMOUS) { - return true; - } + // params for mode change completed + { + auto & p = stable_check_param_; + p.duration = node->declare_parameter("stable_check.duration"); + p.dist_threshold = node->declare_parameter("stable_check.dist_threshold"); + p.speed_upper_threshold = node->declare_parameter("stable_check.speed_upper_threshold"); + p.speed_lower_threshold = node->declare_parameter("stable_check.speed_lower_threshold"); + p.yaw_threshold = node->declare_parameter("stable_check.yaw_threshold"); } - return false; } -bool TransitionToAutoState::checkTransitionTimeout() const +void AutonomousMode::update(bool transition) { - if (data_->current_control_mode.mode == ControlModeReport::AUTONOMOUS) { - return false; - } - - constexpr auto timeout_thr = 3.0; - if ((clock_->now() - transition_requested_time_).seconds() > timeout_thr) { - return true; - } - return false; -} - -State TransitionToAutoState::update() -{ - // return to Manual soon if requested. - const bool is_disengage_requested = isManual(data_->requested_state); - if (is_disengage_requested) { - return data_->requested_state; - } - - // return to Manual when vehicle control_mode is set to Manual after Auto transition is done. - if (checkVehicleOverride()) { - data_->requested_state = State::MANUAL_DIRECT; - return State::MANUAL_DIRECT; - } - - if (checkTransitionTimeout()) { - RCLCPP_WARN(logger_, "time out for ControlMode change. Return to MANUAL state."); - data_->requested_state = State::MANUAL_DIRECT; - return State::MANUAL_DIRECT; - } - - // waiting transition of vehicle_cmd_gate - if (data_->current_gate_operation_mode.mode != OperationMode::TRANSITION_TO_AUTO) { - RCLCPP_INFO(logger_, "transition check: gate operation_mode is still NOT TransitionToAuto"); - return getCurrentState(); - } - - // send Autonomous mode request to vehicle - // NOTE: this should be done after gate_operation_mode is set to TRANSITION_TO_AUTO. Otherwise - // control_cmd with nominal filter will be sent to vehicle. - if (!is_control_mode_request_send_) { - sendAutonomousModeRequest(); - is_control_mode_request_send_ = true; - } - - // waiting transition of vehicle - if (data_->current_control_mode.mode != ControlModeReport::AUTONOMOUS) { - RCLCPP_INFO(logger_, "transition check: vehicle control_mode is still NOT Autonomous"); - return getCurrentState(); - } - - const bool is_system_stable = checkSystemStable(); - - if (is_system_stable) { - return State::AUTONOMOUS; - } else { - return getCurrentState(); + if (!transition) { + stable_start_time_.reset(); } } -bool TransitionToAutoState::checkSystemStable() +bool AutonomousMode::isModeChangeCompleted() { constexpr auto dist_max = 5.0; constexpr auto yaw_max = M_PI_4; @@ -165,29 +88,29 @@ bool TransitionToAutoState::checkSystemStable() return false; }; - if (data_->trajectory.points.size() < 2) { + if (trajectory_.points.size() < 2) { RCLCPP_INFO(logger_, "Not stable yet: trajectory size must be > 2"); return unstable(); } const auto closest_idx = - findNearestIndex(data_->trajectory.points, data_->kinematics.pose.pose, dist_max, yaw_max); + findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max); if (!closest_idx) { RCLCPP_INFO(logger_, "Not stable yet: closest point not found"); return unstable(); } - const auto closest_point = data_->trajectory.points.at(*closest_idx); + const auto closest_point = trajectory_.points.at(*closest_idx); // check for lateral deviation - const auto dist_deviation = calcDistance2d(closest_point.pose, data_->kinematics.pose.pose); + const auto dist_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); if (dist_deviation > stable_check_param_.dist_threshold) { RCLCPP_INFO(logger_, "Not stable yet: distance deviation is too large: %f", dist_deviation); return unstable(); } // check for yaw deviation - const auto yaw_deviation = calcYawDeviation(closest_point.pose, data_->kinematics.pose.pose); + const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); if (yaw_deviation > stable_check_param_.yaw_threshold) { RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation); return unstable(); @@ -195,7 +118,7 @@ bool TransitionToAutoState::checkSystemStable() // check for speed deviation const auto speed_deviation = - data_->kinematics.twist.twist.linear.x - closest_point.longitudinal_velocity_mps; + kinematics_.twist.twist.linear.x - closest_point.longitudinal_velocity_mps; if (speed_deviation > stable_check_param_.speed_upper_threshold) { RCLCPP_INFO(logger_, "Not stable yet: ego speed is too high: %f", speed_deviation); return unstable(); @@ -214,26 +137,136 @@ bool TransitionToAutoState::checkSystemStable() const double stable_time = (clock_->now() - *stable_start_time_).seconds(); const bool is_system_stable = stable_time > stable_check_param_.duration; RCLCPP_INFO(logger_, "Now stable: now duration: %f", stable_time); - return is_system_stable; } -State AutonomousState::update() +bool AutonomousMode::hasDangerAcceleration() { - // check current mode - if (data_->current_control_mode.mode == ControlModeReport::MANUAL) { - data_->requested_state = State::MANUAL_DIRECT; - return State::MANUAL_DIRECT; + debug_info_.target_control_acceleration = control_cmd_.longitudinal.acceleration; + + const bool is_stopping = std::abs(kinematics_.twist.twist.linear.x) < 0.01; + if (is_stopping) { + return false; // any acceleration is ok when stopped } - // check request mode - bool is_disengage_requested = isManual(data_->requested_state); + const bool has_large_acc = + std::abs(control_cmd_.longitudinal.acceleration) > engage_acceptable_param_.acc_threshold; + return has_large_acc; +} + +std::pair AutonomousMode::hasDangerLateralAcceleration() +{ + const auto wheelbase = vehicle_info_.wheel_base_m; + const auto curr_vx = kinematics_.twist.twist.linear.x; + const auto curr_wz = kinematics_.twist.twist.angular.z; + + // Calculate angular velocity from kinematics model. + // Use current_vx to focus on the steering behavior. + const auto target_wz = curr_vx * std::tan(control_cmd_.lateral.steering_tire_angle) / wheelbase; + + const auto curr_lat_acc = curr_vx * curr_wz; + const auto target_lat_acc = curr_vx * target_wz; + + const bool has_large_lat_acc = + std::abs(curr_lat_acc) > engage_acceptable_param_.lateral_acc_threshold; + const bool has_large_lat_acc_diff = + std::abs(curr_lat_acc - target_lat_acc) > engage_acceptable_param_.lateral_acc_diff_threshold; + + debug_info_.lateral_acceleration = curr_lat_acc; + debug_info_.lateral_acceleration_deviation = curr_lat_acc - target_lat_acc; - if (is_disengage_requested) { - return data_->requested_state; - } else { - return getCurrentState(); + return {has_large_lat_acc, has_large_lat_acc_diff}; +} + +bool AutonomousMode::isModeChangeAvailable() +{ + constexpr auto dist_max = 100.0; + constexpr auto yaw_max = M_PI_4; + + const auto current_speed = kinematics_.twist.twist.linear.x; + const auto target_control_speed = control_cmd_.longitudinal.speed; + const auto & param = engage_acceptable_param_; + + if (trajectory_.points.size() < 2) { + RCLCPP_WARN_SKIPFIRST_THROTTLE( + logger_, *clock_, 5000, "Engage unavailable: trajectory size must be > 2"); + debug_info_ = DebugInfo{}; // all false + return false; + } + + const auto closest_idx = + findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max); + if (!closest_idx) { + RCLCPP_INFO(logger_, "Engage unavailable: closest point not found"); + debug_info_ = DebugInfo{}; // all false + return false; // closest trajectory point not found. + } + const auto closest_point = trajectory_.points.at(*closest_idx); + const auto target_planning_speed = closest_point.longitudinal_velocity_mps; + debug_info_.trajectory_available_ok = true; + + // No engagement is lateral control error is large + const auto lateral_deviation = calcDistance2d(closest_point.pose, kinematics_.pose.pose); + const bool lateral_deviation_ok = lateral_deviation < param.dist_threshold; + + // No engagement is yaw control error is large + const auto yaw_deviation = calcYawDeviation(closest_point.pose, kinematics_.pose.pose); + const bool yaw_deviation_ok = yaw_deviation < param.yaw_threshold; + + // No engagement if speed control error is large + const auto speed_deviation = current_speed - target_planning_speed; + const bool speed_upper_deviation_ok = speed_deviation <= param.speed_upper_threshold; + const bool speed_lower_deviation_ok = speed_deviation >= param.speed_lower_threshold; + + // No engagement if the vehicle is moving but the target speed is zero. + const bool stop_ok = !(std::abs(current_speed) > 0.1 && std::abs(target_control_speed) < 0.01); + + // No engagement if the large acceleration is commanded. + const bool large_acceleration_ok = !hasDangerAcceleration(); + + // No engagement if the lateral acceleration is over threshold + const auto [has_large_lat_acc, has_large_lat_acc_diff] = hasDangerLateralAcceleration(); + const auto large_lateral_acceleration_ok = !has_large_lat_acc; + const auto large_lateral_acceleration_diff_ok = !has_large_lat_acc_diff; + + // No engagement if a stop is expected within a certain period of time + // TODO(Horibe): write me + // ... + + const bool is_all_ok = lateral_deviation_ok && yaw_deviation_ok && speed_upper_deviation_ok && + speed_lower_deviation_ok && stop_ok && large_acceleration_ok && + large_lateral_acceleration_ok && large_lateral_acceleration_diff_ok; + + // set for debug info + { + debug_info_.is_all_ok = is_all_ok; + debug_info_.lateral_deviation_ok = lateral_deviation_ok; + debug_info_.yaw_deviation_ok = yaw_deviation_ok; + debug_info_.speed_upper_deviation_ok = speed_upper_deviation_ok; + debug_info_.speed_lower_deviation_ok = speed_lower_deviation_ok; + debug_info_.stop_ok = stop_ok; + debug_info_.large_acceleration_ok = large_acceleration_ok; + debug_info_.large_lateral_acceleration_ok = large_lateral_acceleration_ok; + debug_info_.large_lateral_acceleration_diff_ok = large_lateral_acceleration_diff_ok; + + debug_info_.current_speed = current_speed; + debug_info_.target_control_speed = target_control_speed; + debug_info_.target_planning_speed = target_planning_speed; + + debug_info_.lateral_deviation = lateral_deviation; + debug_info_.yaw_deviation = yaw_deviation; + debug_info_.speed_deviation = speed_deviation; } + + // Engagement is ready if the vehicle is stopped. + // (this is checked in the end to calculate some debug values.) + if (param.allow_autonomous_in_stopped && std::abs(current_speed) < 0.01) { + debug_info_.is_all_ok = true; + debug_info_.engage_allowed_for_stopped_vehicle = true; + return true; + } + + return is_all_ok; } } // namespace operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp new file mode 100644 index 0000000000000..bb8665e44c7d5 --- /dev/null +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -0,0 +1,104 @@ +// Copyright 2022 Autoware Foundation +// +// 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 STATE_HPP_ +#define STATE_HPP_ + +#include "data.hpp" +#include "operation_mode_transition_manager/msg/operation_mode_transition_manager_debug.hpp" + +#include +#include + +#include +#include +#include + +#include +#include + +namespace operation_mode_transition_manager +{ + +class ModeChangeBase +{ +public: + virtual ~ModeChangeBase() = default; + virtual void update([[maybe_unused]] bool transition) {} + virtual bool isModeChangeCompleted() = 0; + virtual bool isModeChangeAvailable() = 0; + + using DebugInfo = operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; + virtual DebugInfo getDebugInfo() { return DebugInfo{}; } +}; + +class StopMode : public ModeChangeBase +{ +public: + bool isModeChangeCompleted() override { return true; } + bool isModeChangeAvailable() override { return true; } +}; + +class AutonomousMode : public ModeChangeBase +{ +public: + explicit AutonomousMode(rclcpp::Node * node); + void update(bool transition) override; + bool isModeChangeCompleted() override; + bool isModeChangeAvailable() override; + DebugInfo getDebugInfo() override { return debug_info_; } + +private: + bool hasDangerAcceleration(); + std::pair hasDangerLateralAcceleration(); + + using AckermannControlCommand = autoware_auto_control_msgs::msg::AckermannControlCommand; + using Odometry = nav_msgs::msg::Odometry; + using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; + rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + EngageAcceptableParam engage_acceptable_param_; + StableCheckParam stable_check_param_; + AckermannControlCommand control_cmd_; + Odometry kinematics_; + Trajectory trajectory_; + vehicle_info_util::VehicleInfo vehicle_info_; + + DebugInfo debug_info_; + std::shared_ptr stable_start_time_; // Reset every transition start. +}; + +// TODO(Takagi, Isamu): Connect with status from local operation node +class LocalMode : public ModeChangeBase +{ +public: + bool isModeChangeCompleted() override { return true; } + bool isModeChangeAvailable() override { return true; } +}; + +// TODO(Takagi, Isamu): Connect with status from remote operation node +class RemoteMode : public ModeChangeBase +{ +public: + bool isModeChangeCompleted() override { return true; } + bool isModeChangeAvailable() override { return true; } +}; + +} // namespace operation_mode_transition_manager + +#endif // STATE_HPP_ diff --git a/control/vehicle_cmd_gate/README.md b/control/vehicle_cmd_gate/README.md index 45eae03cde7a2..3ff5bba47a173 100644 --- a/control/vehicle_cmd_gate/README.md +++ b/control/vehicle_cmd_gate/README.md @@ -26,7 +26,7 @@ | `~/input/emergency/hazard_lights_cmd` | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command from emergency handler | | `~/input/emergency/gear_cmd` | `autoware_auto_vehicle_msgs::msg::GearCommand` | gear command from emergency handler | | `~/input/engage` | `autoware_auto_vehicle_msgs::msg::Engage` | engage signal | -| `~/input/operation_mode` | `tier4_system_msgs::msg::OperationMode` | operation mode of Autoware | +| `~/input/operation_mode` | `autoware_adapi_v1_msgs::msg::OperationModeState` | operation mode of Autoware | ### Output diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index 2b65858601624..ca509f653fdb6 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -13,6 +13,7 @@ autoware_cmake + autoware_adapi_v1_msgs autoware_auto_control_msgs autoware_auto_system_msgs autoware_auto_vehicle_msgs diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 639157b0f9e0d..81cedb34f622a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -66,7 +66,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) engage_pub_ = this->create_publisher("output/engage", durable_qos); pub_external_emergency_ = this->create_publisher("output/external_emergency", durable_qos); - operation_mode_pub_ = this->create_publisher("output/operation_mode", durable_qos); + operation_mode_pub_ = + this->create_publisher("output/operation_mode", durable_qos); // Subscriber emergency_state_sub_ = this->create_subscription( @@ -80,10 +81,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) "input/engage", 1, std::bind(&VehicleCmdGate::onEngage, this, _1)); steer_sub_ = this->create_subscription( "input/steering", 1, std::bind(&VehicleCmdGate::onSteering, this, _1)); - operation_mode_sub_ = this->create_subscription( - "input/operation_mode", 1, [this](const tier4_system_msgs::msg::OperationMode::SharedPtr msg) { - current_operation_mode_ = *msg; - }); + operation_mode_sub_ = this->create_subscription( + "input/operation_mode", rclcpp::QoS(1).transient_local(), + [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ = *msg; }); // Subscriber for auto auto_control_cmd_sub_ = this->create_subscription( @@ -164,6 +164,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) // Set default value current_gate_mode_.data = GateMode::AUTO; + current_operation_mode_.mode = OperationModeState::STOP; // Service srv_engage_ = create_service( @@ -497,11 +498,10 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont { AckermannControlCommand out = in; const double dt = getDt(); - - const auto mode = current_operation_mode_.mode; + const auto mode = current_operation_mode_; // Apply transition_filter when transiting from MANUAL to AUTO. - if (mode == OperationMode::TRANSITION_TO_AUTO) { + if (mode.is_in_transition) { filter_on_transition_.filterAll(dt, current_steer_, out); } else { filter_.filterAll(dt, current_steer_, out); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 589ffa48a266b..0cd80245fd972 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -37,7 +38,6 @@ #include #include #include -#include #include #include @@ -45,6 +45,7 @@ namespace vehicle_cmd_gate { +using autoware_adapi_v1_msgs::msg::OperationModeState; using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_system_msgs::msg::EmergencyState; using autoware_auto_vehicle_msgs::msg::GearCommand; @@ -55,7 +56,6 @@ using tier4_control_msgs::msg::GateMode; using tier4_external_api_msgs::msg::Emergency; using tier4_external_api_msgs::msg::Heartbeat; using tier4_external_api_msgs::srv::SetEmergency; -using tier4_system_msgs::msg::OperationMode; using tier4_vehicle_msgs::msg::VehicleEmergencyStamped; using diagnostic_msgs::msg::DiagnosticStatus; @@ -90,14 +90,14 @@ class VehicleCmdGate : public rclcpp::Node rclcpp::Publisher::SharedPtr hazard_light_cmd_pub_; rclcpp::Publisher::SharedPtr gate_mode_pub_; rclcpp::Publisher::SharedPtr engage_pub_; - rclcpp::Publisher::SharedPtr operation_mode_pub_; + rclcpp::Publisher::SharedPtr operation_mode_pub_; // Subscription rclcpp::Subscription::SharedPtr emergency_state_sub_; rclcpp::Subscription::SharedPtr external_emergency_stop_heartbeat_sub_; rclcpp::Subscription::SharedPtr gate_mode_sub_; rclcpp::Subscription::SharedPtr steer_sub_; - rclcpp::Subscription::SharedPtr operation_mode_sub_; + rclcpp::Subscription::SharedPtr operation_mode_sub_; void onGateMode(GateMode::ConstSharedPtr msg); void onEmergencyState(EmergencyState::ConstSharedPtr msg); @@ -214,7 +214,7 @@ class VehicleCmdGate : public rclcpp::Node AckermannControlCommand filterControlCommand(const AckermannControlCommand & msg); // filtering on transition - OperationMode current_operation_mode_; + OperationModeState current_operation_mode_; VehicleCmdFilter filter_on_transition_; // Pause interface for API diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml index d42e2392bea0f..be18812b59eba 100644 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + transition_timeout: 3.0 frequency_hz: 10.0 engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index e41993f5edd1f..86be1019df728 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -173,7 +173,7 @@ def launch_setup(context, *args, **kwargs): remappings=[ ("input/emergency_state", "/system/emergency/emergency_state"), ("input/steering", "/vehicle/status/steering_status"), - ("input/operation_mode", "/control/operation_mode"), + ("input/operation_mode", "/system/operation_mode/state"), ("input/auto/control_cmd", "/control/trajectory_follower/control_cmd"), ("input/auto/turn_indicators_cmd", "/planning/turn_indicators_cmd"), ("input/auto/hazard_lights_cmd", "/planning/hazard_lights_cmd"), @@ -228,10 +228,8 @@ def launch_setup(context, *args, **kwargs): ("control_cmd", "/control/command/control_cmd"), ("control_mode_report", "/vehicle/status/control_mode"), ("gate_operation_mode", "/control/vehicle_cmd_gate/operation_mode"), - ("operation_mode_request", "/system/operation_mode_request"), # output ("is_autonomous_available", "/control/is_autonomous_available"), - ("operation_mode", "/control/operation_mode"), ("control_mode_request", "/control/control_mode_request"), ], parameters=[