Skip to content

Commit

Permalink
feat(emergency_handler, mrm_handler): change to read topic by polling (…
Browse files Browse the repository at this point in the history 
…#7297)

* replace Subscription to InterProcessPollingSubscriber

Signed-off-by: Autumn60 <[email protected]>

* sort depend packages list in package.xml

Signed-off-by: Autumn60 <[email protected]>

* fix end of file

Signed-off-by: Autumn60 <[email protected]>

* clang format

Signed-off-by: Autumn60 <[email protected]>

* chore: fix comments

Signed-off-by: Autumn60 <[email protected]>

* replace Subscription to InterProcessPollingSubscriber (mrm_handler)

Signed-off-by: Autumn60 <[email protected]>

---------

Signed-off-by: Autumn60 <[email protected]>
Co-authored-by: Autumn60 <[email protected]>
  • Loading branch information
@Autumn60 @KhalilSelyan
2 people authored and KhalilSelyan committed Jul 22, 2024
1 parent 2285394 commit 0479160
Show file tree
Hide file tree
Showing 6 changed files with 116 additions and 168 deletions.
35 changes: 17 additions & 18 deletions system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@
#include <string>

// Autoware
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>

#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
#include <autoware_control_msgs/msg/control.hpp>
#include <autoware_system_msgs/msg/hazard_status_stamped.hpp>
Expand Down Expand Up @@ -56,33 +58,26 @@ class EmergencyHandler : public rclcpp::Node
explicit EmergencyHandler(const rclcpp::NodeOptions & options);

private:
// Subscribers
// Subscribers with callback
rclcpp::Subscription<autoware_system_msgs::msg::HazardStatusStamped>::SharedPtr
sub_hazard_status_stamped_;
rclcpp::Subscription<autoware_control_msgs::msg::Control>::SharedPtr sub_prev_control_command_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
rclcpp::Subscription<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr sub_control_mode_;
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
sub_mrm_comfortable_stop_status_;
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
sub_mrm_emergency_stop_status_;
// Subscribers without callback
tier4_autoware_utils::InterProcessPollingSubscriber<nav_msgs::msg::Odometry> sub_odom_{
this, "~/input/odometry"};
tier4_autoware_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::ControlModeReport>
sub_control_mode_{this, "~/input/control_mode"};
tier4_autoware_utils::InterProcessPollingSubscriber<tier4_system_msgs::msg::MrmBehaviorStatus>
sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"};
tier4_autoware_utils::InterProcessPollingSubscriber<tier4_system_msgs::msg::MrmBehaviorStatus>
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<autoware_control_msgs::msg::Control>::SharedPtr pub_control_command_;
Expand Down Expand Up @@ -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_
1 change: 1 addition & 0 deletions system/emergency_handler/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
<depend>rclcpp_components</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tier4_autoware_utils</depend>
<depend>tier4_system_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
Expand Down
82 changes: 32 additions & 50 deletions system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,24 +30,13 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options)

using std::placeholders::_1;

// Subscriber
// Subscribers with callback
sub_hazard_status_stamped_ = create_subscription<autoware_system_msgs::msg::HazardStatusStamped>(
"~/input/hazard_status", rclcpp::QoS{1},
std::bind(&EmergencyHandler::onHazardStatusStamped, this, _1));
sub_prev_control_command_ = create_subscription<autoware_control_msgs::msg::Control>(
"~/input/prev_control_command", rclcpp::QoS{1},
std::bind(&EmergencyHandler::onPrevControlCommand, this, _1));
sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
"~/input/odometry", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onOdometry, this, _1));
// subscribe control mode
sub_control_mode_ = create_subscription<autoware_vehicle_msgs::msg::ControlModeReport>(
"~/input/control_mode", rclcpp::QoS{1}, std::bind(&EmergencyHandler::onControlMode, this, _1));
sub_mrm_comfortable_stop_status_ = create_subscription<tier4_system_msgs::msg::MrmBehaviorStatus>(
"~/input/mrm/comfortable_stop/status", rclcpp::QoS{1},
std::bind(&EmergencyHandler::onMrmComfortableStopStatus, this, _1));
sub_mrm_emergency_stop_status_ = create_subscription<tier4_system_msgs::msg::MrmBehaviorStatus>(
"~/input/mrm/emergency_stop/status", rclcpp::QoS{1},
std::bind(&EmergencyHandler::onMrmEmergencyStopStatus, this, _1));

// Publisher
pub_control_command_ = create_publisher<autoware_control_msgs::msg::Control>(
Expand All @@ -72,13 +61,6 @@ EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options)
client_mrm_emergency_stop_group_);

// Initialize
odom_ = std::make_shared<const nav_msgs::msg::Odometry>();
control_mode_ = std::make_shared<const autoware_vehicle_msgs::msg::ControlModeReport>();
prev_control_command_ =
autoware_control_msgs::msg::Control::ConstSharedPtr(new autoware_control_msgs::msg::Control);
mrm_comfortable_stop_status_ =
std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>();
mrm_emergency_stop_status_ = std::make_shared<const tier4_system_msgs::msg::MrmBehaviorStatus>();
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;
Expand All @@ -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;
Expand Down Expand Up @@ -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...");
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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 ||
Expand All @@ -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 <rclcpp_components/register_node_macro.hpp>
Expand Down
49 changes: 22 additions & 27 deletions system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
#include <variant>

// Autoware
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>

#include <autoware_adapi_v1_msgs/msg/mrm_state.hpp>
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
#include <autoware_vehicle_msgs/msg/control_mode_report.hpp>
Expand Down Expand Up @@ -66,39 +68,28 @@ class MrmHandler : public rclcpp::Node
// type
enum RequestType { CALL, CANCEL };

// Subscribers
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
rclcpp::Subscription<autoware_vehicle_msgs::msg::ControlModeReport>::SharedPtr sub_control_mode_;
// Subscribers with callback
rclcpp::Subscription<tier4_system_msgs::msg::OperationModeAvailability>::SharedPtr
sub_operation_mode_availability_;
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
sub_mrm_pull_over_status_;
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
sub_mrm_comfortable_stop_status_;
rclcpp::Subscription<tier4_system_msgs::msg::MrmBehaviorStatus>::SharedPtr
sub_mrm_emergency_stop_status_;
rclcpp::Subscription<autoware_adapi_v1_msgs::msg::OperationModeState>::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<nav_msgs::msg::Odometry> sub_odom_{
this, "~/input/odometry"};
tier4_autoware_utils::InterProcessPollingSubscriber<autoware_vehicle_msgs::msg::ControlModeReport>
sub_control_mode_{this, "~/input/control_mode"};
tier4_autoware_utils::InterProcessPollingSubscriber<tier4_system_msgs::msg::MrmBehaviorStatus>
sub_mrm_pull_over_status_{this, "~/input/mrm/pull_over/status"};
tier4_autoware_utils::InterProcessPollingSubscriber<tier4_system_msgs::msg::MrmBehaviorStatus>
sub_mrm_comfortable_stop_status_{this, "~/input/mrm/comfortable_stop/status"};
tier4_autoware_utils::InterProcessPollingSubscriber<tier4_system_msgs::msg::MrmBehaviorStatus>
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

Expand Down Expand Up @@ -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();
};

Expand Down
1 change: 1 addition & 0 deletions system/mrm_handler/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
<depend>rclcpp_components</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>tier4_autoware_utils</depend>
<depend>tier4_system_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
Expand Down
Loading

0 comments on commit 0479160

Please sign in to comment.