diff --git a/control/operation_mode_transition_manager/CMakeLists.txt b/control/operation_mode_transition_manager/CMakeLists.txt new file mode 100644 index 0000000000000..d281961b2c7e3 --- /dev/null +++ b/control/operation_mode_transition_manager/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.14) +project(operation_mode_transition_manager) + +find_package(autoware_cmake REQUIRED) +autoware_package() +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/data.cpp +) + +rclcpp_components_register_node(operation_mode_transition_manager_node + PLUGIN "operation_mode_transition_manager::OperationModeTransitionManager" + EXECUTABLE operation_mode_transition_manager_exe +) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/OperationModeTransitionManagerDebug.msg" + DEPENDENCIES builtin_interfaces +) + +# to use same package defined message +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(operation_mode_transition_manager_node + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(operation_mode_transition_manager_node "${cpp_typesupport_target}") +endif() + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md new file mode 100644 index 0000000000000..e852b552130e8 --- /dev/null +++ b/control/operation_mode_transition_manager/README.md @@ -0,0 +1 @@ +# operation_mode_transition_manager diff --git a/control/operation_mode_transition_manager/config/engage_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/engage_transition_manager.param.yaml new file mode 100644 index 0000000000000..d42e2392bea0f --- /dev/null +++ b/control/operation_mode_transition_manager/config/engage_transition_manager.param.yaml @@ -0,0 +1,18 @@ +/**: + ros__parameters: + frequency_hz: 10.0 + engage_acceptable_limits: + allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. + dist_threshold: 1.5 + yaw_threshold: 0.524 + speed_upper_threshold: 10.0 + speed_lower_threshold: -10.0 + acc_threshold: 1.5 + lateral_acc_threshold: 1.0 + lateral_acc_diff_threshold: 0.5 + stable_check: + duration: 0.1 + dist_threshold: 1.5 + speed_upper_threshold: 2.0 + speed_lower_threshold: -2.0 + yaw_threshold: 0.262 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 new file mode 100644 index 0000000000000..037b6d537b593 --- /dev/null +++ b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/data.hpp @@ -0,0 +1,101 @@ +// 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 +#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 operation_mode_transition_manager::msg::OperationModeTransitionManagerDebug; +using tier4_system_msgs::msg::IsAutonomousAvailable; +using tier4_system_msgs::msg::OperationMode; +using tier4_system_msgs::srv::OperationModeRequest; +using tier4_vehicle_msgs::msg::ControlMode; +using tier4_vehicle_msgs::srv::ControlModeRequest; + +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 new file mode 100644 index 0000000000000..2bda5b300ec55 --- /dev/null +++ b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/operation_mode_transition_manager.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 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 +#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 new file mode 100644 index 0000000000000..2db2e91a891e0 --- /dev/null +++ b/control/operation_mode_transition_manager/include/operation_mode_transition_manager/state.hpp @@ -0,0 +1,121 @@ +// 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 new file mode 100644 index 0000000000000..efdfc1c6041e0 --- /dev/null +++ b/control/operation_mode_transition_manager/launch/operation_mode_transition_manager.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg b/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg new file mode 100644 index 0000000000000..1dc13208b666a --- /dev/null +++ b/control/operation_mode_transition_manager/msg/OperationModeTransitionManagerDebug.msg @@ -0,0 +1,29 @@ +builtin_interfaces/Time stamp + +# states +string requested_state +string current_state + +# flags for engage permission +bool is_all_ok +bool engage_allowed_for_stopped_vehicle +bool trajectory_available_ok +bool lateral_deviation_ok +bool yaw_deviation_ok +bool speed_upper_deviation_ok +bool speed_lower_deviation_ok +bool stop_ok +bool large_acceleration_ok +bool large_lateral_acceleration_ok +bool large_lateral_acceleration_diff_ok + +# values +float64 current_speed +float64 target_control_speed +float64 target_planning_speed +float64 target_control_acceleration +float64 lateral_acceleration +float64 lateral_acceleration_deviation +float64 lateral_deviation +float64 yaw_deviation +float64 speed_deviation diff --git a/control/operation_mode_transition_manager/package.xml b/control/operation_mode_transition_manager/package.xml new file mode 100644 index 0000000000000..73d6f8cb3eb79 --- /dev/null +++ b/control/operation_mode_transition_manager/package.xml @@ -0,0 +1,34 @@ + + operation_mode_transition_manager + 0.1.0 + The vehicle_cmd_gate package + Takamasa Horibe + Apache License 2.0 + + autoware_cmake + rosidl_default_generators + + autoware_auto_control_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + geometry_msgs + rclcpp + rclcpp_components + std_srvs + tier4_autoware_utils + tier4_control_msgs + tier4_system_msgs + tier4_vehicle_msgs + vehicle_info_util + + ament_cmake_gtest + ament_lint_auto + autoware_lint_common + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/control/operation_mode_transition_manager/src/data.cpp b/control/operation_mode_transition_manager/src/data.cpp new file mode 100644 index 0000000000000..744c0c4983102 --- /dev/null +++ b/control/operation_mode_transition_manager/src/data.cpp @@ -0,0 +1,99 @@ +// 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/state.hpp" +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include +#include + +namespace operation_mode_transition_manager +{ + +uint8_t toMsg(const State s) +{ + 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; + } + 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; + } + return State::STOP; +} + +bool isManual(const State s) +{ + if ( + s == State::STOP || s == State::REMOTE_OPERATOR || s == State::MANUAL_DIRECT || + s == State::LOCAL_OPERATOR) { + return true; + } else { + return false; + } +} + +bool isAuto(const State s) +{ + if (s == State::AUTONOMOUS) { + return true; + } else { + return false; + } +} + +std::string toStr(const State s) +{ + 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"; + } +} + +} // namespace operation_mode_transition_manager 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 new file mode 100644 index 0000000000000..e132b7655136d --- /dev/null +++ b/control/operation_mode_transition_manager/src/operation_mode_transition_manager.cpp @@ -0,0 +1,338 @@ +// 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 "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include +#include + +namespace operation_mode_transition_manager +{ + +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcYawDeviation; +using tier4_autoware_utils::findNearestIndex; + +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(get_logger(), "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 new file mode 100644 index 0000000000000..374c80946bd3a --- /dev/null +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -0,0 +1,238 @@ +// 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/state.hpp" + +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include +#include + +namespace operation_mode_transition_manager +{ + +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcYawDeviation; +using tier4_autoware_utils::findNearestIndex; + +EngageStateBase::EngageStateBase(const State state, rclcpp::Node * node) +: logger_(node->get_logger()), clock_(node->get_clock()), state_(state) +{ + // 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->mode.header.stamp = clock_->now(); + request->mode.data = ControlMode::AUTO; + + 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; + } + + if (is_vehicle_mode_change_done_) { + if (mode != ControlModeReport::AUTONOMOUS) { + return true; + } + } + return false; +} + +bool TransitionToAutoState::checkTransitionTimeout() const +{ + 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(); + } +} + +bool TransitionToAutoState::checkSystemStable() +{ + constexpr auto dist_max = 5.0; + constexpr auto yaw_max = M_PI_4; + + const auto unstable = [this]() { + stable_start_time_.reset(); + return false; + }; + + if (data_->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); + 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); + + // check for lateral deviation + const auto dist_deviation = calcDistance2d(closest_point.pose, data_->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); + if (yaw_deviation > stable_check_param_.yaw_threshold) { + RCLCPP_INFO(logger_, "Not stable yet: yaw deviation is too large: %f", yaw_deviation); + return unstable(); + } + + // check for speed deviation + const auto speed_deviation = + data_->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(); + } + if (speed_deviation < stable_check_param_.speed_lower_threshold) { + RCLCPP_INFO(logger_, "Not stable yet: ego speed is too low: %f", speed_deviation); + return unstable(); + } + + // count start. + if (!stable_start_time_) { + stable_start_time_ = std::make_unique(clock_->now()); + } + + // keep being stable for enough time. + 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() +{ + // check current mode + if (data_->current_control_mode.mode == ControlModeReport::MANUAL) { + data_->requested_state = State::MANUAL_DIRECT; + return State::MANUAL_DIRECT; + } + + // check request mode + bool is_disengage_requested = isManual(data_->requested_state); + + if (is_disengage_requested) { + return data_->requested_state; + } else { + return getCurrentState(); + } +} + +} // namespace operation_mode_transition_manager 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 new file mode 100644 index 0000000000000..789c25b3f4c3a --- /dev/null +++ b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -0,0 +1,17 @@ +/**: + ros__parameters: + frequency_hz: 10.0 + engage_acceptable_limits: + dist_threshold: 1.5 + yaw_threshold: 0.524 + speed_upper_threshold: 10.0 + speed_lower_threshold: -10.0 + acc_threshold: 1.5 + lateral_acc_threshold: 1.0 + lateral_acc_diff_threshold: 0.5 + stable_check: + duration: 0.1 + dist_threshold: 1.5 + speed_upper_threshold: 2.0 + speed_lower_threshold: -2.0 + yaw_threshold: 0.262 diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 8b6200b33ca4b..4d568a7aab762 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -50,6 +50,12 @@ def launch_setup(context, *args, **kwargs): with open(lane_departure_checker_param_path, "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + operation_mode_transition_manager_param_path = LaunchConfiguration( + "operation_mode_transition_manager_param_path" + ).perform(context) + with open(operation_mode_transition_manager_param_path, "r") as f: + operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] + controller_component = ComposableNode( package="trajectory_follower_nodes", plugin="autoware::motion::control::trajectory_follower_nodes::Controller", @@ -116,6 +122,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/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"), @@ -137,6 +144,7 @@ def launch_setup(context, *args, **kwargs): ("output/gate_mode", "/control/current_gate_mode"), ("output/engage", "/api/autoware/get/engage"), ("output/external_emergency", "/api/autoware/get/emergency"), + ("output/operation_mode", "/control/vehicle_cmd_gate/operation_mode"), ("~/service/engage", "/api/autoware/set/engage"), ("~/service/external_emergency", "/api/autoware/set/emergency"), # TODO(Takagi, Isamu): deprecated @@ -156,6 +164,31 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # operation mode transition manager + operation_mode_transition_manager_component = ComposableNode( + package="operation_mode_transition_manager", + plugin="operation_mode_transition_manager::OperationModeTransitionManager", + name="operation_mode_transition_manager", + remappings=[ + # input + ("kinematics", "/localization/kinematic_state"), + ("steering", "/vehicle/status/steering_status"), + ("trajectory", "/planning/scenario_planning/trajectory"), + ("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=[ + operation_mode_transition_manager_param, + vehicle_info_param, + ], + ) + # external cmd selector external_cmd_selector_loader = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -190,6 +223,7 @@ def launch_setup(context, *args, **kwargs): lane_departure_component, shift_decider_component, vehicle_cmd_gate_component, + operation_mode_transition_manager_component, ], ) @@ -258,6 +292,14 @@ def add_launch_arg(name: str, default_value=None, description=None): "lane_departure_checker_param_path", [FindPackageShare("lane_departure_checker"), "/config/lane_departure_checker.param.yaml"], ) + add_launch_arg( + "operation_mode_transition_manager_param_path", + [ + FindPackageShare("tier4_control_launch"), + "/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml", + ], + "path to the parameter file of vehicle_cmd_gate", + ) # velocity controller add_launch_arg("show_debug_info", "false", "show debug information") diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index e9f705cc69db6..ae96587af474e 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -61,7 +61,7 @@ def launch_setup(context, *args, **kwargs): ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"), ("input/trajectory", "/planning/scenario_planning/trajectory"), ("input/engage", "/vehicle/engage"), - ("input/control_mode_request", "/system/control_mode_request"), + ("input/control_mode_request", "/control/control_mode_request"), ("output/twist", "/vehicle/status/velocity_status"), ("output/odometry", "/localization/kinematic_state"), ("output/acceleration", "/localization/acceleration"),