feat(operation_mode_transition_manager): add parameter about nearest …

…search (autowarefoundation#2523)

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp>

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

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<bool>("check_engage_condition");
+  nearest_dist_deviation_threshold_ =
+    node->declare_parameter<double>("nearest_dist_deviation_threshold");
+  nearest_yaw_deviation_threshold_ =
+    node->declare_parameter<double>("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

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_;