279 lines
10 KiB
C++
279 lines
10 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* recovery_core — quay tại chỗ, quét cung trước khi quay và đo bằng pose thật.
|
|
*
|
|
* 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 kTwoPi = 2.0 * M_PI;
|
|
constexpr double kDefaultAngularSpeed = 0.4; // [rad/s] độ lớn
|
|
constexpr double kDefaultAccLimTheta = 0.8; // [rad/s^2]
|
|
constexpr double kDefaultSimGranularity = 0.1; // [rad] bước quét cung
|
|
constexpr double kMaxAngularSpeed = 2.0; // [rad/s] trần vệ sinh cho param sai
|
|
constexpr double kGoalTolerance = 1e-3; // [rad]
|
|
} // namespace
|
|
|
|
/**
|
|
* @class RotateRecovery
|
|
* @brief Quay tại chỗ, mặc định đủ một vòng, để costmap quan sát lại xung quanh.
|
|
*
|
|
* Quay đủ 2π là công dụng chính của rotate trong một bộ recovery: nó cho obstacle/voxel layer nhìn
|
|
* thấy toàn bộ vùng quanh robot rồi mới lập plan lại. Đặt `full_rotation: false` hoặc truyền
|
|
* `goal.angle` để quay một góc cụ thể.
|
|
*
|
|
* Hai lớp bảo vệ so với bản trước (bản trước không dùng `ctx()` một lần nào):
|
|
* 1. **Quét toàn bộ cung sẽ quay** tại `onStart()` theo bước `sim_granularity`; chạm vật cản ở bất
|
|
* kỳ góc nào là từ chối khởi động, chứ không quay tới nơi mới phát hiện.
|
|
* 2. **Tiến độ đo bằng pose thật**, cộng dồn từng chênh lệch yaw đã chuẩn hoá — nên quay > π vẫn
|
|
* đếm đúng, và loop chạy chậm không làm robot quay quá góc.
|
|
*
|
|
* @note Yêu cầu robot xoay tại chỗ được (differential/omni). Config workspace hiện tại thoả:
|
|
* `min_turn_radius: 0.0`, `use_rotate_to_heading: true`.
|
|
*/
|
|
class RotateRecovery final : public recovery_core::RecoveryBehavior
|
|
{
|
|
public:
|
|
RotateRecovery() = default;
|
|
|
|
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
|
{
|
|
return std::make_shared<RotateRecovery>();
|
|
}
|
|
|
|
recovery_core::RecoveryOutputType outputKind() const override
|
|
{
|
|
return recovery_core::RecoveryOutputType::kVelocity;
|
|
}
|
|
|
|
protected:
|
|
bool onConfigure(robot::NodeHandle& nh) override
|
|
{
|
|
nh.param("full_rotation", full_rotation_, true);
|
|
nh.param("target_angle", default_target_angle_, kTwoPi);
|
|
nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);
|
|
nh.param("acc_lim_theta", acc_lim_theta_, kDefaultAccLimTheta);
|
|
nh.param("sim_granularity", sim_granularity_, kDefaultSimGranularity);
|
|
|
|
default_target_angle_ = clampAngle(default_target_angle_, kTwoPi);
|
|
default_angular_speed_ = clampSpeed(default_angular_speed_, kDefaultAngularSpeed);
|
|
|
|
if (!std::isfinite(acc_lim_theta_) || acc_lim_theta_ < 0.0)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': acc_lim_theta=%.3f rad/s^2 is invalid; using %.3f.",
|
|
name().c_str(), acc_lim_theta_, kDefaultAccLimTheta);
|
|
acc_lim_theta_ = kDefaultAccLimTheta;
|
|
}
|
|
|
|
if (!std::isfinite(sim_granularity_) || sim_granularity_ <= 0.0)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': sim_granularity=%.3f rad is invalid; using %.3f "
|
|
"rad.", name().c_str(), sim_granularity_, kDefaultSimGranularity);
|
|
sim_granularity_ = kDefaultSimGranularity;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool onStart(const recovery_core::RecoveryGoal& goal) override
|
|
{
|
|
robot_geometry_msgs::PoseStamped pose;
|
|
if (!ctx().pose->getRobotPose(pose))
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
|
|
name().c_str());
|
|
return false;
|
|
}
|
|
|
|
start_pose_ = pose;
|
|
start_yaw_ = recovery_core::yawOf(pose);
|
|
last_yaw_ = start_yaw_;
|
|
rotated_ = 0.0;
|
|
current_speed_ = 0.0;
|
|
|
|
// goal.angle có giá trị (kể cả 0.0) thì tôn trọng đúng giá trị đó. Bản trước coi 0 là "chưa
|
|
// đặt" nên một góc tính ra ~0 bị âm thầm thay bằng pi/2.
|
|
const double requested = goal.angle.value_or(full_rotation_ ? kTwoPi : default_target_angle_);
|
|
target_angle_ = clampAngle(requested, default_target_angle_);
|
|
angular_speed_ = clampSpeed(std::abs(goal.param("angular_speed", default_angular_speed_)),
|
|
default_angular_speed_);
|
|
|
|
if (std::abs(target_angle_) <= kGoalTolerance)
|
|
{
|
|
// Caller nói rõ "đừng quay". Đó là một yêu cầu hợp lệ và hoàn tất ngay.
|
|
zero_rotation_ = true;
|
|
return true;
|
|
}
|
|
zero_rotation_ = false;
|
|
|
|
if (!arcIsClear())
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': the %.3f rad arc is blocked, refusing to rotate.",
|
|
name().c_str(), target_angle_);
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
|
|
{
|
|
const double target = std::abs(target_angle_);
|
|
|
|
if (zero_rotation_)
|
|
{
|
|
return stopResult(recovery_core::RecoveryStatus::kSucceeded)
|
|
.withProgress(1.0, 0.0)
|
|
.withMessage("rotate 0 rad — nothing to rotate");
|
|
}
|
|
|
|
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 rotation");
|
|
}
|
|
|
|
const double yaw = recovery_core::yawOf(pose);
|
|
|
|
// Cộng dồn từng bước đã chuẩn hoá: cách duy nhất đếm đúng khi tổng góc quay vượt pi.
|
|
rotated_ += std::abs(recovery_core::normalizeAngle(yaw - last_yaw_));
|
|
last_yaw_ = yaw;
|
|
|
|
const double remaining = target - rotated_;
|
|
if (remaining <= kGoalTolerance)
|
|
{
|
|
current_speed_ = 0.0;
|
|
return stopResult(recovery_core::RecoveryStatus::kSucceeded)
|
|
.withProgress(1.0, 0.0)
|
|
.withMessage("rotate complete");
|
|
}
|
|
|
|
double speed = recovery_core::rampToward(angular_speed_, current_speed_, acc_lim_theta_, dt);
|
|
if (dt > 0.0)
|
|
{
|
|
speed = std::min(speed, remaining / dt);
|
|
}
|
|
speed = std::max(speed, 0.0);
|
|
|
|
// Cung đã quét ở onStart(), nhưng costmap đổi giữa chừng thì phải phát hiện: kiểm góc dự đoán
|
|
// ở cuối chu kỳ tới trước khi phát lệnh.
|
|
const double direction = target_angle_ >= 0.0 ? 1.0 : -1.0;
|
|
const double next_yaw = yaw + direction * std::max(speed * dt, sim_granularity_);
|
|
if (ctx().collision->footprintCost(pose.pose.position.x, pose.pose.position.y, next_yaw) < 0.0)
|
|
{
|
|
current_speed_ = 0.0;
|
|
return stopResult(recovery_core::RecoveryStatus::kFailed)
|
|
.withMessage("the rotation arc became blocked midway — stopping the rotation");
|
|
}
|
|
|
|
current_speed_ = speed;
|
|
|
|
robot_geometry_msgs::Twist command;
|
|
command.angular.z = std::copysign(speed, target_angle_); // [rad/s], + = ngược chiều kim đồng hồ
|
|
|
|
return recovery_core::RecoveryResult::Velocity(command,
|
|
recovery_core::RecoveryStatus::kRunning)
|
|
.withProgress(rotated_ / target, remaining)
|
|
.withMessage("rotating");
|
|
}
|
|
|
|
recovery_core::RecoveryResult onCancel() override
|
|
{
|
|
current_speed_ = 0.0;
|
|
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("rotate cancelled");
|
|
}
|
|
|
|
private:
|
|
/// @return true nếu toàn bộ cung sẽ quay đều đặt được footprint.
|
|
bool arcIsClear() const
|
|
{
|
|
const double target = std::abs(target_angle_);
|
|
const double direction = target_angle_ >= 0.0 ? 1.0 : -1.0;
|
|
const double x = start_pose_.pose.position.x;
|
|
const double y = start_pose_.pose.position.y;
|
|
|
|
for (double swept = 0.0; swept < target; swept += sim_granularity_)
|
|
{
|
|
if (ctx().collision->footprintCost(x, y, start_yaw_ + direction * swept) < 0.0)
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Kiểm luôn góc cuối: vòng lặp trên dừng trước target nếu target không chia hết cho bước quét.
|
|
return ctx().collision->footprintCost(x, y, start_yaw_ + direction * target) >= 0.0;
|
|
}
|
|
|
|
double clampAngle(double value, double fallback) const
|
|
{
|
|
if (!std::isfinite(value))
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': target_angle is not finite; using %.3f rad.",
|
|
name().c_str(), fallback);
|
|
return fallback;
|
|
}
|
|
if (std::abs(value) > kTwoPi)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': target_angle=%.3f rad exceeds +/-2pi; clamped.",
|
|
name().c_str(), value);
|
|
return std::copysign(kTwoPi, value);
|
|
}
|
|
return value;
|
|
}
|
|
|
|
double clampSpeed(double value, double fallback) const
|
|
{
|
|
if (!std::isfinite(value) || value <= 0.0)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': angular_speed=%.3f rad/s is invalid; using %.3f "
|
|
"rad/s.", name().c_str(), value, fallback);
|
|
return fallback;
|
|
}
|
|
if (value > kMaxAngularSpeed)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': angular_speed=%.3f rad/s > limit %.3f; clamped.",
|
|
name().c_str(), value, kMaxAngularSpeed);
|
|
return kMaxAngularSpeed;
|
|
}
|
|
return value;
|
|
}
|
|
|
|
// Config
|
|
bool full_rotation_ = true;
|
|
double default_target_angle_ = kTwoPi; ///< [rad]
|
|
double default_angular_speed_ = kDefaultAngularSpeed; ///< [rad/s]
|
|
double acc_lim_theta_ = kDefaultAccLimTheta; ///< [rad/s^2]
|
|
double sim_granularity_ = kDefaultSimGranularity; ///< [rad]
|
|
|
|
// Trạng thái lượt hiện tại
|
|
robot_geometry_msgs::PoseStamped start_pose_;
|
|
double start_yaw_ = 0.0; ///< [rad]
|
|
double last_yaw_ = 0.0; ///< [rad]
|
|
double rotated_ = 0.0; ///< [rad] cộng dồn, luôn >= 0
|
|
double target_angle_ = kTwoPi; ///< [rad] có dấu
|
|
double angular_speed_ = kDefaultAngularSpeed; ///< [rad/s] độ lớn
|
|
double current_speed_ = 0.0; ///< [rad/s] đang phát, để ramp
|
|
bool zero_rotation_ = false; ///< caller yêu cầu góc 0
|
|
};
|
|
|
|
} // namespace recovery_plugins
|
|
|
|
BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, RotateRecovery)
|