Skip to content

Commit 1576810

Browse files
style(pre-commit): autofix
1 parent d421e38 commit 1576810

File tree

3 files changed

+378
-11
lines changed

3 files changed

+378
-11
lines changed

system/mrm_handler/image/mrm-state.svg

+357-1
Loading

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,8 @@ class MrmHandler : public rclcpp::Node
129129
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
130130

131131
bool requestMrmBehavior(
132-
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, bool call_or_cancel) const;
132+
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
133+
bool call_or_cancel) const;
133134
void logMrmCallingResult(
134135
const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior,
135136
bool is_call) const;

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

+19-9
Original file line numberDiff line numberDiff line change
@@ -22,9 +22,11 @@ MrmHandler::MrmHandler() : Node("mrm_handler")
2222
{
2323
// Parameter
2424
param_.update_rate = declare_parameter<int>("update_rate", 10);
25-
param_.timeout_operation_mode_availability = declare_parameter<double>("timeout_operation_mode_availability", 0.5);
25+
param_.timeout_operation_mode_availability =
26+
declare_parameter<double>("timeout_operation_mode_availability", 0.5);
2627
param_.timeout_call_mrm_behavior = declare_parameter<double>("timeout_call_mrm_behavior", 0.01);
27-
param_.timeout_cancel_mrm_behavior = declare_parameter<double>("timeout_cancel_mrm_behavior", 0.01);
28+
param_.timeout_cancel_mrm_behavior =
29+
declare_parameter<double>("timeout_cancel_mrm_behavior", 0.01);
2830
param_.use_emergency_holding = declare_parameter<bool>("use_emergency_holding", false);
2931
param_.timeout_emergency_recovery = declare_parameter<double>("timeout_emergency_recovery", 5.0);
3032
param_.timeout_takeover_request = declare_parameter<double>("timeout_takeover_request", 10.0);
@@ -261,7 +263,7 @@ void MrmHandler::handlePostFailureRequest()
261263
{
262264
using autoware_adapi_v1_msgs::msg::MrmState;
263265

264-
if(requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) {
266+
if (requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) {
265267
if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING);
266268
} else {
267269
transitionTo(MrmState::MRM_FAILED);
@@ -270,13 +272,15 @@ void MrmHandler::handlePostFailureRequest()
270272
}
271273

272274
bool MrmHandler::requestMrmBehavior(
273-
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, bool call_or_cancel) const
275+
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
276+
bool call_or_cancel) const
274277
{
275278
using autoware_adapi_v1_msgs::msg::MrmState;
276279

277280
auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
278281
request->operate = call_or_cancel; // true: call, false: cancel
279-
const auto duration = std::chrono::duration<double, std::ratio<1>>(call_or_cancel ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior);
282+
const auto duration = std::chrono::duration<double, std::ratio<1>>(
283+
call_or_cancel ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior);
280284
std::shared_future<std::shared_ptr<tier4_system_msgs::srv::OperateMrm::Response>> future;
281285

282286
const auto behavior2string = [](const int behavior) {
@@ -317,17 +321,23 @@ bool MrmHandler::requestMrmBehavior(
317321
return false;
318322
}
319323

320-
if(future.wait_for(duration) == std::future_status::ready) {
324+
if (future.wait_for(duration) == std::future_status::ready) {
321325
const auto result = future.get();
322326
if (result->response.success == true) {
323-
RCLCPP_WARN(this->get_logger(), call_or_cancel ? "%s is operated." : "%s is canceled.", behavior2string(mrm_behavior));
327+
RCLCPP_WARN(
328+
this->get_logger(), call_or_cancel ? "%s is operated." : "%s is canceled.",
329+
behavior2string(mrm_behavior));
324330
return true;
325331
} else {
326-
RCLCPP_ERROR(this->get_logger(), call_or_cancel ? "%s failed to operate." : "%s failed to cancel.", behavior2string(mrm_behavior));
332+
RCLCPP_ERROR(
333+
this->get_logger(), call_or_cancel ? "%s failed to operate." : "%s failed to cancel.",
334+
behavior2string(mrm_behavior));
327335
return false;
328336
}
329337
} else {
330-
RCLCPP_ERROR(this->get_logger(), call_or_cancel ? "%s call timed out." : "%s cancel timed out.", behavior2string(mrm_behavior));
338+
RCLCPP_ERROR(
339+
this->get_logger(), call_or_cancel ? "%s call timed out." : "%s cancel timed out.",
340+
behavior2string(mrm_behavior));
331341
return false;
332342
}
333343
}

0 commit comments

Comments
 (0)