140 lines
4.8 KiB
C++
140 lines
4.8 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* recovery_core — per-cycle rotate recovery plugin (goal-driven).
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
|
|
#include <recovery_core/recovery_behavior.h>
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <string>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
#include <robot/robot.h>
|
|
|
|
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.
|
|
} // namespace
|
|
|
|
/**
|
|
* @class RotateRecovery
|
|
* @brief Quay tại chỗ tới GÓC ĐÍCH do caller yêu cầu ở start(goal).
|
|
*
|
|
* 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).
|
|
*/
|
|
class RotateRecovery final : public recovery_core::RecoveryBehavior
|
|
{
|
|
public:
|
|
RotateRecovery() = default;
|
|
|
|
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
|
{
|
|
return std::make_shared<RotateRecovery>();
|
|
}
|
|
|
|
protected:
|
|
void onConfigure() 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);
|
|
|
|
if (!std::isfinite(default_target_angle_) || std::abs(default_target_angle_) <= 0.0)
|
|
{
|
|
robot::log_warning("[recovery_core] Invalid target_angle for '%s'; using pi/2.",
|
|
name_.c_str());
|
|
default_target_angle_ = kDefaultTargetAngle;
|
|
}
|
|
if (!std::isfinite(default_angular_speed_) || default_angular_speed_ <= 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;
|
|
}
|
|
}
|
|
|
|
recovery_core::RecoveryResult 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)
|
|
{
|
|
angular_speed_ = default_angular_speed_;
|
|
}
|
|
|
|
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");
|
|
}
|
|
|
|
recovery_core::RecoveryResult onUpdate() override
|
|
{
|
|
const double target = std::abs(target_angle_);
|
|
|
|
if (rotated_angle_ >= target)
|
|
{
|
|
return succeeded(target);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
return recovery_core::RecoveryResult::Velocity(command,
|
|
recovery_core::RecoveryStatus::kRunning)
|
|
.withProgress(rotated_angle_ / target, target - rotated_angle_)
|
|
.withMessage("rotating");
|
|
}
|
|
|
|
private:
|
|
recovery_core::RecoveryResult succeeded(double target)
|
|
{
|
|
return recovery_core::RecoveryResult::Velocity(robot_geometry_msgs::Twist(),
|
|
recovery_core::RecoveryStatus::kSucceeded)
|
|
.withProgress(1.0, 0.0)
|
|
.withMessage("rotate complete");
|
|
}
|
|
|
|
double default_target_angle_ = kDefaultTargetAngle;
|
|
double default_angular_speed_ = kDefaultAngularSpeed;
|
|
double control_period_ = kDefaultControlPeriod;
|
|
|
|
double target_angle_ = kDefaultTargetAngle;
|
|
double angular_speed_ = kDefaultAngularSpeed;
|
|
double rotated_angle_ = 0.0;
|
|
};
|
|
|
|
} // namespace recovery_plugins
|
|
|
|
BOOST_DLL_ALIAS(recovery_plugins::RotateRecovery::create, RotateRecovery)
|