|
not the robot will attempt an in-place rotation when attempting to clear out space.
~clearing_rotation_allowed (bool, default: true)
#来回震荡的距离，超过一定的时间没有移动足够的距离就会进行recovery behavior
#How far in meters the robot must move to be considered not to be oscillating. Moving this far resets the timer counting up to the ~oscillation_timeout
~oscillation_distance (double, default: 0.5)
#How long in seconds to allow for oscillation before executing recovery behaviors. A value of 0.0 corresponds to an infinite timeout.
~oscillation_timeout (double, default: 0.0)
For the default parameters, the aggressive_reset behavior will clear out to a distance of 4 * ~/local_costmap/circumscribed_radius.
clear_costmap_recovery
ROS wiki
代码文档
是把local,global costmap在reset_distance半径之外的free，occupied都清除，变成NO_INFORMATION。不对static layer操作
重要的参数
#距离以机器人为中心，reset_distance米为半径的圆外面的costmap(local,global)都会被清除掉，
#只留下static map原来的信息
~<name>/reset_distance (double, default: 3.0)
怎么使用
#include <tf/transform_listener.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <clear_costmap_recovery/clear_costmap_recovery.h>
tf::TransformListener tf(ros::Duration(10));
costmap_2d::Costmap2DROS global_costmap("global_costmap", tf);
costmap_2d::Costmap2DROS local_costmap("local_costmap", tf);
clear_costmap_recovery::ClearCostmapRecovery ccr;
ccr.initialize("my_clear_costmap_recovery", &tf, &global_costmap, &local_costmap);
//recovery 行为
ccr.runBehavior();
源码
namespace clear_costmap_recovery{
class ClearCostmapRecovery : public nav_core::RecoveryBehavior {
//在多大的半径之外的obstacle全部清除
double reset_distance_;
//那些layer的obstacle要清除
std::set<std::string> clearable_layers_; ///< Layer names which will be cleared.
void ClearCostmapRecovery::initialize(std::string name, tf::TransformListener* tf,
costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){
if(!initialized_){
global_costmap_ = global_costmap;
local_costmap_ = local_costmap;
private_nh.param("reset_distance", reset_distance_, 3.0);
std::vector<std::string> clearable_layers_default, clearable_layers;
clearable_layers_default.push_back( std::string("obstacles") );
private_nh.param("layer_names", clearable_layers, clearable_layers_default);
for(unsigned i=0; i < clearable_layers.size(); i++) {
ROS_INFO("Recovery behavior will clear layer %s", clearable_layers[i].c_str());
clearable_layers_.insert(clearable_layers[i]);
}
initialized_ = true;
}
}
void ClearCostmapRecovery::runBehavior(){
ROS_WARN("Clearing costmap to unstuck robot (%fm).", reset_distance_);
//看好是把global，local的costmap的obstacle都清除掉
clear(global_costmap_);
clear(local_costmap_);
}
void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap){
{
//得到costmap中定义的所有layer
std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();
for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
{
if(clearable_layers_.count(name)!=0){
costmap
|