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..7043b61596f76 --- /dev/null +++ b/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] 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..52538f184c313 --- /dev/null +++ b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/system/mrm_stop_operator/package.xml b/system/mrm_stop_operator/package.xml new file mode 100644 index 0000000000000..b20b12ee136c4 --- /dev/null +++ b/system/mrm_stop_operator/package.xml @@ -0,0 +1,28 @@ + + + + mrm_stop_operator + 0.1.0 + The mrm_stop_operator package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + autoware_adapi_v1_msgs + autoware_vehicle_msgs + 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..abae7a67a5a3d --- /dev/null +++ b/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("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)); + + 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( + "~/input/velocity", 10, not_executed_callback, velocity_options); + + // 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()); + pub_mrm_state_ = + create_publisher("~/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(); + 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(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..d09c9ee00dc47 --- /dev/null +++ b/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 + +#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_; + rclcpp::Subscription::SharedPtr sub_velocity_; + + 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_; + rclcpp::Publisher::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_