diff --git a/system/mrm_stop_operator/CMakeLists.txt b/system/mrm_stop_operator/CMakeLists.txt new file mode 100644 index 0000000000000..f66488c659806 --- /dev/null +++ b/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 +) diff --git a/system/mrm_stop_operator/README.md b/system/mrm_stop_operator/README.md new file mode 100644 index 0000000000000..4407161d510bd --- /dev/null +++ b/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 diff --git a/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml new file mode 100644 index 0000000000000..5dfbb2956d668 --- /dev/null +++ b/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] + diff --git a/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml new file mode 100644 index 0000000000000..9d5226cf99f19 --- /dev/null +++ b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/system/mrm_stop_operator/package.xml b/system/mrm_stop_operator/package.xml new file mode 100644 index 0000000000000..f3822268496b0 --- /dev/null +++ b/system/mrm_stop_operator/package.xml @@ -0,0 +1,26 @@ + + + + mrm_stop_operator + 0.1.0 + The mrm_stop_operator package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + rclcpp + rclcpp_components + tier4_planning_msgs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.cpp b/system/mrm_stop_operator/src/mrm_stop_operator.cpp new file mode 100644 index 0000000000000..d403e886ed7a1 --- /dev/null +++ b/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("min_acceleration", -4.0); + params_.max_jerk = declare_parameter("max_jerk", 5.0); + params_.min_jerk = declare_parameter("min_jerk", -5.0); + + // Subscriber + sub_mrm_request_ = create_subscription( + "~/input/mrm_request", 10, + std::bind(&MrmStopOperator::onMrmRequest, this, std::placeholders::_1)); + + // Publisher + pub_velocity_limit_ = create_publisher( + "~/output/velocity_limit", rclcpp::QoS{10}.transient_local()); + pub_velocity_limit_clear_command_ = create_publisher( + "~/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(mrm_stop_operator::MrmStopOperator) diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.hpp b/system/mrm_stop_operator/src/mrm_stop_operator.hpp new file mode 100644 index 0000000000000..750c343f13062 --- /dev/null +++ b/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 + +#include +#include +#include + +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::SharedPtr sub_mrm_request_; + + void onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg); + + // Service + + // Publisher + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::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_ \ No newline at end of file