feat: add publisher mrm state

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

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_velocity" default="/vehicle/status/velocity_status"/>
+  <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/velocity" to="$(var input_velocity)"/>
+    <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>autoware_auto_vehicle_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,13 +30,29 @@ MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options)
     "~/input/mrm_request", 10,
     std::bind(&MrmStopOperator::onMrmRequest, this, std::placeholders::_1));
 
+  sub_velocity_group_ =
+    create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false);
+  rclcpp::SubscriptionOptions velocity_options = rclcpp::SubscriptionOptions();
+  velocity_options.callback_group = sub_velocity_group_;
+  auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr msg) {};
+  sub_velocity_ = create_subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>(
+    "~/input/velocity", 10, not_executed_callback, velocity_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_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
 
@@ -48,13 +64,13 @@ MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options)
   initState();
 
   // Diagnostics
-
 }
 
 void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg)
 {
-  if (msg->type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP &&
-      last_mrm_request_.type != tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) {
+  if (
+    msg->type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP &&
+    last_mrm_request_.type != tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) {
     tier4_planning_msgs::msg::VelocityLimit velocity_limit;
     velocity_limit.stamp = this->now();
     velocity_limit.max_velocity = 0.0;
@@ -64,14 +80,49 @@ void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::Co
     velocity_limit.constraints.min_jerk = params_.min_jerk;
     velocity_limit.sender = "mrm_stop_operator";
     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
+    }
+  }
+  current_mrm_state_.stamp = this->now();
+  pub_mrm_state_->publish(current_mrm_state_);
+}
+
+bool MrmStopOperator::isStopped()
+{
+  constexpr auto th_stopped_velocity = 0.001;
+  auto current_velocity  = std::make_shared<autoware_auto_vehicle_msgs::msg::VelocityReport>();
+  rclcpp::MessageInfo message_info;
+
+  const bool success = sub_velocity_->take(*current_velocity, message_info);
+  if (success) {
+    return current_velocity->longitudinal_velocity < th_stopped_velocity;
+  } else {
+    return false;
+  }
 }
 
 }  // namespace mrm_stop_operator

system/mrm_stop_operator/src/mrm_stop_operator.hpp

@@ -12,15 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_
-#define MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_
+#ifndef MRM_STOP_OPERATOR_HPP_
+#define MRM_STOP_OPERATOR_HPP_
 
 // include
 #include <rclcpp/rclcpp.hpp>
 
 #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 <autoware_auto_vehicle_msgs/msg/velocity_report.hpp>
 
 namespace mrm_stop_operator
 {
@@ -44,29 +46,36 @@ class MrmStopOperator : public rclcpp::Node
 
   // Subscriber
   rclcpp::Subscription<tier4_system_msgs::msg::MrmBehavior>::SharedPtr sub_mrm_request_;
+  rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::VelocityReport>::SharedPtr sub_velocity_;
 
   void onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg);
 
   // Service
 
   // Publisher
   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_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_velocity_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
-
 };
 }  // namespace mrm_stop_operator
 
-#endif  // MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_
+#endif  // MRM_STOP_OPERATOR_HPP_