From f984fbb708cb02947ec2824ce041c739c35940f7 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 6 Dec 2022 13:55:17 +0900 Subject: [PATCH] feat(transition_manager): add param to ignore autonomous transition condition (#2453) * feat(transition_manager): add param to ignore autonomous transition condition Signed-off-by: Takamasa Horibe * same for modeChangeCompleted Signed-off-by: Takamasa Horibe * remove debug print Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../operation_mode_transition_manager.param.yaml | 1 + .../operation_mode_transition_manager/src/state.cpp | 11 +++++++++++ .../operation_mode_transition_manager/src/state.hpp | 1 + .../operation_mode_transition_manager.param.yaml | 1 + 4 files changed, 14 insertions(+) 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 7976bb45d4c6f..775083908cdfd 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; @@ -188,6 +194,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