feat(mrm_stop_operator): add mrm stop operator (#1682)

* feat: add mrm stop operator

Signed-off-by: TetsuKawa <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: TetsuKawa <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

system/mrm_stop_operator/CMakeLists.txt

@@ -0,0 +1,21 @@
+cmake_minimum_required(VERSION 3.14)
+project(mrm_stop_operator)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(mrm_stop_operator SHARED
+  src/mrm_stop_operator.cpp
+)
+ament_target_dependencies(mrm_stop_operator)
+
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "mrm_stop_operator::MrmStopOperator"
+  EXECUTABLE ${PROJECT_NAME}_node
+)
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+  config
+)

system/mrm_stop_operator/README.md

@@ -0,0 +1,23 @@
+# Mrm Stop Operator
+
+## Purpose
+
+## Inner-workings / Algorithms
+
+## Inputs / Outputs
+
+### Input
+
+### Output
+
+## Parameters
+
+## Assumptions / Known limits
+
+## (Optional) Error detection and handling
+
+## (Optional) Performance characterization
+
+## (Optional) References/External links
+
+## (Optional) Future extensions / Unimplemented parts

system/mrm_stop_operator/config/mrm_stop_operator.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    min_acceleration: -4.0  # min acceleration for sub ecu mrm stop [m/s^2]
+    max_jerk: 5.0           # max jerk for sub ecu mrm stop [m/s^3]
+    min_jerk: -5.0          # min jerk for sub ecu mrm stop [m/s^3]

system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml

@@ -0,0 +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="$(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

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mrm_stop_operator</name>
+  <version>0.1.0</version>
+  <description>The mrm_stop_operator package</description>
+  <maintainer email="[email protected]">Makoto Kurihara</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <!-- depend -->
+  <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_vehicle_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>tier4_planning_msgs</depend>
+  <depend>tier4_system_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

system/mrm_stop_operator/src/mrm_stop_operator.cpp

@@ -0,0 +1,130 @@
+// Copyright 2024 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 "mrm_stop_operator.hpp"
+
+namespace mrm_stop_operator
+{
+
+MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options)
+: Node("mrm_stop_operator", node_options)
+{
+  // Parameter
+  params_.min_acceleration = declare_parameter<double>("min_acceleration", -4.0);
+  params_.max_jerk = declare_parameter<double>("max_jerk", 5.0);
+  params_.min_jerk = declare_parameter<double>("min_jerk", -5.0);
+
+  // Subscriber
+  sub_mrm_request_ = create_subscription<tier4_system_msgs::msg::MrmBehavior>(
+    "~/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_vehicle_msgs::msg::VelocityReport::ConstSharedPtr
+         msg) {};
+  sub_velocity_ = create_subscription<autoware_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_mrm_state_ =
+    create_publisher<autoware_adapi_v1_msgs::msg::MrmState>("~/output/mrm_state", rclcpp::QoS{1});
+
+  // 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
+
+  // Client
+
+  // Timer
+
+  // State
+  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) {
+    tier4_planning_msgs::msg::VelocityLimit velocity_limit;
+    velocity_limit.stamp = this->now();
+    velocity_limit.max_velocity = 0.0;
+    velocity_limit.use_constraints = true;
+    velocity_limit.constraints.min_acceleration = params_.min_acceleration;
+    velocity_limit.constraints.max_jerk = params_.max_jerk;
+    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->type;
+    current_mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING;
+  }
+}
+
+void MrmStopOperator::initState()
+{
+  last_mrm_request_.type = tier4_system_msgs::msg::MrmBehavior::NONE;
+  current_mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL;
+  current_mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE;
+}
+
+void MrmStopOperator::onTimer()
+{
+  if (current_mrm_state_.state == autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING) {
+    if (current_mrm_state_.behavior == autoware_adapi_v1_msgs::msg::MrmState::COMFORTABLE_STOP) {
+      if (isStopped()) {
+        current_mrm_state_.state = autoware_adapi_v1_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_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
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(mrm_stop_operator::MrmStopOperator)

system/mrm_stop_operator/src/mrm_stop_operator.hpp

@@ -0,0 +1,81 @@
+// Copyright 2024 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 MRM_STOP_OPERATOR_HPP_
+#define MRM_STOP_OPERATOR_HPP_
+
+// include
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
+#include <autoware_vehicle_msgs/msg/velocity_report.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>
+
+namespace mrm_stop_operator
+{
+
+struct Parameters
+{
+  double min_acceleration;
+  double max_jerk;
+  double min_jerk;
+};
+
+class MrmStopOperator : public rclcpp::Node
+{
+public:
+  explicit MrmStopOperator(const rclcpp::NodeOptions & node_options);
+  ~MrmStopOperator() = default;
+
+private:
+  // Parameter
+  Parameters params_;
+
+  // Subscriber
+  rclcpp::Subscription<tier4_system_msgs::msg::MrmBehavior>::SharedPtr sub_mrm_request_;
+  rclcpp::Subscription<autoware_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<autoware_adapi_v1_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_;
+  autoware_adapi_v1_msgs::msg::MrmState current_mrm_state_;
+
+  void initState();
+  void onTimer();
+  bool isStopped();
+
+  // Diagnostics
+};
+}  // namespace mrm_stop_operator
+
+#endif  // MRM_STOP_OPERATOR_HPP_