From c8e80c6a8a8c28c768db62508563a383479d72e8 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 5 Dec 2022 15:24:29 +0900 Subject: [PATCH 1/3] feat(transition_manager): add param to ignore autonomous transition condition Signed-off-by: Takamasa Horibe --- .../config/operation_mode_transition_manager.param.yaml | 1 + control/operation_mode_transition_manager/src/data.hpp | 1 + control/operation_mode_transition_manager/src/state.cpp | 9 ++++++++- .../operation_mode_transition_manager.param.yaml | 1 + 4 files changed, 11 insertions(+), 1 deletion(-) 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/data.hpp b/control/operation_mode_transition_manager/src/data.hpp index 9b822936a0252..22bcb9d7c92fd 100644 --- a/control/operation_mode_transition_manager/src/data.hpp +++ b/control/operation_mode_transition_manager/src/data.hpp @@ -58,6 +58,7 @@ struct Transition struct EngageAcceptableParam { + bool check_engage_condition = true; // if false, the vehicle is engaged without any checks. bool allow_autonomous_in_stopped = true; double dist_threshold = 2.0; double speed_upper_threshold = 10.0; diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 11c4fd070343f..0053804c93dc7 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -45,6 +45,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) // params for mode change available { auto & p = engage_acceptable_param_; + p.check_engage_condition = node->declare_parameter("check_engage_condition"); p.allow_autonomous_in_stopped = node->declare_parameter("engage_acceptable_limits.allow_autonomous_in_stopped"); p.dist_threshold = node->declare_parameter("engage_acceptable_limits.dist_threshold"); @@ -180,12 +181,18 @@ std::pair AutonomousMode::hasDangerLateralAcceleration() bool AutonomousMode::isModeChangeAvailable() { + const auto & param = engage_acceptable_param_; + if (!param.check_engage_condition) { + RCLCPP_INFO(logger_, "check_engage_condition is false. Engage is accepted."); + debug_info_.is_all_ok = true; + return true; + } + constexpr auto dist_max = 100.0; constexpr auto yaw_max = M_PI_4; const auto current_speed = kinematics_.twist.twist.linear.x; const auto target_control_speed = control_cmd_.longitudinal.speed; - const auto & param = engage_acceptable_param_; if (trajectory_.points.size() < 2) { RCLCPP_WARN_SKIPFIRST_THROTTLE( 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 From e47fbc236b97f6598aca6cfc6bdb58123f7732d3 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 5 Dec 2022 19:51:21 +0900 Subject: [PATCH 2/3] same for modeChangeCompleted Signed-off-by: Takamasa Horibe --- .../operation_mode_transition_manager/src/data.hpp | 1 - .../operation_mode_transition_manager/src/state.cpp | 13 ++++++++++--- .../operation_mode_transition_manager/src/state.hpp | 1 + 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/control/operation_mode_transition_manager/src/data.hpp b/control/operation_mode_transition_manager/src/data.hpp index 22bcb9d7c92fd..9b822936a0252 100644 --- a/control/operation_mode_transition_manager/src/data.hpp +++ b/control/operation_mode_transition_manager/src/data.hpp @@ -58,7 +58,6 @@ struct Transition struct EngageAcceptableParam { - bool check_engage_condition = true; // if false, the vehicle is engaged without any checks. bool allow_autonomous_in_stopped = true; double dist_threshold = 2.0; double speed_upper_threshold = 10.0; diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 0053804c93dc7..40188de2d6636 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -42,10 +42,11 @@ 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_; - p.check_engage_condition = node->declare_parameter("check_engage_condition"); p.allow_autonomous_in_stopped = node->declare_parameter("engage_acceptable_limits.allow_autonomous_in_stopped"); p.dist_threshold = node->declare_parameter("engage_acceptable_limits.dist_threshold"); @@ -81,6 +82,12 @@ void AutonomousMode::update(bool transition) bool AutonomousMode::isModeChangeCompleted() { + if (!check_engage_condition_) { + RCLCPP_INFO( + logger_, "check_engage_condition is false. Mode change completion check is ignored."); + return true; + } + constexpr auto dist_max = 5.0; constexpr auto yaw_max = M_PI_4; @@ -181,8 +188,7 @@ std::pair AutonomousMode::hasDangerLateralAcceleration() bool AutonomousMode::isModeChangeAvailable() { - const auto & param = engage_acceptable_param_; - if (!param.check_engage_condition) { + if (!check_engage_condition_) { RCLCPP_INFO(logger_, "check_engage_condition is false. Engage is accepted."); debug_info_.is_all_ok = true; return true; @@ -193,6 +199,7 @@ bool AutonomousMode::isModeChangeAvailable() const auto current_speed = kinematics_.twist.twist.linear.x; const auto target_control_speed = control_cmd_.longitudinal.speed; + const auto & param = engage_acceptable_param_; if (trajectory_.points.size() < 2) { RCLCPP_WARN_SKIPFIRST_THROTTLE( 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_; From 67e9978f259f617e2a7a054d296b3d24d3d9d204 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 5 Dec 2022 20:04:00 +0900 Subject: [PATCH 3/3] remove debug print Signed-off-by: Takamasa Horibe --- control/operation_mode_transition_manager/src/state.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 40188de2d6636..3456838bf6bf3 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -83,8 +83,6 @@ void AutonomousMode::update(bool transition) bool AutonomousMode::isModeChangeCompleted() { if (!check_engage_condition_) { - RCLCPP_INFO( - logger_, "check_engage_condition is false. Mode change completion check is ignored."); return true; } @@ -189,7 +187,6 @@ std::pair AutonomousMode::hasDangerLateralAcceleration() bool AutonomousMode::isModeChangeAvailable() { if (!check_engage_condition_) { - RCLCPP_INFO(logger_, "check_engage_condition is false. Engage is accepted."); debug_info_.is_all_ok = true; return true; }