feat(default_ad_api): add operation mode api (autowarefoundation#1569)

* feat(autoware_ad_api_msgs): define operation mode interface

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat(default_ad_api): add operation mode api

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: add message

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* 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>

* fix: add message callback

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: add topic monitoring

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: use topic monitoring

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: modify topic monitoring config

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: config name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: modify diag name

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: move adapi message

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: change message type

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: merge

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* WIP

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix: fix build error

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: move diagnostics

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: remove diagnostics

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: modify error message

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: remove unused code

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

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"
 )
 

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",

system/default_ad_api/package.xml

@@ -22,6 +22,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>tier4_control_msgs</depend>
+  <depend>tier4_system_msgs</depend>
 
   <exec_depend>python3-flask</exec_depend>
 

system/default_ad_api/src/interface.hpp

@@ -16,7 +16,6 @@
 #define INTERFACE_HPP_
 
 #include <autoware_ad_api_specs/interface.hpp>
-#include <component_interface_utils/rclcpp.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 // This file should be included after messages.

system/default_ad_api/src/localization.hpp

@@ -17,7 +17,6 @@
 
 #include <autoware_ad_api_specs/localization.hpp>
 #include <component_interface_specs/localization.hpp>
-#include <component_interface_utils/rclcpp.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 // This file should be included after messages.

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 <memory>
+#include <string>
+#include <vector>
+
+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<std::string> 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<ModeChangeAvailable>(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 <class ResponseT>
+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<OperationModeRequest>();
+  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<AutowareControlRequest>();
+  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<AutowareControlRequest>();
+  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_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::OperationModeNode)

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 <autoware_ad_api_specs/operation_mode.hpp>
+#include <component_interface_specs/system.hpp>
+#include <component_interface_utils/status.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <unordered_map>
+#include <vector>
+
+// TODO(Takagi, Isamu): define interface
+#include <tier4_system_msgs/msg/mode_change_available.hpp>
+
+// 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<OperationModeState::Message::_mode_type, bool> mode_available_;
+
+  rclcpp::CallbackGroup::SharedPtr group_cli_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  Pub<autoware_ad_api::operation_mode::OperationModeState> pub_state_;
+  Srv<autoware_ad_api::operation_mode::ChangeToStop> srv_stop_mode_;
+  Srv<autoware_ad_api::operation_mode::ChangeToAutonomous> srv_autonomous_mode_;
+  Srv<autoware_ad_api::operation_mode::ChangeToLocal> srv_local_mode_;
+  Srv<autoware_ad_api::operation_mode::ChangeToRemote> srv_remote_mode_;
+  Srv<autoware_ad_api::operation_mode::EnableAutowareControl> srv_enable_control_;
+  Srv<autoware_ad_api::operation_mode::DisableAutowareControl> srv_disable_control_;
+  Sub<system_interface::OperationModeState> sub_state_;
+  Cli<system_interface::ChangeOperationMode> cli_mode_;
+  Cli<system_interface::ChangeAutowareControl> cli_control_;
+
+  std::vector<bool> module_states_;
+  std::vector<rclcpp::Subscription<ModeChangeAvailable>::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 <class ResponseT>
+  void change_mode(const ResponseT res, const OperationModeRequest::_mode_type mode);
+};
+
+}  // namespace default_ad_api
+
+#endif  // OPERATION_MODE_HPP_