Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <clear_costmap_recovery/clear_costmap_recovery.h>
00038 #include <pluginlib/class_list_macros.h>
00039
00040
00041 PLUGINLIB_EXPORT_CLASS( clear_costmap_recovery::ClearCostmapRecovery, nav_core::RecoveryBehavior)
00042
00043 namespace clear_costmap_recovery {
00044 ClearCostmapRecovery::ClearCostmapRecovery(): global_costmap_(NULL), local_costmap_(NULL),
00045 tf_(NULL), initialized_(false) {}
00046
00047 void ClearCostmapRecovery::initialize(std::string name, tf::TransformListener* tf,
00048 costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){
00049 if(!initialized_){
00050 name_ = name;
00051 tf_ = tf;
00052 global_costmap_ = global_costmap;
00053 local_costmap_ = local_costmap;
00054
00055
00056 ros::NodeHandle private_nh("~/" + name_);
00057
00058 private_nh.param("reset_distance", reset_distance_, 3.0);
00059
00060 initialized_ = true;
00061 }
00062 else{
00063 ROS_ERROR("You should not call initialize twice on this object, doing nothing");
00064 }
00065 }
00066
00067 void ClearCostmapRecovery::runBehavior(){
00068 if(!initialized_){
00069 ROS_ERROR("This object must be initialized before runBehavior is called");
00070 return;
00071 }
00072
00073 if(global_costmap_ == NULL || local_costmap_ == NULL){
00074 ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot be NULL. Doing nothing.");
00075 return;
00076 }
00077 ROS_WARN("Clearing costmap to unstuck robot.");
00078
00079 double gc_radius = global_costmap_->getCircumscribedRadius();
00080 double lc_radius = local_costmap_->getCircumscribedRadius();
00081
00082 global_costmap_->resetMapOutsideWindow(reset_distance_, reset_distance_);
00083 local_costmap_->resetMapOutsideWindow(reset_distance_, reset_distance_);
00084
00085 global_costmap_->clearNonLethalWindow(4 * gc_radius, 4 * gc_radius);
00086 local_costmap_->clearNonLethalWindow(4 * lc_radius, 4 * lc_radius);
00087 }
00088 };