diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 26ac6dac1289f..bea6cd19c7a7e 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -8,6 +8,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/interface.cpp src/localization.cpp src/motion.cpp + src/operation_mode.cpp src/routing.cpp src/utils/route_conversion.cpp ) @@ -16,6 +17,7 @@ rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" + "default_ad_api::OperationModeNode" "default_ad_api::RoutingNode" ) diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index 636985e65a134..5f127894d17f9 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -18,22 +18,23 @@ from launch_ros.descriptions import ComposableNode -def _create_api_node(node_name, class_name, **kwargs): +def create_api_node(node_name, class_name, **kwargs): return ComposableNode( namespace="default_ad_api/node", name=node_name, package="default_ad_api", plugin="default_ad_api::" + class_name, - **kwargs + **kwargs, ) def generate_launch_description(): components = [ - _create_api_node("interface", "InterfaceNode"), - _create_api_node("localization", "LocalizationNode"), - _create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]), - _create_api_node("routing", "RoutingNode"), + create_api_node("interface", "InterfaceNode"), + create_api_node("localization", "LocalizationNode"), + create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]), + create_api_node("operation_mode", "OperationModeNode"), + create_api_node("routing", "RoutingNode"), ] container = ComposableNodeContainer( namespace="default_ad_api", diff --git a/system/default_ad_api/package.xml b/system/default_ad_api/package.xml index 97b94e5c13403..bc0d1edecb8dd 100644 --- a/system/default_ad_api/package.xml +++ b/system/default_ad_api/package.xml @@ -22,6 +22,7 @@ rclcpp rclcpp_components tier4_control_msgs + tier4_system_msgs python3-flask diff --git a/system/default_ad_api/src/interface.hpp b/system/default_ad_api/src/interface.hpp index 074b1454f6e61..46c70abf17f36 100644 --- a/system/default_ad_api/src/interface.hpp +++ b/system/default_ad_api/src/interface.hpp @@ -16,7 +16,6 @@ #define INTERFACE_HPP_ #include -#include #include // This file should be included after messages. diff --git a/system/default_ad_api/src/localization.hpp b/system/default_ad_api/src/localization.hpp index 077c60d86df01..71517c4c6c769 100644 --- a/system/default_ad_api/src/localization.hpp +++ b/system/default_ad_api/src/localization.hpp @@ -17,7 +17,6 @@ #include #include -#include #include // This file should be included after messages. diff --git a/system/default_ad_api/src/operation_mode.cpp b/system/default_ad_api/src/operation_mode.cpp new file mode 100644 index 0000000000000..a61a1b75697ab --- /dev/null +++ b/system/default_ad_api/src/operation_mode.cpp @@ -0,0 +1,168 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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.hpp" + +#include +#include +#include + +namespace default_ad_api +{ + +using ServiceResponse = autoware_adapi_v1_msgs::srv::ChangeOperationMode::Response; + +OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options) +: Node("operation_mode", options) +{ + const auto adaptor = component_interface_utils::NodeAdaptor(this); + group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + adaptor.init_sub(sub_state_, this, &OperationModeNode::on_state); + adaptor.init_pub(pub_state_); + adaptor.init_srv(srv_stop_mode_, this, &OperationModeNode::on_change_to_stop); + adaptor.init_srv(srv_autonomous_mode_, this, &OperationModeNode::on_change_to_autonomous); + adaptor.init_srv(srv_local_mode_, this, &OperationModeNode::on_change_to_local); + adaptor.init_srv(srv_remote_mode_, this, &OperationModeNode::on_change_to_remote); + adaptor.init_srv(srv_enable_control_, this, &OperationModeNode::on_enable_autoware_control); + adaptor.init_srv(srv_disable_control_, this, &OperationModeNode::on_disable_autoware_control); + adaptor.init_cli(cli_mode_, group_cli_); + adaptor.init_cli(cli_control_, group_cli_); + + const std::vector module_names = { + "sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system", + }; + + for (size_t i = 0; i < module_names.size(); ++i) { + const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i]; + const auto qos = rclcpp::QoS(1).transient_local(); + const auto callback = [this, i](const ModeChangeAvailable::ConstSharedPtr msg) { + module_states_[i] = msg->available; + }; + sub_module_states_.push_back(create_subscription(name, qos, callback)); + } + module_states_.resize(module_names.size()); + + timer_ = rclcpp::create_timer( + this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this)); + + curr_state_.mode = OperationModeState::Message::UNKNOWN; + prev_state_.mode = OperationModeState::Message::UNKNOWN; + mode_available_[OperationModeState::Message::UNKNOWN] = false; + mode_available_[OperationModeState::Message::STOP] = true; + mode_available_[OperationModeState::Message::AUTONOMOUS] = false; + mode_available_[OperationModeState::Message::LOCAL] = true; + mode_available_[OperationModeState::Message::REMOTE] = true; +} + +template +void OperationModeNode::change_mode( + const ResponseT res, const OperationModeRequest::_mode_type mode) +{ + if (!mode_available_[mode]) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change is blocked by the system."); + } + const auto req = std::make_shared(); + req->mode = mode; + component_interface_utils::status::copy(cli_mode_->call(req), res); // NOLINT +} + +void OperationModeNode::on_change_to_stop( + const ChangeToStop::Service::Request::SharedPtr, + const ChangeToStop::Service::Response::SharedPtr res) +{ + change_mode(res, OperationModeRequest::STOP); +} + +void OperationModeNode::on_change_to_autonomous( + const ChangeToAutonomous::Service::Request::SharedPtr, + const ChangeToAutonomous::Service::Response::SharedPtr res) +{ + change_mode(res, OperationModeRequest::AUTONOMOUS); +} + +void OperationModeNode::on_change_to_local( + const ChangeToLocal::Service::Request::SharedPtr, + const ChangeToLocal::Service::Response::SharedPtr res) +{ + change_mode(res, OperationModeRequest::LOCAL); +} + +void OperationModeNode::on_change_to_remote( + const ChangeToRemote::Service::Request::SharedPtr, + const ChangeToRemote::Service::Response::SharedPtr res) +{ + change_mode(res, OperationModeRequest::REMOTE); +} + +void OperationModeNode::on_enable_autoware_control( + const EnableAutowareControl::Service::Request::SharedPtr, + const EnableAutowareControl::Service::Response::SharedPtr res) +{ + if (!mode_available_[curr_state_.mode]) { + throw component_interface_utils::ServiceException( + ServiceResponse::ERROR_NOT_AVAILABLE, "The mode change is blocked by the system."); + } + const auto req = std::make_shared(); + req->autoware_control = true; + component_interface_utils::status::copy(cli_control_->call(req), res); // NOLINT +} + +void OperationModeNode::on_disable_autoware_control( + const DisableAutowareControl::Service::Request::SharedPtr, + const DisableAutowareControl::Service::Response::SharedPtr res) +{ + const auto req = std::make_shared(); + req->autoware_control = false; + component_interface_utils::status::copy(cli_control_->call(req), res); // NOLINT +} + +void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedPtr msg) +{ + curr_state_ = *msg; + update_state(); +} + +void OperationModeNode::on_timer() +{ + bool autonomous_available = true; + for (const auto & state : module_states_) { + autonomous_available &= state; + } + mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available; + + update_state(); +} + +void OperationModeNode::update_state() +{ + // Clear stamp to compare other fields. + OperationModeState::Message state = curr_state_; + state.stamp = builtin_interfaces::msg::Time(); + state.is_stop_mode_available &= mode_available_[OperationModeState::Message::STOP]; + state.is_autonomous_mode_available &= mode_available_[OperationModeState::Message::AUTONOMOUS]; + state.is_local_mode_available &= mode_available_[OperationModeState::Message::LOCAL]; + state.is_remote_mode_available &= mode_available_[OperationModeState::Message::REMOTE]; + + if (prev_state_ != state) { + prev_state_ = state; + state.stamp = now(); + pub_state_->publish(state); + } +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::OperationModeNode) diff --git a/system/default_ad_api/src/operation_mode.hpp b/system/default_ad_api/src/operation_mode.hpp new file mode 100644 index 0000000000000..5f5699f42cab7 --- /dev/null +++ b/system/default_ad_api/src/operation_mode.hpp @@ -0,0 +1,100 @@ +// Copyright 2022 TIER IV, Inc. +// +// 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_HPP_ +#define OPERATION_MODE_HPP_ + +#include +#include +#include +#include + +#include +#include + +// TODO(Takagi, Isamu): define interface +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ +class OperationModeNode : public rclcpp::Node +{ +public: + explicit OperationModeNode(const rclcpp::NodeOptions & options); + +private: + using OperationModeState = autoware_ad_api::operation_mode::OperationModeState; + using EnableAutowareControl = autoware_ad_api::operation_mode::EnableAutowareControl; + using DisableAutowareControl = autoware_ad_api::operation_mode::DisableAutowareControl; + using ChangeToStop = autoware_ad_api::operation_mode::ChangeToStop; + using ChangeToAutonomous = autoware_ad_api::operation_mode::ChangeToAutonomous; + using ChangeToLocal = autoware_ad_api::operation_mode::ChangeToLocal; + using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote; + using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request; + using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request; + using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable; + + OperationModeState::Message curr_state_; + OperationModeState::Message prev_state_; + std::unordered_map mode_available_; + + rclcpp::CallbackGroup::SharedPtr group_cli_; + rclcpp::TimerBase::SharedPtr timer_; + Pub pub_state_; + Srv srv_stop_mode_; + Srv srv_autonomous_mode_; + Srv srv_local_mode_; + Srv srv_remote_mode_; + Srv srv_enable_control_; + Srv srv_disable_control_; + Sub sub_state_; + Cli cli_mode_; + Cli cli_control_; + + std::vector module_states_; + std::vector::SharedPtr> sub_module_states_; + + void on_change_to_stop( + const ChangeToStop::Service::Request::SharedPtr req, + const ChangeToStop::Service::Response::SharedPtr res); + void on_change_to_autonomous( + const ChangeToAutonomous::Service::Request::SharedPtr req, + const ChangeToAutonomous::Service::Response::SharedPtr res); + void on_change_to_local( + const ChangeToLocal::Service::Request::SharedPtr req, + const ChangeToLocal::Service::Response::SharedPtr res); + void on_change_to_remote( + const ChangeToRemote::Service::Request::SharedPtr req, + const ChangeToRemote::Service::Response::SharedPtr res); + void on_enable_autoware_control( + const EnableAutowareControl::Service::Request::SharedPtr req, + const EnableAutowareControl::Service::Response::SharedPtr res); + void on_disable_autoware_control( + const DisableAutowareControl::Service::Request::SharedPtr req, + const DisableAutowareControl::Service::Response::SharedPtr res); + + void on_state(const OperationModeState::Message::ConstSharedPtr msg); + void on_timer(); + void update_state(); + + template + void change_mode(const ResponseT res, const OperationModeRequest::_mode_type mode); +}; + +} // namespace default_ad_api + +#endif // OPERATION_MODE_HPP_