From e2b6761d01d5dad5943bc28201fec63bf2a5827d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 26 Sep 2023 18:25:32 +0900 Subject: [PATCH 1/4] feat(concealer): Add support for optional MrmState::PULL_OVER for mrm behavior --- ...ator_application_for_autoware_universe.cpp | 83 +++++++++++++------ 1 file changed, 58 insertions(+), 25 deletions(-) diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 3c987456ec0..7ad4bd815d7 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -136,6 +136,13 @@ DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_LEFT); DEFINE_STATIC_DATA_MEMBER_DETECTOR(AVOIDANCE_BY_LC_RIGHT); DEFINE_STATIC_DATA_MEMBER_DETECTOR(NO_DRIVABLE_LANE); +// For MrmState::behavior +DEFINE_STATIC_DATA_MEMBER_DETECTOR(COMFORTABLE_STOP); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(EMERGENCY_STOP); +// DEFINE_STATIC_DATA_MEMBER_DETECTOR(NONE); // NOTE: This is defined above. +DEFINE_STATIC_DATA_MEMBER_DETECTOR(UNKNOWN); +DEFINE_STATIC_DATA_MEMBER_DETECTOR(PULL_OVER); + #undef DEFINE_STATIC_DATA_MEMBER_DETECTOR /** @@ -221,7 +228,8 @@ bool isValidCooperateStatus( * NOTE2: The difference in the variable referred as a distance is the impact of the * message specification changes in the following URL. * This was also decided after consulting with a member of TIER IV planning and control team. - * ref: https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef + * ref: + * https://github.com/tier4/tier4_autoware_msgs/commit/8b85e6e43aa48cf4a439c77bf4bf6aee2e70c3ef */ if constexpr (HasDistance::value) { return cooperate_status.module.type == module_type && @@ -455,34 +463,59 @@ auto FieldOperatorApplicationFor::receiveEmergencyState( #undef CASE } +template +auto toMinimumRiskManeuverBehaviorString(const std::uint8_t & behavior_number) +{ + static const std::unordered_map behavior_string_map = [&]() { + std::unordered_map behavior_string_map; + +#define EMPLACE(IDENTIFIER) \ + if constexpr (HasStatic##IDENTIFIER::value) { \ + behavior_string_map.emplace(T::IDENTIFIER, #IDENTIFIER); \ + } \ + static_assert(true) + + EMPLACE(COMFORTABLE_STOP); + EMPLACE(EMERGENCY_STOP); + EMPLACE(NONE); + EMPLACE(UNKNOWN); + EMPLACE(PULL_OVER); + +#undef EMPLACE + return behavior_string_map; + }(); + + if (const auto behavior = behavior_string_map.find(behavior_number); + behavior == behavior_string_map.end()) { + throw common::Error( + "Unexpected autoware_adapi_v1_msgs::msg::MrmState::behavior, number: ", behavior_number); + } else { + return behavior->second; + } +} + auto FieldOperatorApplicationFor::receiveMrmState( const autoware_adapi_v1_msgs::msg::MrmState & message) -> void { -#define CASE(IDENTIFIER, VARIABLE) \ - case autoware_adapi_v1_msgs::msg::MrmState::IDENTIFIER: \ - VARIABLE = #IDENTIFIER; \ - break + auto to_minimum_risk_maneuver_state_string = [](const std::uint8_t & state_number) { + std::unordered_map state_string_map = { + {autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED, "MRM_FAILED"}, + {autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING, "MRM_OPERATING"}, + {autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED, "MRM_SUCCEEDED"}, + {autoware_adapi_v1_msgs::msg::MrmState::NORMAL, "NORMAL"}, + {autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN, "UNKNOWN"}, + }; - switch (message.state) { - CASE(MRM_FAILED, minimum_risk_maneuver_state); - CASE(MRM_OPERATING, minimum_risk_maneuver_state); - CASE(MRM_SUCCEEDED, minimum_risk_maneuver_state); - CASE(NORMAL, minimum_risk_maneuver_state); - CASE(UNKNOWN, minimum_risk_maneuver_state); - default: - throw common::Error( - "Unsupported MrmState::state, number : ", static_cast(message.state)); - } + if (const auto state = state_string_map.find(state_number); state == state_string_map.end()) { + throw common::Error("Unexpected MrmState::state, number: ", state_number); + } else { + return state->second; + } + }; - switch (message.behavior) { - CASE(COMFORTABLE_STOP, minimum_risk_maneuver_behavior); - CASE(EMERGENCY_STOP, minimum_risk_maneuver_behavior); - CASE(NONE, minimum_risk_maneuver_behavior); - CASE(UNKNOWN, minimum_risk_maneuver_behavior); - default: - throw common::Error( - "Unsupported MrmState::behavior, number : ", static_cast(message.behavior)); - } -#undef CASE + minimum_risk_maneuver_state = to_minimum_risk_maneuver_state_string(message.state); + + minimum_risk_maneuver_behavior = + toMinimumRiskManeuverBehaviorString(message.behavior); } } // namespace concealer From 2974751235871213fdb29a56c165d30525daf70a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 11:31:26 +0900 Subject: [PATCH 2/4] refactor(concealer): convert a lambda function to local function --- ...ator_application_for_autoware_universe.cpp | 38 ++++++++++--------- 1 file changed, 20 insertions(+), 18 deletions(-) diff --git a/external/concealer/src/field_operator_application_for_autoware_universe.cpp b/external/concealer/src/field_operator_application_for_autoware_universe.cpp index 9a2f9387493..ca7f7ffd423 100644 --- a/external/concealer/src/field_operator_application_for_autoware_universe.cpp +++ b/external/concealer/src/field_operator_application_for_autoware_universe.cpp @@ -483,7 +483,7 @@ auto FieldOperatorApplicationFor::receiveEmergencyState( } template -auto toMinimumRiskManeuverBehaviorString(const std::uint8_t & behavior_number) +auto toMinimumRiskManeuverBehaviorString(const std::uint8_t & behavior_number) -> std::string { static const std::unordered_map behavior_string_map = [&]() { std::unordered_map behavior_string_map; @@ -513,26 +513,28 @@ auto toMinimumRiskManeuverBehaviorString(const std::uint8_t & behavior_number) } } -auto FieldOperatorApplicationFor::receiveMrmState( - const autoware_adapi_v1_msgs::msg::MrmState & message) -> void +auto toMinimumRiskManeuverStateString(const std::uint8_t & state_number) -> std::string { - auto to_minimum_risk_maneuver_state_string = [](const std::uint8_t & state_number) { - std::unordered_map state_string_map = { - {autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED, "MRM_FAILED"}, - {autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING, "MRM_OPERATING"}, - {autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED, "MRM_SUCCEEDED"}, - {autoware_adapi_v1_msgs::msg::MrmState::NORMAL, "NORMAL"}, - {autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN, "UNKNOWN"}, - }; - - if (const auto state = state_string_map.find(state_number); state == state_string_map.end()) { - throw common::Error("Unexpected MrmState::state, number: ", state_number); - } else { - return state->second; - } + static const std::unordered_map state_string_map = { + {autoware_adapi_v1_msgs::msg::MrmState::MRM_FAILED, "MRM_FAILED"}, + {autoware_adapi_v1_msgs::msg::MrmState::MRM_OPERATING, "MRM_OPERATING"}, + {autoware_adapi_v1_msgs::msg::MrmState::MRM_SUCCEEDED, "MRM_SUCCEEDED"}, + {autoware_adapi_v1_msgs::msg::MrmState::NORMAL, "NORMAL"}, + {autoware_adapi_v1_msgs::msg::MrmState::UNKNOWN, "UNKNOWN"}, }; - minimum_risk_maneuver_state = to_minimum_risk_maneuver_state_string(message.state); + if (const auto state = state_string_map.find(state_number); state == state_string_map.end()) { + throw common::Error( + "Unexpected autoware_adapi_v1_msgs::msg::MrmState::state, number: ", state_number); + } else { + return state->second; + } +} + +auto FieldOperatorApplicationFor::receiveMrmState( + const autoware_adapi_v1_msgs::msg::MrmState & message) -> void +{ + minimum_risk_maneuver_state = toMinimumRiskManeuverStateString(message.state); minimum_risk_maneuver_behavior = toMinimumRiskManeuverBehaviorString(message.behavior); From f6305868493583624468ddbf6082c92c41a08a9a Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 16 Feb 2024 19:12:55 +0900 Subject: [PATCH 3/4] change example Signed-off-by: Masaya Kataoka Signed-off-by: Kotaro Yoshimoto --- .github/pull_request_samples/example_detail.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/pull_request_samples/example_detail.md b/.github/pull_request_samples/example_detail.md index c31fe9672cd..a9beca2f5d2 100644 --- a/.github/pull_request_samples/example_detail.md +++ b/.github/pull_request_samples/example_detail.md @@ -28,8 +28,8 @@ This PR fixes how the length of the curve is computed ## References -- [determine-arc-length-of-a-catmull-rom-spline-to-move-at-a-constant-speed](https://gamedev.stackexchange.com/questions/14985/determine-arc-length-of-a-catmull-rom-spline-to-move-at-a-constant-speed) - - This link is an example and is not directly related to this sample. +Please check [this document about spline curve.](https://people.computing.clemson.edu/~dhouse/courses/405/notes/splines.pdf) +This link is an example and is not directly related to this sample. # Destructive Changes From 15ed7d29127dcdd334a23747506cb28676fab4bb Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 16 Feb 2024 19:16:42 +0900 Subject: [PATCH 4/4] change link Signed-off-by: Masaya Kataoka --- .github/pull_request_samples/example_detail.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/pull_request_samples/example_detail.md b/.github/pull_request_samples/example_detail.md index a9beca2f5d2..caea007de38 100644 --- a/.github/pull_request_samples/example_detail.md +++ b/.github/pull_request_samples/example_detail.md @@ -28,7 +28,7 @@ This PR fixes how the length of the curve is computed ## References -Please check [this document about spline curve.](https://people.computing.clemson.edu/~dhouse/courses/405/notes/splines.pdf) +See also [this document.](https://tier4.github.io/scenario_simulator_v2-docs/) This link is an example and is not directly related to this sample. # Destructive Changes