262 lines
8.9 KiB
C++
262 lines
8.9 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* recovery_core — xoá vật cản đã tích trong costmap. Một tick, không output.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
|
|
#include <recovery_core/recovery_behavior.h>
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <set>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
#include <boost/pointer_cast.hpp>
|
|
#include <boost/thread/locks.hpp>
|
|
#include <robot/robot.h>
|
|
#include <robot_costmap_2d/costmap_2d_robot.h>
|
|
#include <robot_costmap_2d/costmap_layer.h>
|
|
|
|
namespace recovery_plugins
|
|
{
|
|
namespace
|
|
{
|
|
constexpr double kDefaultResetDistance = 3.0; // [m]
|
|
constexpr double kMaxResetDistance = 100.0; // [m] trần vệ sinh cho param sai
|
|
|
|
std::string leafName(std::string name)
|
|
{
|
|
const std::string::size_type slash = name.rfind('/');
|
|
if (slash != std::string::npos)
|
|
{
|
|
name = name.substr(slash + 1);
|
|
}
|
|
return name;
|
|
}
|
|
} // namespace
|
|
|
|
/**
|
|
* @class ClearCostmapRecovery
|
|
* @brief Xoá vùng vật cản đã tích trong các layer được chỉ định.
|
|
*
|
|
* Dùng **hai instance** trong bộ default:
|
|
* - `conservative_reset` (`invert_area_to_clear: false`) xoá vùng gần robot;
|
|
* - `aggressive_reset` (`invert_area_to_clear: true`) xoá mọi thứ **ngoài** vùng đó.
|
|
*
|
|
* Không phát output, hoàn tất trong một tick.
|
|
*
|
|
* @note Cố ý **không** gọi `Costmap2DROBOT::updateMap()`. Hàm đó giữ mutex master rồi chạy toàn bộ
|
|
* chuỗi layer `updateBounds`/`updateCosts` — cỡ chục tới trăm ms — trong khi behavior này
|
|
* chạy trên thread phát cmd_vel ở 30 Hz. Costmap tự update ở chu kỳ riêng của nó ngay sau
|
|
* đó; ép update tại đây chỉ để đổi lấy một chu kỳ control bị lỡ.
|
|
*/
|
|
class ClearCostmapRecovery final : public recovery_core::RecoveryBehavior
|
|
{
|
|
public:
|
|
ClearCostmapRecovery() = default;
|
|
|
|
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
|
{
|
|
return std::make_shared<ClearCostmapRecovery>();
|
|
}
|
|
|
|
recovery_core::RecoveryOutputType outputKind() const override
|
|
{
|
|
return recovery_core::RecoveryOutputType::kNone;
|
|
}
|
|
|
|
protected:
|
|
bool onConfigure(robot::NodeHandle& nh) override
|
|
{
|
|
nh.param("reset_distance", reset_distance_, kDefaultResetDistance);
|
|
nh.param("invert_area_to_clear", invert_area_to_clear_, false);
|
|
nh.param("affected_maps", affected_maps_, std::string("both"));
|
|
|
|
if (!std::isfinite(reset_distance_) || reset_distance_ <= 0.0 ||
|
|
reset_distance_ > kMaxResetDistance)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': reset_distance=%.3f m outside (0, %.0f]; using "
|
|
"%.3f m.", name().c_str(), reset_distance_, kMaxResetDistance,
|
|
kDefaultResetDistance);
|
|
reset_distance_ = kDefaultResetDistance;
|
|
}
|
|
|
|
if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both")
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': affected_maps='%s' is invalid; using 'both'.",
|
|
name().c_str(), affected_maps_.c_str());
|
|
affected_maps_ = "both";
|
|
}
|
|
|
|
std::vector<std::string> clearable_layers_default{"obstacles"};
|
|
std::vector<std::string> clearable_layers;
|
|
nh.param("layer_names", clearable_layers, clearable_layers_default);
|
|
clearable_layers_.insert(clearable_layers.begin(), clearable_layers.end());
|
|
|
|
if (clearable_layers_.empty())
|
|
{
|
|
robot::log_error("[recovery_core] '%s': layer_names is empty — this behavior will not clear "
|
|
"anything.",
|
|
name().c_str());
|
|
return false;
|
|
}
|
|
|
|
const bool needs_global = affected_maps_ == "global" || affected_maps_ == "both";
|
|
const bool needs_local = affected_maps_ == "local" || affected_maps_ == "both";
|
|
|
|
if (needs_global && ctx().global_costmap == nullptr)
|
|
{
|
|
robot::log_error("[recovery_core] '%s': affected_maps='%s' but the global costmap is "
|
|
"missing.",
|
|
name().c_str(), affected_maps_.c_str());
|
|
return false;
|
|
}
|
|
if (needs_local && ctx().local_costmap == nullptr)
|
|
{
|
|
robot::log_error("[recovery_core] '%s': affected_maps='%s' but the local costmap is missing.",
|
|
name().c_str(), affected_maps_.c_str());
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool onStart(const recovery_core::RecoveryGoal& /*goal*/) override
|
|
{
|
|
// One-shot: công việc thực hiện ở tick đầu tiên, giữ start() không có tác dụng phụ.
|
|
return true;
|
|
}
|
|
|
|
recovery_core::RecoveryResult onUpdate(const robot::Time& /*now*/, double /*dt*/) override
|
|
{
|
|
bool ok = true;
|
|
|
|
if (affected_maps_ == "global" || affected_maps_ == "both")
|
|
{
|
|
ok = clear(ctx().global_costmap, "global") && ok;
|
|
}
|
|
|
|
if (affected_maps_ == "local" || affected_maps_ == "both")
|
|
{
|
|
ok = clear(ctx().local_costmap, "local") && ok;
|
|
}
|
|
|
|
return ok ? recovery_core::RecoveryResult::Succeeded()
|
|
.withProgress(1.0, 0.0)
|
|
.withMessage("clear costmap complete")
|
|
: recovery_core::RecoveryResult::Failed().withMessage("clear costmap failed");
|
|
}
|
|
|
|
private:
|
|
bool clear(robot_costmap_2d::Costmap2DROBOT* costmap, const char* which)
|
|
{
|
|
if (costmap == nullptr || costmap->getLayeredCostmap() == nullptr)
|
|
{
|
|
robot::log_error("[recovery_core] '%s': %s costmap is missing.", name().c_str(), which);
|
|
return false;
|
|
}
|
|
|
|
robot_geometry_msgs::PoseStamped pose;
|
|
if (!costmap->getRobotPose(pose))
|
|
{
|
|
robot::log_error("[recovery_core] '%s': could not get the robot pose on the %s costmap.",
|
|
name().c_str(), which);
|
|
return false;
|
|
}
|
|
|
|
std::vector<boost::shared_ptr<robot_costmap_2d::Layer>>* plugins =
|
|
costmap->getLayeredCostmap()->getPlugins();
|
|
if (plugins == nullptr)
|
|
{
|
|
robot::log_error("[recovery_core] '%s': %s costmap has no layer.", name().c_str(),
|
|
which);
|
|
return false;
|
|
}
|
|
|
|
bool touched_layer = false;
|
|
std::string available;
|
|
|
|
for (const boost::shared_ptr<robot_costmap_2d::Layer>& plugin : *plugins)
|
|
{
|
|
if (!plugin)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
const std::string layer_name = leafName(plugin->getName());
|
|
|
|
if (!available.empty())
|
|
{
|
|
available += ", ";
|
|
}
|
|
available += layer_name;
|
|
|
|
if (clearable_layers_.count(layer_name) == 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
if (dynamic_cast<robot_costmap_2d::CostmapLayer*>(plugin.get()) == nullptr)
|
|
{
|
|
robot::log_warning("[recovery_core] '%s': layer '%s' is not a CostmapLayer; skipped.",
|
|
name().c_str(), layer_name.c_str());
|
|
continue;
|
|
}
|
|
|
|
clearMap(boost::static_pointer_cast<robot_costmap_2d::CostmapLayer>(plugin),
|
|
pose.pose.position.x, pose.pose.position.y);
|
|
touched_layer = true;
|
|
}
|
|
|
|
if (!touched_layer)
|
|
{
|
|
// Nguyên nhân phổ biến nhất của "recovery này không làm gì" là sai tên layer trong config.
|
|
// Bản trước trả kFailed lặng lẽ, nên không có cách nào biết vì sao.
|
|
robot::log_error("[recovery_core] '%s': no layer of the %s costmap matches layer_names. "
|
|
"Layers present: [%s].", name().c_str(), which, available.c_str());
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void clearMap(const boost::shared_ptr<robot_costmap_2d::CostmapLayer>& layer, double pose_x,
|
|
double pose_y)
|
|
{
|
|
boost::unique_lock<robot_costmap_2d::Costmap2D::mutex_t> lock(*(layer->getMutex()));
|
|
|
|
const double start_point_x = pose_x - reset_distance_ / 2.0;
|
|
const double start_point_y = pose_y - reset_distance_ / 2.0;
|
|
const double end_point_x = start_point_x + reset_distance_;
|
|
const double end_point_y = start_point_y + reset_distance_;
|
|
|
|
int start_x = 0;
|
|
int start_y = 0;
|
|
int end_x = 0;
|
|
int end_y = 0;
|
|
layer->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
|
|
layer->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
|
|
|
|
layer->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_);
|
|
|
|
// Báo cho layer biết toàn bộ vùng của nó cần được ghi lại vào master ở chu kỳ update kế tiếp.
|
|
layer->addExtraBounds(layer->getOriginX(), layer->getOriginY(),
|
|
layer->getOriginX() + layer->getSizeInMetersX(),
|
|
layer->getOriginY() + layer->getSizeInMetersY());
|
|
}
|
|
|
|
double reset_distance_ = kDefaultResetDistance; ///< [m] cạnh vùng vuông quanh robot
|
|
bool invert_area_to_clear_ = false; ///< true = xoá phần NGOÀI vùng
|
|
std::string affected_maps_ = "both"; ///< local | global | both
|
|
std::set<std::string> clearable_layers_;
|
|
};
|
|
|
|
} // namespace recovery_plugins
|
|
|
|
BOOST_DLL_ALIAS(recovery_plugins::ClearCostmapRecovery::create, ClearCostmapRecovery)
|