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");