246 lines
9.7 KiB
C++
246 lines
9.7 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* 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>
|
|
#include <string>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
#include <robot/robot.h>
|
|
|
|
namespace recovery_plugins
|
|
{
|
|
namespace
|
|
{
|
|
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 theo hướng ban đầu tới khi đủ quãng yêu cầu.
|
|
*
|
|
* Đâ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
|
|
{
|
|
public:
|
|
BackUpRecovery() = default;
|
|
|
|
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
|
{
|
|
return std::make_shared<BackUpRecovery>();
|
|
}
|
|
|
|
recovery_core::RecoveryOutputType outputKind() const override
|
|
{
|
|
return recovery_core::RecoveryOutputType::kVelocity;
|
|
}
|
|
|
|
protected:
|
|
bool onConfigure(robot::NodeHandle& nh) override
|
|
{
|
|
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(backup_distance_max_) || backup_distance_max_ <= 0.0)
|
|
{
|
|
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;
|
|
}
|
|
|
|
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] '%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;
|
|
}
|
|
|
|
bool onStart(const recovery_core::RecoveryGoal& goal) override
|
|
{
|
|
// Base đã bảo đảm ctx().pose và ctx().collision khác null cho họ velocity.
|
|
if (!ctx().pose->getRobotPose(start_pose_))
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
|
|
name().c_str());
|
|
return false;
|
|
}
|
|
|
|
start_yaw_ = recovery_core::yawOf(start_pose_);
|
|
|
|
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))
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': the space behind is already blocked, refusing to "
|
|
"back up.",
|
|
name().c_str());
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
|
|
{
|
|
robot_geometry_msgs::PoseStamped pose;
|
|
if (!ctx().pose->getRobotPose(pose))
|
|
{
|
|
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 = -speed; // [m/s], âm = lùi
|
|
|
|
return recovery_core::RecoveryResult::Velocity(command,
|
|
recovery_core::RecoveryStatus::kRunning)
|
|
.withProgress(traveled / backup_distance_, remaining)
|
|
.withMessage("backing up");
|
|
}
|
|
|
|
recovery_core::RecoveryResult onCancel() override
|
|
{
|
|
current_speed_ = 0.0;
|
|
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("backup cancelled");
|
|
}
|
|
|
|
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;
|
|
|
|
/// @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
|
|
|
|
BOOST_DLL_ALIAS(recovery_plugins::BackUpRecovery::create, BackUpRecovery)
|