clear_costmap_recovery.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2009, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include <clear_costmap_recovery/clear_costmap_recovery.h>
00038 #include <pluginlib/class_list_macros.h>
00039 #include <vector>
00040 
00041 //register this planner as a RecoveryBehavior plugin
00042 PLUGINLIB_DECLARE_CLASS(clear_costmap_recovery, ClearCostmapRecovery, clear_costmap_recovery::ClearCostmapRecovery, nav_core::RecoveryBehavior)
00043 
00044 using costmap_2d::NO_INFORMATION;
00045 
00046 namespace clear_costmap_recovery {
00047 ClearCostmapRecovery::ClearCostmapRecovery(): global_costmap_(NULL), local_costmap_(NULL), 
00048   tf_(NULL), initialized_(false) {} 
00049 
00050 void ClearCostmapRecovery::initialize(std::string name, tf::TransformListener* tf,
00051     costmap_2d::Costmap2DROS* global_costmap, costmap_2d::Costmap2DROS* local_costmap){
00052   if(!initialized_){
00053     name_ = name;
00054     tf_ = tf;
00055     global_costmap_ = global_costmap;
00056     local_costmap_ = local_costmap;
00057 
00058     //get some parameters from the parameter server
00059     ros::NodeHandle private_nh("~/" + name_);
00060 
00061     private_nh.param("reset_distance", reset_distance_, 3.0);
00062     
00063     std::vector<std::string> clearable_layers_default, clearable_layers;
00064     clearable_layers_default.push_back( std::string("obstacles") );
00065     private_nh.param("layer_names", clearable_layers, clearable_layers_default);
00066 
00067     for(unsigned i=0; i < clearable_layers.size(); i++) {
00068         ROS_INFO("Recovery behavior will clear layer %s", clearable_layers[i].c_str());
00069         clearable_layers_.insert(clearable_layers[i]);
00070     }
00071 
00072 
00073     initialized_ = true;
00074   }
00075   else{
00076     ROS_ERROR("You should not call initialize twice on this object, doing nothing");
00077   }
00078 }
00079 
00080 void ClearCostmapRecovery::runBehavior(){
00081   if(!initialized_){
00082     ROS_ERROR("This object must be initialized before runBehavior is called");
00083     return;
00084   }
00085 
00086   if(global_costmap_ == NULL || local_costmap_ == NULL){
00087     ROS_ERROR("The costmaps passed to the ClearCostmapRecovery object cannot be NULL. Doing nothing.");
00088     return;
00089   }
00090   ROS_WARN("Clearing costmap to unstuck robot (%fm).", reset_distance_);
00091   clear(global_costmap_);
00092   clear(local_costmap_);
00093 }
00094 
00095 void ClearCostmapRecovery::clear(costmap_2d::Costmap2DROS* costmap){
00096   std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = costmap->getLayeredCostmap()->getPlugins();
00097 
00098   tf::Stamped<tf::Pose> pose;
00099 
00100   if(!costmap->getRobotPose(pose)){
00101     ROS_ERROR("Cannot clear map because pose cannot be retrieved");
00102     return;
00103   }
00104 
00105   double x = pose.getOrigin().x();
00106   double y = pose.getOrigin().y();
00107 
00108   for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
00109     boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
00110     std::string name = plugin->getName();
00111     int slash = name.rfind('/');
00112     if( slash != std::string::npos ){
00113         name = name.substr(slash+1);
00114     }
00115 
00116     if(clearable_layers_.count(name)!=0){
00117       boost::shared_ptr<costmap_2d::CostmapLayer> costmap;
00118       costmap = boost::static_pointer_cast<costmap_2d::CostmapLayer>(plugin);
00119       clearMap(costmap, x, y);
00120     }
00121   }
00122 }
00123 
00124 
00125 void ClearCostmapRecovery::clearMap(boost::shared_ptr<costmap_2d::CostmapLayer> costmap, 
00126                                         double pose_x, double pose_y){
00127   boost::unique_lock< boost::shared_mutex > lock(*(costmap->getLock()));
00128  
00129   double start_point_x = pose_x - reset_distance_ / 2;
00130   double start_point_y = pose_y - reset_distance_ / 2;
00131   double end_point_x = start_point_x + reset_distance_;
00132   double end_point_y = start_point_y + reset_distance_;
00133 
00134   int start_x, start_y, end_x, end_y;
00135   costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y);
00136   costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y);
00137 
00138   unsigned char* grid = costmap->getCharMap();
00139   for(int x=0; x<(int)costmap->getSizeInCellsX(); x++){
00140     bool xrange = x>start_x && x<end_x;
00141                    
00142     for(int y=0; y<(int)costmap->getSizeInCellsY(); y++){
00143       if(xrange && y>start_y && y<end_y)
00144         continue;
00145       int index = costmap->getIndex(x,y);
00146       if(grid[index]!=NO_INFORMATION){
00147         grid[index] = NO_INFORMATION;
00148       }
00149     }
00150   }
00151 
00152   double ox = costmap->getOriginX(), oy = costmap->getOriginY();
00153   double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY();
00154   costmap->addExtraBounds(ox, oy, ox + width, oy + height);
00155   return;
00156 }
00157 
00158 };


clear_costmap_recovery
Author(s): Eitan Marder-Eppstein, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:24