Skip to content

Commit d5882c9

Browse files
feat(mrm_handler): operate mrm only when autonomous operation mode (autowarefoundation#7784)
* feat: add isOperationModeAutonomous() function to MRM handler core The code changes add a new function `isOperationModeAutonomous()` to the MRM handler core. This function is used to check if the operation mode is set to autonomous. Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent ce09907 commit d5882c9

File tree

2 files changed

+14
-4
lines changed

2 files changed

+14
-4
lines changed

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -148,7 +148,8 @@ class MrmHandler : public rclcpp::Node
148148
bool isStopped();
149149
bool isDrivingBackwards();
150150
bool isEmergency() const;
151-
bool isAutonomous();
151+
bool isControlModeAutonomous();
152+
bool isOperationModeAutonomous();
152153
bool isPullOverStatusAvailable();
153154
bool isComfortableStopStatusAvailable();
154155
bool isEmergencyStopStatusAvailable();

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

+12-3
Original file line numberDiff line numberDiff line change
@@ -396,12 +396,13 @@ void MrmHandler::updateMrmState()
396396
}
397397

398398
// Get mode
399-
const bool is_auto_mode = isAutonomous();
399+
const bool is_control_mode_autonomous = isControlModeAutonomous();
400+
const bool is_operation_mode_autonomous = isOperationModeAutonomous();
400401

401402
// State Machine
402403
switch (mrm_state_.state) {
403404
case MrmState::NORMAL:
404-
if (is_auto_mode) {
405+
if (is_control_mode_autonomous && is_operation_mode_autonomous) {
405406
transitionTo(MrmState::MRM_OPERATING);
406407
}
407408
return;
@@ -537,14 +538,22 @@ bool MrmHandler::isEmergency() const
537538
is_operation_mode_availability_timeout;
538539
}
539540

540-
bool MrmHandler::isAutonomous()
541+
bool MrmHandler::isControlModeAutonomous()
541542
{
542543
using autoware_vehicle_msgs::msg::ControlModeReport;
543544
auto mode = sub_control_mode_.takeData();
544545
if (mode == nullptr) return false;
545546
return mode->mode == ControlModeReport::AUTONOMOUS;
546547
}
547548

549+
bool MrmHandler::isOperationModeAutonomous()
550+
{
551+
using autoware_adapi_v1_msgs::msg::OperationModeState;
552+
auto state = sub_operation_mode_state_.takeData();
553+
if (state == nullptr) return false;
554+
return state->mode == OperationModeState::AUTONOMOUS;
555+
}
556+
548557
bool MrmHandler::isPullOverStatusAvailable()
549558
{
550559
auto status = sub_mrm_pull_over_status_.takeData();

0 commit comments

Comments
 (0)