feat: add publisher of mrm state

Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>

Author: TetsuKawa

system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml

@@ -1,11 +1,18 @@
 <launch>
   <arg name="mrm_stop_operator_param_path" default="$(find-pkg-share mrm_stop_operator)/config/mrm_stop_operator.param.yaml"/>
+  <arg name="input_mrm_request" default="/system/mrm_request"/>
+  <arg name="input_odometry" default="/localization/kinematic_state"/>
+  <arg name="output_max_velocity" default="/planning/scenario_planning/max_velocity_candidates"/>
+  <arg name="output_max_velocity_clear_command" default="/planning/scenario_planning/clear_velocity_limit"/>
+  <arg name="output_mrm_state" default="/system/fail_safe/mrm_state"/>
 
   <node pkg="mrm_stop_operator" exec="mrm_stop_operator_node" name="mrm_stop_operator" output="screen">
     <param from="$(var mrm_stop_operator_param_path)"/>
 
-    <remap from="~/input/mrm_request" to="/system/mrm_request"/>
-    <remap from="~/output/velocity_limit" to="/planning/scenario_planning/max_velocity_candidates"/>
-    <remap from="~/output/velocity_limit_clear_command" to="/planning/scenario_planning/clear_velocity_limit"/>
+    <remap from="~/input/mrm_request" to="$(var input_mrm_request)"/>
+    <remap from="~/input/odometry" to="$(var input_odometry)"/>
+    <remap from="~/output/velocity_limit" to="$(var output_max_velocity)"/>
+    <remap from="~/output/velocity_limit_clear_command" to="$(var output_max_velocity_clear_command)"/>
+    <remap from="~/output/mrm_state" to="$(var output_mrm_state)"/>
   </node>
 </launch>

system/mrm_stop_operator/package.xml

@@ -16,6 +16,8 @@
   <depend>rclcpp_components</depend>
   <depend>tier4_planning_msgs</depend>
   <depend>tier4_system_msgs</depend>
+  <depend>nav_msgs</depend>
+
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>

system/mrm_stop_operator/src/mrm_stop_operator.cpp

@@ -30,14 +30,29 @@ MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options)
     "~/input/mrm_request", 10,
     std::bind(&MrmStopOperator::onMrmRequest, this, std::placeholders::_1));
 
+  sub_odm_group_ =
+    create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  rclcpp::SubscriptionOptions odm_options = rclcpp::SubscriptionOptions();
+  odm_options.callback_group = sub_odm_group_;
+  auto not_executed_callback = []([[maybe_unused]] const typename nav_msgs::msg::Odometry::ConstSharedPtr msg) {};
+  sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
+    "~/input/odometry", 10, not_executed_callback, odm_options);
+
   // Publisher
   pub_velocity_limit_ = create_publisher<tier4_planning_msgs::msg::VelocityLimit>(
     "~/output/velocity_limit", rclcpp::QoS{10}.transient_local());
   pub_velocity_limit_clear_command_ =
     create_publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>(
       "~/output/velocity_limit_clear_command", rclcpp::QoS{10}.transient_local());
+  pub_mrm_state_ =
+    create_publisher<tier4_system_msgs::msg::MrmState>(
+      "~/output/mrm_state", rclcpp::QoS{1}.transient_local());
+  
 
   // Timer
+  const auto update_period_ns = rclcpp::Rate(10).period();
+  timer_ = rclcpp::create_timer(
+    this, get_clock(), update_period_ns, std::bind(&MrmStopOperator::onTimer, this));
 
   // Service
 
@@ -67,12 +82,45 @@ void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::Co
     pub_velocity_limit_->publish(velocity_limit);
 
     last_mrm_request_ = *msg;
+    current_mrm_state_.behavior = *msg;
+    current_mrm_state_.state = tier4_system_msgs::msg::MrmState::MRM_OPERATING;
   }
 }
 
 void MrmStopOperator::initState()
 {
   last_mrm_request_.type = tier4_system_msgs::msg::MrmBehavior::NONE;
+  current_mrm_state_.state = tier4_system_msgs::msg::MrmState::NORMAL;
+  current_mrm_state_.behavior.type = tier4_system_msgs::msg::MrmBehavior::NONE;
+}
+
+void MrmStopOperator::onTimer()
+{ 
+  if (current_mrm_state_.state == tier4_system_msgs::msg::MrmState::MRM_OPERATING) {
+    if (current_mrm_state_.behavior.type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) {
+      if (isStopped()) {
+        current_mrm_state_.state = tier4_system_msgs::msg::MrmState::MRM_SUCCEEDED;
+      } else {
+        // nothing to do
+      }
+    } else {
+      //TODO
+    }
+  }
+}
+
+bool MrmStopOperator::isStopped()
+{
+  constexpr auto th_stopped_velocity = 0.001;
+  auto current_odom  = std::make_shared<nav_msgs::msg::Odometry>();
+  rclcpp::MessageInfo message_info;
+
+  const bool success = sub_odom_->take(*current_odom, message_info);
+  if (success) {
+    return current_odom->twist.twist.linear.x < th_stopped_velocity;
+  } else {
+    return false;
+  }
 }
 
 }  // namespace mrm_stop_operator

system/mrm_stop_operator/src/mrm_stop_operator.hpp

@@ -21,6 +21,8 @@
 #include <tier4_planning_msgs/msg/velocity_limit.hpp>
 #include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
 #include <tier4_system_msgs/msg/mrm_behavior.hpp>
+#include <tier4_system_msgs/msg/mrm_state.hpp>
+#include <nav_msgs/msg/odometry.hpp>
 
 namespace mrm_stop_operator
 {
@@ -44,6 +46,7 @@ class MrmStopOperator : public rclcpp::Node
 
   // Subscriber
   rclcpp::Subscription<tier4_system_msgs::msg::MrmBehavior>::SharedPtr sub_mrm_request_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
 
   void onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg);
 
@@ -53,17 +56,23 @@ class MrmStopOperator : public rclcpp::Node
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimit>::SharedPtr pub_velocity_limit_;
   rclcpp::Publisher<tier4_planning_msgs::msg::VelocityLimitClearCommand>::SharedPtr
     pub_velocity_limit_clear_command_;
-
+  rclcpp::Publisher<tier4_system_msgs::msg::MrmState>::SharedPtr pub_mrm_state_;
   // Service
 
   // Client
 
   // Timer
+  rclcpp::TimerBase::SharedPtr timer_;
+
+  rclcpp::CallbackGroup::SharedPtr sub_odm_group_;
 
   // State
   tier4_system_msgs::msg::MrmBehavior last_mrm_request_;
+  tier4_system_msgs::msg::MrmState current_mrm_state_;
 
   void initState();
+  void onTimer();
+  bool isStopped();
 
   // Diagnostics
 };