DuongTD/recovery_core
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

optimal & fix file cmake

Browse Source
This commit is contained in:
duongtd
2026-08-03 22:32:40 +07:00
parent 89add78c7f
commit 887bff1b97
98 changed files with 8971 additions and 1339 deletions
Download Patch File Download Diff File Expand all files Collapse all files

269
plugins/rotate_recovery.cpp
View File

@@ -2,12 +2,13 @@
*
* Software License Agreement (BSD License)
*
* recovery_core — per-cycle rotate recovery plugin (goal-driven).
* 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>
@@ -20,18 +21,30 @@ namespace recovery_plugins
{
namespace
{
constexpr double kDefaultTargetAngle = 1.57079632679; // pi/2 rad.
constexpr double kDefaultAngularSpeed = 0.4; // rad/s.
constexpr double kDefaultControlPeriod = 0.1; // s per update tick.
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ỗ tới GÓC ĐÍCH do caller yêu cầu ở start(goal).
* @brief Quay tại chỗ, mặc định đủ một vòng, để costmap quan sát lại xung quanh.
*
* goal.angle (rad, có dấu) là góc quay lượt này; 0 nghĩa là dùng default configured. Tốc độ
* góc mặc định đọc từ param, có thể override qua goal.params["angular_speed"]. Mỗi update() trả
* Twist.angular.z kèm progress/remaining tới khi đủ góc -> kSucceeded (zero command).
* 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
{
@@ -43,95 +56,221 @@ public:
return std::make_shared<RotateRecovery>();
}
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("target_angle", default_target_angle_, kDefaultTargetAngle);
private_nh.param("angular_speed", default_angular_speed_, kDefaultAngularSpeed);
private_nh.param("control_period", control_period_, kDefaultControlPeriod);
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);
if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0)
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] Invalid target_angle for '%s'; using pi/2.",
name_.c_str());
default_target_angle_ = kDefaultTargetAngle;
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(default_angular_speed_) || default_angular_speed_ <= 0.0)
if (!std::isfinite(sim_granularity_) || sim_granularity_ <= 0.0)
{
robot::log_warning("[recovery_core] Invalid angular_speed for '%s'; using 0.4 rad/s.",
name_.c_str());
default_angular_speed_ = kDefaultAngularSpeed;
}
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': sim_granularity=%.3f rad is invalid; using %.3f "
"rad.", name().c_str(), sim_granularity_, kDefaultSimGranularity);
sim_granularity_ = kDefaultSimGranularity;
}
return true;
}
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& goal) override
bool onStart(const recovery_core::RecoveryGoal& goal) override
{
// Góc đích: goal.angle nếu hợp lệ, ngược lại default configured.
target_angle_ = (std::isfinite(goal.angle) && std::abs(goal.angle) > 0.0)
? goal.angle
: default_target_angle_;
angular_speed_ = std::abs(goal.param("angular_speed", default_angular_speed_));
if (!std::isfinite(angular_speed_) || angular_speed_ <= 0.0)
robot_geometry_msgs::PoseStamped pose;
if (!ctx().pose->getRobotPose(pose))
{
angular_speed_ = default_angular_speed_;
robot::log_warning("[recovery_core] '%s': could not get a pose at start-up (TF stale?).",
name().c_str());
return false;
}
rotated_angle_ = 0.0;
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kRunning)
.withProgress(0.0, std::abs(target_angle_))
.withMessage("rotate start");
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() override
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double dt) override
{
const double target = std::abs(target_angle_);
if (rotated_angle_ >= target)
if (zero_rotation_)
{
return succeeded(target);
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(angular_speed_, target_angle_);
rotated_angle_ =
std::min(target, rotated_angle_ + std::abs(command.angular.z) * control_period_);
if (rotated_angle_ >= target)
{
return succeeded(target);
}
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_angle_ / target, target - rotated_angle_)
.withProgress(rotated_ / target, remaining)
.withMessage("rotating");
}
private:
recovery_core::RecoveryResult succeeded(double target)
recovery_core::RecoveryResult onCancel() override
{
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
recovery_core::RecoveryStatus::kSucceeded)
.withProgress(1.0, 0.0)
.withMessage("rotate complete");
current_speed_ = 0.0;
return stopResult(recovery_core::RecoveryStatus::kCancelled).withMessage("rotate cancelled");
}
double default_target_angle_ = kDefaultTargetAngle;
double default_angular_speed_ = kDefaultAngularSpeed;
double control_period_ = kDefaultControlPeriod;
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;
double target_angle_ = kDefaultTargetAngle;
double angular_speed_ = kDefaultAngularSpeed;
double rotated_angle_ = 0.0;
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