85 lines
2.3 KiB
C++
85 lines
2.3 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* recovery_core — path output recovery plugin.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
|
|
#include <recovery_core/recovery_behavior.h>
|
|
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
#include <robot/robot.h>
|
|
|
|
namespace recovery_plugins
|
|
{
|
|
|
|
class RegenPathRecovery final : public recovery_core::RecoveryBehavior
|
|
{
|
|
public:
|
|
RegenPathRecovery() = 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
|
|
{
|
|
if (initialized_)
|
|
{
|
|
robot::log_error("[recovery_core] RegenPathRecovery '%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;
|
|
|
|
initialized_ = true;
|
|
status_ = recovery_core::RecoveryStatus::kIdle;
|
|
}
|
|
|
|
recovery_core::RecoveryResult runBehavior() override
|
|
{
|
|
if (!initialized_ || global_path_ == nullptr || global_path_->empty())
|
|
{
|
|
status_ = recovery_core::RecoveryStatus::kFailed;
|
|
return recovery_core::RecoveryResult::Failed();
|
|
}
|
|
|
|
robot_nav_msgs::Path path;
|
|
path.poses = *global_path_;
|
|
|
|
status_ = recovery_core::RecoveryStatus::kSucceeded;
|
|
return recovery_core::RecoveryResult::PathOut(path, status_);
|
|
}
|
|
|
|
recovery_core::RecoveryStatus status() const override
|
|
{
|
|
return status_;
|
|
}
|
|
|
|
static recovery_core::RecoveryBehavior::RecoveryBehaviorPtr create()
|
|
{
|
|
return std::make_shared<RegenPathRecovery>();
|
|
}
|
|
|
|
private:
|
|
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;
|
|
};
|
|
|
|
} // namespace recovery_plugins
|
|
|
|
BOOST_DLL_ALIAS(recovery_plugins::RegenPathRecovery::create, RegenPathRecovery)
|