From f8af70c4af2ff7d5b90144d51accf83d9a37305b Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 19 Dec 2022 20:45:09 +0900 Subject: [PATCH] feat(operation_mode_transition_manager): add parameter about nearest search (#2523) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- ...eration_mode_transition_manager.param.yaml | 2 ++ .../src/state.cpp | 20 +++++++++---------- .../src/state.hpp | 4 +++- 3 files changed, 15 insertions(+), 11 deletions(-) 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 a0ddef6e363cc..a86443f5cabdb 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 @@ -3,6 +3,8 @@ 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. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index 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 775083908cdfd..8980f665d916d 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -43,6 +43,10 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); check_engage_condition_ = node->declare_parameter("check_engage_condition"); + nearest_dist_deviation_threshold_ = + node->declare_parameter("nearest_dist_deviation_threshold"); + nearest_yaw_deviation_threshold_ = + node->declare_parameter("nearest_yaw_deviation_threshold"); // params for mode change available { @@ -86,9 +90,6 @@ bool AutonomousMode::isModeChangeCompleted() return true; } - constexpr auto dist_max = 5.0; - constexpr auto yaw_max = M_PI_4; - const auto current_speed = kinematics_.twist.twist.linear.x; const auto & param = engage_acceptable_param_; @@ -107,8 +108,9 @@ bool AutonomousMode::isModeChangeCompleted() return unstable(); } - const auto closest_idx = - findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max); + const auto closest_idx = findNearestIndex( + trajectory_.points, kinematics_.pose.pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); if (!closest_idx) { RCLCPP_INFO(logger_, "Not stable yet: closest point not found"); return unstable(); @@ -199,9 +201,6 @@ bool AutonomousMode::isModeChangeAvailable() 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_; @@ -213,8 +212,9 @@ bool AutonomousMode::isModeChangeAvailable() return false; } - const auto closest_idx = - findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max); + const auto closest_idx = findNearestIndex( + trajectory_.points, kinematics_.pose.pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); if (!closest_idx) { RCLCPP_INFO(logger_, "Engage unavailable: closest point not found"); debug_info_ = DebugInfo{}; // all false diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index dc72d57aed231..9d384857bbe3d 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -72,7 +72,9 @@ 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. + bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + double nearest_dist_deviation_threshold_; // [m] for finding nearest index + double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; AckermannControlCommand control_cmd_;