Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(mrm_handler, emergency_handler): remove takeover #6522

Merged
merged 2 commits into from
Mar 6, 2024
Merged

feat(mrm_handler, emergency_handler): remove takeover #6522

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
55c10e2
update(mrm_handler, emergency_handler): remove takeover
veqcc Mar 1, 2024
cd7c049
Merge branch 'main' into update_mrm_handler
veqcc Mar 5, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 0 additions & 2 deletions system/emergency_handler/config/emergency_handler.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,8 +4,6 @@
ros__parameters:
update_rate: 10
timeout_hazard_status: 0.5
timeout_takeover_request: 10.0
use_takeover_request: false
use_parking_after_stopped: false
use_comfortable_stop: false

Expand Down
4 changes: 0 additions & 4 deletions system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,6 @@ struct Param
{
int update_rate;
double timeout_hazard_status;
double timeout_takeover_request;
bool use_takeover_request;
bool use_parking_after_stopped;
bool use_comfortable_stop;
HazardLampPolicy turning_hazard_on{};
Expand Down Expand Up @@ -135,8 +133,6 @@ class EmergencyHandler : public rclcpp::Node
rclcpp::Time stamp_hazard_status_;

// Algorithm
rclcpp::Time takeover_requested_time_;
bool is_takeover_request_ = false;
void transitionTo(const int new_state);
void updateMrmState();
void operateMrm();
Expand Down
12 changes: 0 additions & 12 deletions system/emergency_handler/schema/emergency_handler.schema.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,6 @@
"description": "If the input `hazard_status` topic cannot be received for more than `timeout_hazard_status`, vehicle will make an emergency stop.",
"default": 0.5
},
"timeout_takeover_request": {
"type": "number",
"description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.",
"default": 10.0
},
"use_takeover_request": {
"type": "boolean",
"description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.",
"default": "false"
},
"use_parking_after_stopped": {
"type": "boolean",
"description": "If this parameter is true, it will publish PARKING shift command.",
Expand All @@ -51,8 +41,6 @@
"required": [
"update_rate",
"timeout_hazard_status",
"timeout_takeover_request",
"use_takeover_request",
"use_parking_after_stopped",
"use_comfortable_stop",
"turning_hazard_on"
Expand Down
28 changes: 4 additions & 24 deletions system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,6 @@
// Parameter
param_.update_rate = declare_parameter<int>("update_rate");
param_.timeout_hazard_status = declare_parameter<double>("timeout_hazard_status");
param_.timeout_takeover_request = declare_parameter<double>("timeout_takeover_request");
param_.use_takeover_request = declare_parameter<bool>("use_takeover_request");
param_.use_parking_after_stopped = declare_parameter<bool>("use_parking_after_stopped");
param_.use_comfortable_stop = declare_parameter<bool>("use_comfortable_stop");
param_.turning_hazard_on.emergency = declare_parameter<bool>("turning_hazard_on.emergency");
Expand Down Expand Up @@ -374,42 +372,24 @@
const bool is_emergency = isEmergency(hazard_status_stamped_->status);

// Get mode
const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL;

// State Machine
if (mrm_state_.state == MrmState::NORMAL) {
// NORMAL
if (is_auto_mode && is_emergency) {
if (param_.use_takeover_request) {
takeover_requested_time_ = this->get_clock()->now();
is_takeover_request_ = true;
return;
} else {
transitionTo(MrmState::MRM_OPERATING);
return;
}
transitionTo(MrmState::MRM_OPERATING);
return;

Check warning on line 382 in system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp#L381-L382 

Added lines #L381 - L382 were not covered by tests
}
} else {
// Emergency
// Send recovery events if "not emergency" or "takeover done"
// Send recovery events if "not emergency"
if (!is_emergency) {
transitionTo(MrmState::NORMAL);
return;
}
// TODO(Kenji Miyake): Check if human can safely override, for example using DSM
if (is_takeover_done) {
transitionTo(MrmState::NORMAL);
return;
}

if (is_takeover_request_) {
const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_;
if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) {
transitionTo(MrmState::MRM_OPERATING);
return;
}
} else if (mrm_state_.state == MrmState::MRM_OPERATING) {
if (mrm_state_.state == MrmState::MRM_OPERATING) {

Check notice on line 392 in system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

EmergencyHandler::updateMrmState decreases in cyclomatic complexity from 14 to 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 392 in system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Bumpy Road Ahead 

EmergencyHandler::updateMrmState decreases from 4 to 3 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check warning on line 392 in system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp#L392 

Added line #L392 was not covered by tests
// TODO(Kenji Miyake): Check MRC is accomplished
if (isStopped()) {
transitionTo(MrmState::MRM_SUCCEEDED);
Expand Down
2 changes: 0 additions & 2 deletions system/mrm_handler/config/mrm_handler.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,6 @@
timeout_operation_mode_availability: 0.5
use_emergency_holding: false
timeout_emergency_recovery: 5.0
timeout_takeover_request: 10.0
use_takeover_request: false
use_parking_after_stopped: false
use_pull_over: false
use_comfortable_stop: false
Expand Down
4 changes: 0 additions & 4 deletions system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,6 @@ struct Param
double timeout_operation_mode_availability;
bool use_emergency_holding;
double timeout_emergency_recovery;
double timeout_takeover_request;
bool use_takeover_request;
bool use_parking_after_stopped;
bool use_pull_over;
bool use_comfortable_stop;
Expand Down Expand Up @@ -145,8 +143,6 @@ class MrmHandler : public rclcpp::Node
std::optional<rclcpp::Time> stamp_autonomous_become_unavailable_ = std::nullopt;

// Algorithm
rclcpp::Time takeover_requested_time_;
bool is_takeover_request_ = false;
bool is_emergency_holding_ = false;
void transitionTo(const int new_state);
void updateMrmState();
Expand Down
12 changes: 0 additions & 12 deletions system/mrm_handler/schema/mrm_handler.schema.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,16 +26,6 @@
"description": "If the duration of the EMERGENCY state is longer than `timeout_emergency_recovery`, it does not recover to the NORMAL state.",
"default": 5.0
},
"timeout_takeover_request": {
"type": "number",
"description": "Transition to MRR_OPERATING if the time from the last takeover request exceeds `timeout_takeover_request`.",
"default": 10.0
},
"use_takeover_request": {
"type": "boolean",
"description": "If this parameter is true, the handler will record the time and make take over request to the driver when emergency state occurs.",
"default": "false"
},
"use_parking_after_stopped": {
"type": "boolean",
"description": "If this parameter is true, it will publish PARKING shift command.",
Expand Down Expand Up @@ -68,8 +58,6 @@
"timeout_operation_mode_availability",
"use_emergency_holding",
"timeout_emergency_recovery",
"timeout_takeover_request",
"use_takeover_request",
"use_parking_after_stopped",
"use_pull_over",
"use_comfortable_stop",
Expand Down
28 changes: 4 additions & 24 deletions system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2024 TIER IV, Inc.

Check notice on line 1 in system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 4.95 to 4.77, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -26,8 +26,6 @@
declare_parameter<double>("timeout_operation_mode_availability", 0.5);
param_.use_emergency_holding = declare_parameter<bool>("use_emergency_holding", false);
param_.timeout_emergency_recovery = declare_parameter<double>("timeout_emergency_recovery", 5.0);
param_.timeout_takeover_request = declare_parameter<double>("timeout_takeover_request", 10.0);
param_.use_takeover_request = declare_parameter<bool>("use_takeover_request", false);
param_.use_parking_after_stopped = declare_parameter<bool>("use_parking_after_stopped", false);
param_.use_pull_over = declare_parameter<bool>("use_pull_over", false);
param_.use_comfortable_stop = declare_parameter<bool>("use_comfortable_stop", false);
Expand Down Expand Up @@ -433,42 +431,24 @@
const bool is_emergency = isEmergency();

// Get mode
const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS;
const bool is_takeover_done = control_mode_->mode == ControlModeReport::MANUAL;

// State Machine
if (mrm_state_.state == MrmState::NORMAL) {
// NORMAL
if (is_auto_mode && is_emergency) {
if (param_.use_takeover_request) {
takeover_requested_time_ = this->get_clock()->now();
is_takeover_request_ = true;
return;
} else {
transitionTo(MrmState::MRM_OPERATING);
return;
}
transitionTo(MrmState::MRM_OPERATING);
return;

Check warning on line 441 in system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp#L440-L441 

Added lines #L440 - L441 were not covered by tests
}
} else {
// Emergency
// Send recovery events if "not emergency" or "takeover done"
// Send recovery events if "not emergency"
if (!is_emergency) {
transitionTo(MrmState::NORMAL);
return;
}
// TODO(TetsuKawa): Check if human can safely override, for example using DSM
if (is_takeover_done) {
transitionTo(MrmState::NORMAL);
return;
}

if (is_takeover_request_) {
const auto time_from_takeover_request = this->get_clock()->now() - takeover_requested_time_;
if (time_from_takeover_request.seconds() > param_.timeout_takeover_request) {
transitionTo(MrmState::MRM_OPERATING);
return;
}
} else if (mrm_state_.state == MrmState::MRM_OPERATING) {
if (mrm_state_.state == MrmState::MRM_OPERATING) {

Check notice on line 451 in system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

MrmHandler::updateMrmState decreases in cyclomatic complexity from 18 to 14, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check notice on line 451 in system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Bumpy Road Ahead 

MrmHandler::updateMrmState decreases from 6 to 5 logical blocks with deeply nested code, threshold is one single block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check warning on line 451 in system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

View check run for this annotation

Codecov / codecov/patch

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp#L451 

Added line #L451 was not covered by tests
// TODO(TetsuKawa): Check MRC is accomplished
if (mrm_state_.behavior == MrmState::PULL_OVER) {
if (isStopped() && isArrivedAtGoal()) {
Expand Down
Loading