Merge branch 'feat/add-mrm-v0.6-launch-based-on-v3.0.0' into feat/upd…

…ate-mrm-handler-for-mrm-v0.6

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,6 @@
+/**:
+  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,11 @@
+<launch>
+  <arg name="mrm_stop_operator_param_path" default="$(find-pkg-share mrm_stop_operator)/config/mrm_stop_operator.param.yaml"/>
+
+  <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"/>
+  </node>
+</launch>

system/mrm_stop_operator/package.xml

@@ -0,0 +1,26 @@
+<?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>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,80 @@
+// 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));
+
+  // 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());
+
+  // Timer
+
+  // 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;
+  }
+}
+
+void MrmStopOperator::initState()
+{
+  last_mrm_request_.type = tier4_system_msgs::msg::MrmBehavior::NONE;
+}
+
+}  // 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,72 @@
+// 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__MRM_STOP_OPERATOR_HPP_
+#define MRM_STOP_OPERATOR__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>
+
+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_;
+
+  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_;
+
+  // Service
+
+  // Client
+
+  // Timer
+
+  // State
+  tier4_system_msgs::msg::MrmBehavior last_mrm_request_;
+
+  void initState();
+
+  // Diagnostics
+
+};
+}  // namespace mrm_stop_operator
+
+#endif  // MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_