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

temporary storage 7/9/2026 19:41

Browse Source
This commit is contained in:
duongtd
2026-07-09 19:41:20 +07:00
parent 915cf85cc5
commit e9394434ba
17 changed files with 658 additions and 566 deletions
Download Patch File Download Diff File Expand all files Collapse all files

72
plugins/clear_costmap_recovery.cpp
View File

@@ -46,24 +46,14 @@ class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
public:
ClearCostmapRecovery() = default;
void initialize(std::string name, tf3::BufferCore* tf,
std::vector<robot_geometry_msgs::PoseStamped>* global_path,
robot_costmap_2d::Costmap2DROBOT* global_costmap,
robot_costmap_2d::Costmap2DROBOT* local_costmap) override
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
if (initialized_)
{
robot::log_error("[recovery_core] ClearCostmapRecovery '%s' initialized twice; ignoring.",
name_.c_str());
return;
}
name_ = std::move(name);
tf_ = tf;
global_path_ = global_path;
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
return std::make_shared<ClearCostmapRecovery>();
}
protected:
void onConfigure() override
{
robot::NodeHandle private_nh("~/" + name_);
private_nh.param("reset_distance", reset_distance_, 3.0);
private_nh.param("invert_area_to_clear", invert_area_to_clear_, false);
@@ -89,52 +79,37 @@ public:
std::vector<std::string> clearable_layers;
private_nh.param("layer_names", clearable_layers, clearable_layers_default);
clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());
initialized_ = true;
status_ = recovery_core::RecoveryStatus::kIdle;
}
recovery_core::RecoveryResult runBehavior() override
recovery_core::RecoveryResult onStart(const recovery_core::RecoveryGoal& /*goal*/) override
{
if (!initialized_)
{
status_ = recovery_core::RecoveryStatus::kFailed;
return recovery_core::RecoveryResult::Failed();
}
// One-shot: công việc thực hiện ở onUpdate() lần đầu.
return recovery_core::RecoveryResult::Running().withMessage("clear costmap start");
}
recovery_core::RecoveryResult onUpdate(double /*dt*/) override
{
bool ok = true;
if (affected_maps_ == "global" || affected_maps_ == "both")
{
ok = clear(global_costmap_) && ok;
if (ok && force_updating_ && global_costmap_ != nullptr)
ok = clear(ctx().global_costmap) && ok;
if (ok && force_updating_ && ctx().global_costmap != nullptr)
{
global_costmap_->updateMap();
ctx().global_costmap->updateMap();
}
}
if (affected_maps_ == "local" || affected_maps_ == "both")
{
ok = clear(local_costmap_) && ok;
if (ok && force_updating_ && local_costmap_ != nullptr)
ok = clear(ctx().local_costmap) && ok;
if (ok && force_updating_ && ctx().local_costmap != nullptr)
{
local_costmap_->updateMap();
ctx().local_costmap->updateMap();
}
}
status_ = ok ? recovery_core::RecoveryStatus::kSucceeded :
recovery_core::RecoveryStatus::kFailed;
return ok ? recovery_core::RecoveryResult::Succeeded() :
recovery_core::RecoveryResult::Failed();
}
recovery_core::RecoveryStatus status() const override
{
return status_;
}
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
{
return std::make_shared<ClearCostmapRecovery>();
return ok ? recovery_core::RecoveryResult::Succeeded().withMessage("clear costmap complete")
: recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed");
}
private:
@@ -216,13 +191,6 @@ private:
costmap->getOriginY() + costmap->getSizeInMetersY());
}
tf3::BufferCore* tf_ = nullptr;
std::vector<robot_geometry_msgs::PoseStamped>* global_path_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* global_costmap_ = nullptr;
robot_costmap_2d::Costmap2DROBOT* local_costmap_ = nullptr;
bool initialized_ = false;
recovery_core::RecoveryStatus status_ = recovery_core::RecoveryStatus::kIdle;
bool force_updating_ = false;
double reset_distance_ = 3.0;
bool invert_area_to_clear_ = false;