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

temporary storage 7/9/2026 20:41

Browse Source
This commit is contained in:
duongtd
2026-07-09 20:41:35 +07:00
parent e9394434ba
commit 17088a227d
12 changed files with 82 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
plugins/rotate_recovery.cpp
View File

@@ -22,6 +22,7 @@ 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
/**
@@ -48,6 +49,7 @@ protected:
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)
{
@@ -61,6 +63,12 @@ protected:
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
@@ -83,7 +91,7 @@ protected:
.withMessage("rotate start");
}
recovery_core::RecoveryResult onUpdate(double dt) override
recovery_core::RecoveryResult onUpdate() override
{
const double target = std::abs(target_angle_);
@@ -94,7 +102,8 @@ protected:
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) * dt);
rotated_angle_ =
std::min(target, rotated_angle_ + std::abs(command.angular.z) * control_period_);
if (rotated_angle_ >= target)
{
@@ -118,6 +127,7 @@ private:
double default_target_angle_ = kDefaultTargetAngle;
double default_angular_speed_ = kDefaultAngularSpeed;
double control_period_ = kDefaultControlPeriod;
double target_angle_ = kDefaultTargetAngle;
double angular_speed_ = kDefaultAngularSpeed;