diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 012bb55af3068..cd66cde64c9a5 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -20,6 +20,8 @@ #include // Autoware +#include + #include #include #include @@ -56,33 +58,26 @@ class EmergencyHandler : public rclcpp::Node explicit EmergencyHandler(const rclcpp::NodeOptions & options); private: - // Subscribers + // Subscribers with callback rclcpp::Subscription::SharedPtr sub_hazard_status_stamped_; rclcpp::Subscription::SharedPtr sub_prev_control_command_; - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_control_mode_; - rclcpp::Subscription::SharedPtr - sub_mrm_comfortable_stop_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_emergency_stop_status_; + // Subscribers without callback + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_mode_{this, "~/input/control_mode"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr hazard_status_stamped_; autoware_control_msgs::msg::Control::ConstSharedPtr prev_control_command_; - nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; void onHazardStatusStamped( const autoware_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg); void onPrevControlCommand(const autoware_control_msgs::msg::Control::ConstSharedPtr msg); - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); - void onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); // Publisher rclcpp::Publisher::SharedPtr pub_control_command_; @@ -135,9 +130,13 @@ class EmergencyHandler : public rclcpp::Node void updateMrmState(); void operateMrm(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); - bool isStopped(); + + bool isAutonomous(); bool isDrivingBackwards(); bool isEmergency(); + bool isStopped(); + bool isComfortableStopStatusAvailable(); + bool isEmergencyStopStatusAvailable(); }; #endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_ diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index aa2090c86edb8..f26080fd8ef56 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -21,6 +21,7 @@ rclcpp_components std_msgs std_srvs + tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index ae3d60e32a445..b2ee12afc9c76 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -30,24 +30,13 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) using std::placeholders::_1; - // Subscriber + // Subscribers with callback sub_hazard_status_stamped_ = create_subscription( "~/input/hazard_status", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1)); sub_prev_control_command_ = create_subscription( "~/input/prev_control_command", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onPrevControlCommand, this, _1)); - sub_odom_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1)); - // subscribe control mode - sub_control_mode_ = create_subscription( - "~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1)); - sub_mrm_comfortable_stop_status_ = create_subscription( - "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onMrmComfortableStopStatus, this, _1)); - sub_mrm_emergency_stop_status_ = create_subscription( - "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, - std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1)); // Publisher pub_control_command_ = create_publisher( @@ -72,13 +61,6 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) client_mrm_emergency_stop_group_); // Initialize - odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - prev_control_command_ = - autoware_control_msgs::msg::Control::ConstSharedPtr(new autoware_control_msgs::msg::Control); - mrm_comfortable_stop_status_ = - std::make_shared(); - mrm_emergency_stop_status_ = std::make_shared(); mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; @@ -105,29 +87,6 @@ void EmergencyHandler::onPrevControlCommand( prev_control_command_ = autoware_control_msgs::msg::Control::ConstSharedPtr(control_command); } -void EmergencyHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - odom_ = msg; -} - -void EmergencyHandler::onControlMode( - const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - control_mode_ = msg; -} - -void EmergencyHandler::onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_comfortable_stop_status_ = msg; -} - -void EmergencyHandler::onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_emergency_stop_status_ = msg; -} - autoware_vehicle_msgs::msg::HazardLightsCommand EmergencyHandler::createHazardCmdMsg() { using autoware_vehicle_msgs::msg::HazardLightsCommand; @@ -293,17 +252,14 @@ bool EmergencyHandler::isDataReady() return false; } - if ( - param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == - tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm comfortable stop to become available..."); return false; } - if ( - mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (!isEmergencyStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm emergency stop to become available..."); @@ -381,7 +337,7 @@ void EmergencyHandler::updateMrmState() const bool is_emergency = isEmergency(); // Get mode - const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; + const bool is_auto_mode = isAutonomous(); // State Machine if (mrm_state_.state == MrmState::NORMAL) { @@ -447,6 +403,14 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type EmergencyHandler::getCurre return mrm_state_.behavior; } +bool EmergencyHandler::isAutonomous() +{ + using autoware_vehicle_msgs::msg::ControlModeReport; + auto mode = sub_control_mode_.takeData(); + if (mode == nullptr) return false; + return mode->mode == ControlModeReport::AUTONOMOUS; +} + bool EmergencyHandler::isEmergency() { return hazard_status_stamped_->status.emergency || @@ -455,14 +419,32 @@ bool EmergencyHandler::isEmergency() bool EmergencyHandler::isStopped() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_stopped_velocity = 0.001; - return (std::abs(odom_->twist.twist.linear.x) < th_stopped_velocity); + return (std::abs(odom->twist.twist.linear.x) < th_stopped_velocity); +} + +bool EmergencyHandler::isComfortableStopStatusAvailable() +{ + auto status = sub_mrm_comfortable_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool EmergencyHandler::isEmergencyStopStatusAvailable() +{ + auto status = sub_mrm_emergency_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; } bool EmergencyHandler::isDrivingBackwards() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_moving_backwards = -0.001; - return odom_->twist.twist.linear.x < th_moving_backwards; + return odom->twist.twist.linear.x < th_moving_backwards; } #include diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 7a160b6c531e9..33d8af5004c47 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -22,6 +22,8 @@ #include // Autoware +#include + #include #include #include @@ -66,39 +68,28 @@ class MrmHandler : public rclcpp::Node // type enum RequestType { CALL, CANCEL }; - // Subscribers - rclcpp::Subscription::SharedPtr sub_odom_; - rclcpp::Subscription::SharedPtr sub_control_mode_; + // Subscribers with callback rclcpp::Subscription::SharedPtr sub_operation_mode_availability_; - rclcpp::Subscription::SharedPtr - sub_mrm_pull_over_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_comfortable_stop_status_; - rclcpp::Subscription::SharedPtr - sub_mrm_emergency_stop_status_; - rclcpp::Subscription::SharedPtr - sub_operation_mode_state_; - - nav_msgs::msg::Odometry::ConstSharedPtr odom_; - autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr control_mode_; + // Subscribers without callback + tier4_autoware_utils::InterProcessPollingSubscriber sub_odom_{ + this, "~/input/odometry"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_control_mode_{this, "~/input/control_mode"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_pull_over_status_{this, "~/input/mrm/pull_over/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber + sub_mrm_emergency_stop_status_{this, "~/input/mrm/emergency_stop/status"}; + tier4_autoware_utils::InterProcessPollingSubscriber< + autoware_adapi_v1_msgs::msg::OperationModeState> + sub_operation_mode_state_{this, "~/input/api/operation_mode/state"}; + tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr operation_mode_availability_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_pull_over_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_comfortable_stop_status_; - tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr mrm_emergency_stop_status_; - autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr operation_mode_state_; - void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); - void onControlMode(const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg); void onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg); - void onMrmPullOverStatus(const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg); - void onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg); // Publisher @@ -156,6 +147,10 @@ class MrmHandler : public rclcpp::Node bool isStopped(); bool isDrivingBackwards(); bool isEmergency() const; + bool isAutonomous(); + bool isPullOverStatusAvailable(); + bool isComfortableStopStatusAvailable(); + bool isEmergencyStopStatusAvailable(); bool isArrivedAtGoal(); }; diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index 2db22cecaa82d..5774cce3215a9 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -21,6 +21,7 @@ rclcpp_components std_msgs std_srvs + tier4_autoware_utils tier4_system_msgs ament_lint_auto diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index d70303a9801ac..44407c40c6787 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -36,28 +36,11 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" using std::placeholders::_1; - // Subscriber - sub_odom_ = create_subscription( - "~/input/odometry", rclcpp::QoS{1}, std::bind(&MrmHandler::onOdometry, this, _1)); - // subscribe control mode - sub_control_mode_ = create_subscription( - "~/input/control_mode", rclcpp::QoS{1}, std::bind(&MrmHandler::onControlMode, this, _1)); + // Subscribers with callback sub_operation_mode_availability_ = create_subscription( "~/input/operation_mode_availability", rclcpp::QoS{1}, std::bind(&MrmHandler::onOperationModeAvailability, this, _1)); - sub_mrm_pull_over_status_ = create_subscription( - "~/input/mrm/pull_over/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmPullOverStatus, this, _1)); - sub_mrm_comfortable_stop_status_ = create_subscription( - "~/input/mrm/comfortable_stop/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmComfortableStopStatus, this, _1)); - sub_mrm_emergency_stop_status_ = create_subscription( - "~/input/mrm/emergency_stop/status", rclcpp::QoS{1}, - std::bind(&MrmHandler::onMrmEmergencyStopStatus, this, _1)); - sub_operation_mode_state_ = create_subscription( - "~/input/api/operation_mode/state", rclcpp::QoS{1}, - std::bind(&MrmHandler::onOperationModeState, this, _1)); // Publisher pub_hazard_cmd_ = create_publisher( @@ -84,13 +67,6 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" client_mrm_emergency_stop_group_); // Initialize - odom_ = std::make_shared(); - control_mode_ = std::make_shared(); - mrm_pull_over_status_ = std::make_shared(); - mrm_comfortable_stop_status_ = - std::make_shared(); - mrm_emergency_stop_status_ = std::make_shared(); - operation_mode_state_ = std::make_shared(); mrm_state_.stamp = this->now(); mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; @@ -102,17 +78,6 @@ MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler" this, get_clock(), update_period_ns, std::bind(&MrmHandler::onTimer, this)); } -void MrmHandler::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) -{ - odom_ = msg; -} - -void MrmHandler::onControlMode( - const autoware_vehicle_msgs::msg::ControlModeReport::ConstSharedPtr msg) -{ - control_mode_ = msg; -} - void MrmHandler::onOperationModeAvailability( const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr msg) { @@ -141,30 +106,6 @@ void MrmHandler::onOperationModeAvailability( operation_mode_availability_ = msg; } -void MrmHandler::onMrmPullOverStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_pull_over_status_ = msg; -} - -void MrmHandler::onMrmComfortableStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_comfortable_stop_status_ = msg; -} - -void MrmHandler::onMrmEmergencyStopStatus( - const tier4_system_msgs::msg::MrmBehaviorStatus::ConstSharedPtr msg) -{ - mrm_emergency_stop_status_ = msg; -} - -void MrmHandler::onOperationModeState( - const autoware_adapi_v1_msgs::msg::OperationModeState::ConstSharedPtr msg) -{ - operation_mode_state_ = msg; -} - void MrmHandler::publishHazardCmd() { using autoware_vehicle_msgs::msg::HazardLightsCommand; @@ -352,26 +293,21 @@ bool MrmHandler::isDataReady() return false; } - if ( - param_.use_pull_over && - mrm_pull_over_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_pull_over && !isPullOverStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm pull over to become available..."); return false; } - if ( - param_.use_comfortable_stop && mrm_comfortable_stop_status_->state == - tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (param_.use_comfortable_stop && !isComfortableStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm comfortable stop to become available..."); return false; } - if ( - mrm_emergency_stop_status_->state == tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE) { + if (!isEmergencyStopStatusAvailable()) { RCLCPP_INFO_THROTTLE( this->get_logger(), *this->get_clock(), std::chrono::milliseconds(5000).count(), "waiting for mrm emergency stop to become available..."); @@ -460,7 +396,7 @@ void MrmHandler::updateMrmState() } // Get mode - const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; + const bool is_auto_mode = isAutonomous(); // State Machine switch (mrm_state_.state) { @@ -581,14 +517,18 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB bool MrmHandler::isStopped() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_stopped_velocity = 0.001; - return std::abs((odom_->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity); + return (std::abs(odom->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity); } bool MrmHandler::isDrivingBackwards() { + auto odom = sub_odom_.takeData(); + if (odom == nullptr) return false; constexpr auto th_moving_backwards = -0.001; - return odom_->twist.twist.linear.x < th_moving_backwards; + return odom->twist.twist.linear.x < th_moving_backwards; } bool MrmHandler::isEmergency() const @@ -597,11 +537,41 @@ bool MrmHandler::isEmergency() const is_operation_mode_availability_timeout; } +bool MrmHandler::isAutonomous() +{ + using autoware_vehicle_msgs::msg::ControlModeReport; + auto mode = sub_control_mode_.takeData(); + if (mode == nullptr) return false; + return mode->mode == ControlModeReport::AUTONOMOUS; +} + +bool MrmHandler::isPullOverStatusAvailable() +{ + auto status = sub_mrm_pull_over_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool MrmHandler::isComfortableStopStatusAvailable() +{ + auto status = sub_mrm_comfortable_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + +bool MrmHandler::isEmergencyStopStatusAvailable() +{ + auto status = sub_mrm_emergency_stop_status_.takeData(); + if (status == nullptr) return false; + return status->state != tier4_system_msgs::msg::MrmBehaviorStatus::NOT_AVAILABLE; +} + bool MrmHandler::isArrivedAtGoal() { using autoware_adapi_v1_msgs::msg::OperationModeState; - - return operation_mode_state_->mode == OperationModeState::STOP; + auto state = sub_operation_mode_state_.takeData(); + if (state == nullptr) return false; + return state->mode == OperationModeState::STOP; } #include