From 8a2af617b5e52d701890121e33cb677da536bd49 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 4 Jul 2024 11:44:57 +0900 Subject: [PATCH] feat: add publisher mrm state Signed-off-by: TetsuKawa --- .../launch/mrm_stop_operator.launch.xml | 15 +++-- system/mrm_stop_operator/package.xml | 2 + .../src/mrm_stop_operator.cpp | 63 +++++++++++++++++-- .../src/mrm_stop_operator.hpp | 23 ++++--- 4 files changed, 86 insertions(+), 17 deletions(-) diff --git a/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml index 9d5226cf99f19..52538f184c313 100644 --- a/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml +++ b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml @@ -1,11 +1,18 @@ + + + + + - - - - + + + + + + diff --git a/system/mrm_stop_operator/package.xml b/system/mrm_stop_operator/package.xml index f3822268496b0..2c29f111d969e 100644 --- a/system/mrm_stop_operator/package.xml +++ b/system/mrm_stop_operator/package.xml @@ -16,6 +16,8 @@ rclcpp_components tier4_planning_msgs tier4_system_msgs + autoware_auto_vehicle_msgs + ament_lint_auto autoware_lint_common diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.cpp b/system/mrm_stop_operator/src/mrm_stop_operator.cpp index d403e886ed7a1..e858bc7b4a1dc 100644 --- a/system/mrm_stop_operator/src/mrm_stop_operator.cpp +++ b/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( + "~/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_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}.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(); + 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 diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.hpp b/system/mrm_stop_operator/src/mrm_stop_operator.hpp index 750c343f13062..80ac42230046f 100644 --- a/system/mrm_stop_operator/src/mrm_stop_operator.hpp +++ b/system/mrm_stop_operator/src/mrm_stop_operator.hpp @@ -12,8 +12,8 @@ // 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 @@ -21,6 +21,8 @@ #include #include #include +#include +#include namespace mrm_stop_operator { @@ -44,6 +46,7 @@ class MrmStopOperator : public rclcpp::Node // Subscriber rclcpp::Subscription::SharedPtr sub_mrm_request_; + rclcpp::Subscription::SharedPtr sub_velocity_; void onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg); @@ -51,22 +54,28 @@ class MrmStopOperator : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_velocity_limit_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_clear_command_; - + 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_; - + 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_ \ No newline at end of file +#endif // MRM_STOP_OPERATOR_HPP_