Skip to content

Commit e5ce69b

Browse files
committed
feat: replace define with enum
Signed-off-by: TetsuKawa <kawaguchitnon@icloud.com>
1 parent 1576810 commit e5ce69b

File tree

2 files changed

+24
-21
lines changed

2 files changed

+24
-21
lines changed

system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp

+8-4
Original file line numberDiff line numberDiff line change
@@ -38,8 +38,6 @@
3838
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
3939
#include <nav_msgs/msg/odometry.hpp>
4040

41-
#define CALL true
42-
#define CANCEL false
4341

4442
struct HazardLampPolicy
4543
{
@@ -68,6 +66,13 @@ class MrmHandler : public rclcpp::Node
6866
MrmHandler();
6967

7068
private:
69+
//type
70+
enum RequestType
71+
{
72+
CALL,
73+
CANCEL
74+
};
75+
7176
// Subscribers
7277
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
7378
rclcpp::Subscription<autoware_auto_vehicle_msgs::msg::ControlModeReport>::SharedPtr
@@ -129,8 +134,7 @@ class MrmHandler : public rclcpp::Node
129134
rclcpp::Client<tier4_system_msgs::srv::OperateMrm>::SharedPtr client_mrm_emergency_stop_;
130135

131136
bool requestMrmBehavior(
132-
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
133-
bool call_or_cancel) const;
137+
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) const;
134138
void logMrmCallingResult(
135139
const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior,
136140
bool is_call) const;

system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp

+16-17
Original file line numberDiff line numberDiff line change
@@ -226,7 +226,7 @@ void MrmHandler::operateMrm()
226226
if (current_mrm_behavior == mrm_state_.behavior) {
227227
return;
228228
}
229-
if (requestMrmBehavior(mrm_state_.behavior, CANCEL)) {
229+
if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) {
230230
mrm_state_.behavior = current_mrm_behavior;
231231
} else {
232232
handlePostFailureRequest();
@@ -238,9 +238,9 @@ void MrmHandler::operateMrm()
238238
if (current_mrm_behavior == mrm_state_.behavior) {
239239
return;
240240
}
241-
if (!requestMrmBehavior(mrm_state_.behavior, CANCEL)) {
241+
if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) {
242242
handlePostFailureRequest();
243-
} else if (requestMrmBehavior(current_mrm_behavior, CALL)) {
243+
} else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) {
244244
mrm_state_.behavior = current_mrm_behavior;
245245
} else {
246246
handlePostFailureRequest();
@@ -272,15 +272,20 @@ void MrmHandler::handlePostFailureRequest()
272272
}
273273

274274
bool MrmHandler::requestMrmBehavior(
275-
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior,
276-
bool call_or_cancel) const
275+
const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, RequestType request_type) const
277276
{
278277
using autoware_adapi_v1_msgs::msg::MrmState;
279278

280279
auto request = std::make_shared<tier4_system_msgs::srv::OperateMrm::Request>();
281-
request->operate = call_or_cancel; // true: call, false: cancel
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);
280+
if (request_type == RequestType::CALL) {
281+
request->operate = true;
282+
} else if (request_type == RequestType::CANCEL) {
283+
request->operate = false;
284+
} else {
285+
RCLCPP_ERROR(this->get_logger(), "invalid request type: %d", request_type);
286+
return false;
287+
}
288+
const auto duration = std::chrono::duration<double, std::ratio<1>>(request->operate ? param_.timeout_call_mrm_behavior : param_.timeout_cancel_mrm_behavior);
284289
std::shared_future<std::shared_ptr<tier4_system_msgs::srv::OperateMrm::Response>> future;
285290

286291
const auto behavior2string = [](const int behavior) {
@@ -324,20 +329,14 @@ bool MrmHandler::requestMrmBehavior(
324329
if (future.wait_for(duration) == std::future_status::ready) {
325330
const auto result = future.get();
326331
if (result->response.success == true) {
327-
RCLCPP_WARN(
328-
this->get_logger(), call_or_cancel ? "%s is operated." : "%s is canceled.",
329-
behavior2string(mrm_behavior));
332+
RCLCPP_WARN(this->get_logger(), request->operate ? "%s is operated." : "%s is canceled.", behavior2string(mrm_behavior));
330333
return true;
331334
} else {
332-
RCLCPP_ERROR(
333-
this->get_logger(), call_or_cancel ? "%s failed to operate." : "%s failed to cancel.",
334-
behavior2string(mrm_behavior));
335+
RCLCPP_ERROR(this->get_logger(), request->operate ? "%s failed to operate." : "%s failed to cancel.", behavior2string(mrm_behavior));
335336
return false;
336337
}
337338
} else {
338-
RCLCPP_ERROR(
339-
this->get_logger(), call_or_cancel ? "%s call timed out." : "%s cancel timed out.",
340-
behavior2string(mrm_behavior));
339+
RCLCPP_ERROR(this->get_logger(), request->operate ? "%s call timed out." : "%s cancel timed out.", behavior2string(mrm_behavior));
341340
return false;
342341
}
343342
}

0 commit comments

Comments
 (0)