From 55f4bb733e2ed4ca330dfbbb2279f3e42e62713f Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Mon, 28 Nov 2022 18:04:45 +0900 Subject: [PATCH] feat(operation_mode_transition_manager): add guard to invalid parameter Signed-off-by: tomoya.kimura --- .../operation_mode_transition_manager/src/node.cpp | 13 ++++++++++++- .../operation_mode_transition_manager/src/state.cpp | 2 +- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp index ddd37b0206fad..155b01172f4bf 100644 --- a/control/operation_mode_transition_manager/src/node.cpp +++ b/control/operation_mode_transition_manager/src/node.cpp @@ -50,12 +50,23 @@ OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::Nod } // initialize state - transition_timeout_ = declare_parameter("transition_timeout"); current_mode_ = OperationMode::STOP; transition_ = nullptr; gate_operation_mode_.mode = OperationModeState::UNKNOWN; gate_operation_mode_.is_in_transition = false; control_mode_report_.mode = ControlModeReport::NO_COMMAND; + transition_timeout_ = declare_parameter("transition_timeout"); + { + // check `transition_timeout` value + const auto stable_duration = declare_parameter("stable_check.duration"); + const double TIMEOUT_MARGIN = 0.5; + if (transition_timeout_ < stable_duration + TIMEOUT_MARGIN) { + transition_timeout_ = stable_duration + TIMEOUT_MARGIN; + RCLCPP_WARN( + get_logger(), "`transition_timeout` must be somewhat larger than `stable_check.duration`"); + RCLCPP_WARN_STREAM(get_logger(), "transition_timeout is set to " << transition_timeout_); + } + } // modes modes_[OperationMode::STOP] = std::make_unique(); diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 11c4fd070343f..7836e4facfc65 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -63,7 +63,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) // params for mode change completed { auto & p = stable_check_param_; - p.duration = node->declare_parameter("stable_check.duration"); + p.duration = node->get_parameter("stable_check.duration").as_double(); p.dist_threshold = node->declare_parameter("stable_check.dist_threshold"); p.speed_upper_threshold = node->declare_parameter("stable_check.speed_upper_threshold"); p.speed_lower_threshold = node->declare_parameter("stable_check.speed_lower_threshold");