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_