feat(default_ad_api): add fail-safe api (autowarefoundation#2295)

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

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Signed-off-by: Kotaro Yoshimoto <pythagora.yoshimoto@gmail.com>

common/autoware_ad_api_specs/include/autoware_ad_api_specs/fail_safe.hpp

@@ -0,0 +1,36 @@
+// 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 AUTOWARE_AD_API_SPECS__FAIL_SAFE_HPP_
+#define AUTOWARE_AD_API_SPECS__FAIL_SAFE_HPP_
+
+#include <rclcpp/qos.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+
+namespace autoware_ad_api::fail_safe
+{
+
+struct MrmState
+{
+  using Message = autoware_adapi_v1_msgs::msg::MrmState;
+  static constexpr char name[] = "/api/fail_safe/mrm_state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
+}  // namespace autoware_ad_api::fail_safe
+
+#endif  // AUTOWARE_AD_API_SPECS__FAIL_SAFE_HPP_

common/component_interface_specs/include/component_interface_specs/system.hpp

@@ -17,13 +17,23 @@
 
 #include <rclcpp/qos.hpp>
 
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
 #include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
 #include <tier4_system_msgs/srv/change_autoware_control.hpp>
 #include <tier4_system_msgs/srv/change_operation_mode.hpp>
 
 namespace system_interface
 {
 
+struct MrmState
+{
+  using Message = autoware_adapi_v1_msgs::msg::MrmState;
+  static constexpr char name[] = "/system/fail_safe/mrm_state";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
+};
+
 struct ChangeAutowareControl
 {
   using Service = tier4_system_msgs::srv::ChangeAutowareControl;

system/default_ad_api/CMakeLists.txt

@@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/fail_safe.cpp
   src/interface.cpp
   src/localization.cpp
   src/motion.cpp
@@ -14,6 +15,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
 )
 
 rclcpp_components_register_nodes(${PROJECT_NAME}
+  "default_ad_api::FailSafeNode"
   "default_ad_api::InterfaceNode"
   "default_ad_api::LocalizationNode"
   "default_ad_api::MotionNode"

system/default_ad_api/launch/default_ad_api.launch.py

@@ -30,6 +30,7 @@ def create_api_node(node_name, class_name, **kwargs):
 
 def generate_launch_description():
     components = [
+        create_api_node("fail_safe", "FailSafeNode"),
         create_api_node("interface", "InterfaceNode"),
         create_api_node("localization", "LocalizationNode"),
         create_api_node("motion", "MotionNode", parameters=[{"require_accept_start": False}]),

system/default_ad_api/src/fail_safe.cpp

@@ -0,0 +1,41 @@
+// 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 "fail_safe.hpp"
+
+namespace default_ad_api
+{
+
+FailSafeNode::FailSafeNode(const rclcpp::NodeOptions & options) : Node("fail_safe", options)
+{
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_pub(pub_mrm_state_);
+  adaptor.init_sub(sub_mrm_state_, this, &FailSafeNode::on_state);
+
+  prev_state_.state = MrmState::UNKNOWN;
+}
+
+void FailSafeNode::on_state(const MrmState::ConstSharedPtr msg)
+{
+  prev_state_.stamp = msg->stamp;
+  if (prev_state_ != *msg) {
+    prev_state_ = *msg;
+    pub_mrm_state_->publish(*msg);
+  }
+}
+
+}  // namespace default_ad_api
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::FailSafeNode)

system/default_ad_api/src/fail_safe.hpp

@@ -0,0 +1,44 @@
+// 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 FAIL_SAFE_HPP_
+#define FAIL_SAFE_HPP_
+
+#include <autoware_ad_api_specs/fail_safe.hpp>
+#include <component_interface_specs/system.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+// This file should be included after messages.
+#include "utils/types.hpp"
+
+namespace default_ad_api
+{
+
+class FailSafeNode : public rclcpp::Node
+{
+public:
+  explicit FailSafeNode(const rclcpp::NodeOptions & options);
+
+private:
+  using MrmState = autoware_ad_api::fail_safe::MrmState::Message;
+  Pub<autoware_ad_api::fail_safe::MrmState> pub_mrm_state_;
+  Sub<system_interface::MrmState> sub_mrm_state_;
+  MrmState prev_state_;
+  void on_state(const MrmState::ConstSharedPtr msg);
+};
+
+}  // namespace default_ad_api
+
+#endif  // FAIL_SAFE_HPP_