diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index 45014428d0bd1..a0ddef6e363cc 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -2,6 +2,7 @@ ros__parameters: transition_timeout: 10.0 frequency_hz: 10.0 + check_engage_condition: false # set false if you do not want to care about the engage condition. engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. dist_threshold: 1.5 diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 11c4fd070343f..3456838bf6bf3 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -42,6 +42,8 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) sub_trajectory_ = node->create_subscription( "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); + check_engage_condition_ = node->declare_parameter("check_engage_condition"); + // params for mode change available { auto & p = engage_acceptable_param_; @@ -80,6 +82,10 @@ void AutonomousMode::update(bool transition) bool AutonomousMode::isModeChangeCompleted() { + if (!check_engage_condition_) { + return true; + } + constexpr auto dist_max = 5.0; constexpr auto yaw_max = M_PI_4; @@ -180,6 +186,11 @@ std::pair AutonomousMode::hasDangerLateralAcceleration() bool AutonomousMode::isModeChangeAvailable() { + if (!check_engage_condition_) { + debug_info_.is_all_ok = true; + return true; + } + constexpr auto dist_max = 100.0; constexpr auto yaw_max = M_PI_4; diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index bb8665e44c7d5..dc72d57aed231 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -72,6 +72,7 @@ class AutonomousMode : public ModeChangeBase rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; + bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; AckermannControlCommand control_cmd_; diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml index 45014428d0bd1..a0ddef6e363cc 100644 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -2,6 +2,7 @@ ros__parameters: transition_timeout: 10.0 frequency_hz: 10.0 + check_engage_condition: false # set false if you do not want to care about the engage condition. engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. dist_threshold: 1.5