feat(transition_manager): add param to ignore autonomous transition c…

…ondition (autowarefoundation#2453) * feat(transition_manager): add param to ignore autonomous transition condition Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * same for modeChangeCompleted Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * remove debug print Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
kyoichi-sugahara · Dec 6, 2022 · f984fbb · f984fbb
1 parent d3e640d
commit f984fbb
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 0 deletions.
diff --git a/...rol/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/...rol/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
@@ -42,6 +42,8 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node)
   sub_trajectory_ = node->create_subscription<Trajectory>(
     "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; });
 
+  check_engage_condition_ = node->declare_parameter<bool>("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<bool, bool> 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
@@ -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/...nch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/...nch/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