optimal & fix file cmake

2026-08-03 22:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
--- a/plugins/back_up_recovery.cpp
+++ b/plugins/back_up_recovery.cpp
@@ -2,12 +2,13 @@
 *
 * Software License Agreement (BSD License)
 *
- * recovery_core — per-cycle backup recovery plugin (goal-driven).
+ * recovery_core — lùi thẳng một quãng, đo bằng pose thật và có kiểm va chạm.
 *
 * Author: DuongTD
 *********************************************************************/

 #include <recovery_core/recovery_behavior.h>
+#include <recovery_core/recovery_math.h>

 #include <algorithm>
 #include <cmath>
@@ -20,18 +21,30 @@ namespace recovery_plugins
 {
 namespace
 {
-constexpr double kDefaultBackupDistance = 0.5;  // m.
-constexpr double kDefaultLinearSpeed = 0.1;     // m/s.
-constexpr double kDefaultControlPeriod = 0.1;   // s per update tick.
+constexpr double kDefaultBackupDistance = 0.3;      // [m]
+constexpr double kDefaultBackupDistanceMax = 1.0;   // [m] trần cứng cho quãng lùi
+constexpr double kDefaultLinearSpeed = 0.1;         // [m/s] độ lớn
+constexpr double kDefaultAccLimX = 0.3;             // [m/s^2]
+constexpr double kMaxLinearSpeed = 1.0;             // [m/s] trần vệ sinh cho param sai
+constexpr double kGoalTolerance = 1e-3;             // [m]
 }  // namespace

 /**
 * @class BackUpRecovery
- * @brief Lùi thẳng tới KHOẢNG ĐÍCH do caller yêu cầu ở start(goal).
+ * @brief Lùi thẳng theo hướng ban đầu tới khi đủ quãng yêu cầu.
 *
- * goal.distance (m, > 0) là khoảng lùi lượt này; 0 nghĩa là dùng default configured. Tốc độ
- * tuyến tính mặc định đọc từ param, có thể override qua goal.params["linear_speed"]. Mỗi
- * update() trả Twist.linear.x < 0 kèm progress/remaining tới khi đủ khoảng -> kSucceeded.
+ * Đây là behavior **nguy hiểm nhất** trong bộ default: lùi là hướng robot thường không có sensor.
+ * Vì vậy nó xếp cuối danh sách, và có ba lớp bảo vệ:
+ *
+ *  1. **Tiến độ đo bằng pose thật** — hình chiếu delta pose lên hướng xuất phát, không tích phân
+ *     vận tốc lệnh. Bản trước nhân vận tốc lệnh với `control_period` lấy từ config, nên control
+ *     loop chạy chậm gấp đôi là robot lùi gấp đôi quãng yêu cầu, còn bánh trượt thì vẫn báo xong.
+ *  2. **Kiểm va chạm mỗi tick** trên pose dự đoán ở cuối chu kỳ tới, trước khi phát lệnh; và một
+ *     lần nữa lúc `onStart()` để không bao giờ khởi động vào chỗ đã bị chặn.
+ *  3. **Mất pose là dừng** — `PoseProvider` trả false thì trả `kFailed` + Twist 0.
+ *
+ * Quãng lùi: `goal.distance` cho lượt này, ngược lại param `backup_distance`; cả hai bị kẹp bởi
+ * `backup_distance_max`.
 */
 class BackUpRecovery final : public recovery_core::RecoveryBehavior
 {
@@ -43,101 +56,188 @@ public:
    return std::make_shared<BackUpRecovery>();
  }

+  recovery_core::RecoveryOutputType outputKind() const override
+  {
+    return recovery_core::RecoveryOutputType::kVelocity;
+  }
+
 protected:
-  void onConfigure() override
+  bool onConfigure(robot::NodeHandle& nh) override
  {
-    robot::NodeHandle private_nh("~/" + name_);
-    private_nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance);
-    private_nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed);
-    private_nh.param("control_period", control_period_, kDefaultControlPeriod);
-    private_nh.param("require_costmap", require_costmap_, false);
+    nh.param("backup_distance", default_backup_distance_, kDefaultBackupDistance);
+    nh.param("backup_distance_max", backup_distance_max_, kDefaultBackupDistanceMax);
+    nh.param("linear_speed", default_linear_speed_, kDefaultLinearSpeed);
+    nh.param("acc_lim_x", acc_lim_x_, kDefaultAccLimX);

-    if (!std::isfinite(default_backup_distance_) || default_backup_distance_ <= 0.0)
+    if (!std::isfinite(backup_distance_max_) || backup_distance_max_ <= 0.0)
    {
-      robot::log_warning("[recovery_core] Invalid backup_distance for '%s'; using 0.5 m.",
-                         name_.c_str());
-      default_backup_distance_ = kDefaultBackupDistance;
+      robot::log_warning("[recovery_core] '%s': backup_distance_max=%.3f m is invalid; using %.3f "
+                         "m.", name().c_str(), backup_distance_max_,
+                         kDefaultBackupDistanceMax);
+      backup_distance_max_ = kDefaultBackupDistanceMax;
    }
-    if (!std::isfinite(default_linear_speed_) || default_linear_speed_ <= 0.0)
+
+    default_backup_distance_ = clampDistance(default_backup_distance_, kDefaultBackupDistance);
+    default_linear_speed_ = clampSpeed(default_linear_speed_, kDefaultLinearSpeed);
+
+    if (!std::isfinite(acc_lim_x_) || acc_lim_x_ < 0.0)
    {
-      robot::log_warning("[recovery_core] Invalid linear_speed for '%s'; using 0.1 m/s.",
-                         name_.c_str());
-      default_linear_speed_ = kDefaultLinearSpeed;
-    }
-    if (!std::isfinite(control_period_) || control_period_ <= 0.0)
-    {
-      robot::log_warning("[recovery_core] Invalid control_period for '%s'; using 0.1 s.",
-                         name_.c_str());
-      control_period_ = kDefaultControlPeriod;
+      robot::log_warning("[recovery_core] '%s': acc_lim_x=%.3f m/s^2 is invalid; using %.3f.",
+                         name().c_str(), acc_lim_x_, kDefaultAccLimX);
+      acc_lim_x_ = kDefaultAccLimX;
    }
+
+    return true;
  }

-  recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override
+  bool onStart(const recovery_core::RecoveryGoal& goal) override
  {
-    // Costmap là bắt buộc? fail sớm trước khi xuất vận tốc lùi.
-    if (require_costmap_ && ctx().local_costmap == nullptr)
+    // Base đã bảo đảm ctx().pose và ctx().collision khác null cho họ velocity.
+    if (!ctx().pose->getRobotPose(start_pose_))
    {
-      return recovery_core::RecoveryResult::Failed().withMessage("backup requires local costmap");
+      robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
+                         name().c_str());
+      return false;
    }

-    backup_distance_ = (std::isfinite(goal.distance) && goal.distance > 0.0)
-                           ? goal.distance
-                           : default_backup_distance_;
+    start_yaw_ = recovery_core::yawOf(start_pose_);

-    linear_speed_ = std::abs(goal.param("linear_speed", default_linear_speed_));
-    if (!std::isfinite(linear_speed_) || linear_speed_ <= 0.0)
+    backup_distance_ = clampDistance(goal.distance.value_or(default_backup_distance_),
+                                     default_backup_distance_);
+    linear_speed_ = clampSpeed(std::abs(goal.param("linear_speed", default_linear_speed_)),
+                               default_linear_speed_);
+
+    current_speed_ = 0.0;
+
+    // Kiểm ngay tại chỗ: nếu vị trí lùi đầu tiên đã bị chặn thì từ chối khởi động, để state machine
+    // chuyển sang behavior kế tiếp thay vì phát một lệnh lùi rồi mới hỏng.
+    if (blockedAhead(start_pose_, kProbeDistance))
    {
-      linear_speed_ = default_linear_speed_;
+      robot::log_warning("[recovery_core] '%s': the space behind is already blocked, refusing to "
+                         "back up.",
+                         name().c_str());
+      return false;
    }

-    traveled_distance_ = 0.0;
-    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
-                                                   recovery_core::RecoveryStatus::kRunning)
-        .withProgress(0.0, backup_distance_)
-        .withMessage("backup start");
+    return true;
  }

-  recovery_core::RecoveryResult onUpdate() override
+  recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
  {
-    if (traveled_distance_ >= backup_distance_)
+    robot_geometry_msgs::PoseStamped pose;
+    if (!ctx().pose->getRobotPose(pose))
    {
-      return succeeded();
+      current_speed_ = 0.0;
+      return stopResult(recovery_core::RecoveryStatus::kFailed)
+          .withMessage("robot pose lost (TF stale?) — stopping the back-up");
    }

+    // Tiến độ = phần đi ngược hướng xuất phát. Dấu âm của hình chiếu chính là quãng đã lùi.
+    const double traveled = -recovery_core::projectOntoHeading(pose, start_pose_, start_yaw_);
+    const double remaining = backup_distance_ - traveled;
+
+    if (remaining <= kGoalTolerance)
+    {
+      current_speed_ = 0.0;
+      return stopResult(recovery_core::RecoveryStatus::kSucceeded)
+          .withProgress(1.0, 0.0)
+          .withMessage("backup complete");
+    }
+
+    // Vận tốc của tick này, đã ramp theo trần gia tốc, và không vượt quãng còn lại nếu dt cho phép.
+    double speed = recovery_core::rampToward(linear_speed_, current_speed_, acc_lim_x_, dt);
+    if (dt > 0.0)
+    {
+      speed = std::min(speed, remaining / dt);
+    }
+    speed = std::max(speed, 0.0);
+
+    // Pose dự đoán ở CUỐI chu kỳ tới — kiểm trước khi phát lệnh, không phải sau.
+    const double probe = std::max(speed * std::max(dt, kMinProbeDt), kProbeDistance);
+    if (blockedAhead(pose, probe))
+    {
+      current_speed_ = 0.0;
+      return stopResult(recovery_core::RecoveryStatus::kFailed)
+          .withMessage("obstacle behind — cancelling the back-up");
+    }
+
+    current_speed_ = speed;
+
    robot_geometry_msgs::Twist command;
-    command.linear.x = -std::abs(linear_speed_);
-    traveled_distance_ = std::min(
-        backup_distance_, traveled_distance_ + std::abs(command.linear.x) * control_period_);
-
-    if (traveled_distance_ >= backup_distance_)
-    {
-      return succeeded();
-    }
+    command.linear.x = -speed;  // [m/s], âm = lùi

    return recovery_core::RecoveryResult::Velocity(command,
                                                   recovery_core::RecoveryStatus::kRunning)
-        .withProgress(traveled_distance_ / backup_distance_,
-                      backup_distance_ - traveled_distance_)
+        .withProgress(traveled / backup_distance_, remaining)
        .withMessage("backing up");
  }

-private:
-  recovery_core::RecoveryResult succeeded()
+  recovery_core::RecoveryResult onCancel() override
  {
-    return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
-                                                   recovery_core::RecoveryStatus::kSucceeded)
-        .withProgress(1.0, 0.0)
-        .withMessage("backup complete");
+    current_speed_ = 0.0;
+    return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("backup cancelled");
  }

-  double default_backup_distance_ = kDefaultBackupDistance;
-  double default_linear_speed_ = kDefaultLinearSpeed;
-  double control_period_ = kDefaultControlPeriod;
-  bool require_costmap_ = false;
+private:
+  /// Khoảng dò tối thiểu [m] — luôn nhìn trước ít nhất một ô costmap kể cả khi dt rất nhỏ.
+  static constexpr double kProbeDistance = 0.05;
+  /// dt tối thiểu [s] dùng khi dự đoán, để tick đầu (dt = 0) vẫn dò về phía trước.
+  static constexpr double kMinProbeDt = 0.1;

-  double backup_distance_ = kDefaultBackupDistance;
-  double linear_speed_ = kDefaultLinearSpeed;
-  double traveled_distance_ = 0.0;
+  /// @return true nếu đặt robot lùi thêm @p distance từ @p from là va chạm.
+  bool blockedAhead(const robot_geometry_msgs::PoseStamped& from, double distance) const
+  {
+    const double next_x = from.pose.position.x - std::cos(start_yaw_) * distance;
+    const double next_y = from.pose.position.y - std::sin(start_yaw_) * distance;
+    return ctx().collision->footprintCost(next_x, next_y, start_yaw_) < 0.0;
+  }
+
+  double clampDistance(double value, double fallback) const
+  {
+    if (!std::isfinite(value) || value <= 0.0)
+    {
+      robot::log_warning("[recovery_core] '%s': backup_distance=%.3f m is invalid; using %.3f m.",
+                         name().c_str(), value, fallback);
+      return std::min(fallback, backup_distance_max_);
+    }
+    if (value > backup_distance_max_)
+    {
+      robot::log_warning("[recovery_core] '%s': backup_distance=%.3f m > limit %.3f m; clamped.",
+                         name().c_str(), value, backup_distance_max_);
+      return backup_distance_max_;
+    }
+    return value;
+  }
+
+  double clampSpeed(double value, double fallback) const
+  {
+    if (!std::isfinite(value) || value <= 0.0)
+    {
+      robot::log_warning("[recovery_core] '%s': linear_speed=%.3f m/s is invalid; using %.3f m/s.",
+                         name().c_str(), value, fallback);
+      return fallback;
+    }
+    if (value > kMaxLinearSpeed)
+    {
+      robot::log_warning("[recovery_core] '%s': linear_speed=%.3f m/s > limit %.3f m/s; clamped.",
+                         name().c_str(), value, kMaxLinearSpeed);
+      return kMaxLinearSpeed;
+    }
+    return value;
+  }
+
+  // Config
+  double default_backup_distance_ = kDefaultBackupDistance;    ///< [m]
+  double backup_distance_max_ = kDefaultBackupDistanceMax;     ///< [m]
+  double default_linear_speed_ = kDefaultLinearSpeed;          ///< [m/s]
+  double acc_lim_x_ = kDefaultAccLimX;                         ///< [m/s^2]
+
+  // Trạng thái lượt hiện tại
+  robot_geometry_msgs::PoseStamped start_pose_;
+  double start_yaw_ = 0.0;                                     ///< [rad]
+  double backup_distance_ = kDefaultBackupDistance;            ///< [m]
+  double linear_speed_ = kDefaultLinearSpeed;                  ///< [m/s]
+  double current_speed_ = 0.0;                                 ///< [m/s] đang phát, để ramp
 };

 }  // namespace recovery_plugins