static_layer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
00006  *  Copyright (c) 2015, Fetch Robotics, Inc.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Eitan Marder-Eppstein
00037  *         David V. Lu!!
00038  *********************************************************************/
00039 #include <costmap_2d/static_layer.h>
00040 #include <costmap_2d/costmap_math.h>
00041 #include <pluginlib/class_list_macros.h>
00042 
00043 PLUGINLIB_EXPORT_CLASS(costmap_2d::StaticLayer, costmap_2d::Layer)
00044 
00045 using costmap_2d::NO_INFORMATION;
00046 using costmap_2d::LETHAL_OBSTACLE;
00047 using costmap_2d::FREE_SPACE;
00048 
00049 namespace costmap_2d
00050 {
00051 
00052 StaticLayer::StaticLayer() : dsrv_(NULL) {}
00053 
00054 StaticLayer::~StaticLayer()
00055 {
00056   if (dsrv_)
00057     delete dsrv_;
00058 }
00059 
00060 void StaticLayer::onInitialize()
00061 {
00062   ros::NodeHandle nh("~/" + name_), g_nh;
00063   current_ = true;
00064 
00065   global_frame_ = layered_costmap_->getGlobalFrameID();
00066 
00067   std::string map_topic;
00068   nh.param("map_topic", map_topic, std::string("map"));
00069   nh.param("first_map_only", first_map_only_, false);
00070   nh.param("subscribe_to_updates", subscribe_to_updates_, false);
00071 
00072   nh.param("track_unknown_space", track_unknown_space_, true);
00073   nh.param("use_maximum", use_maximum_, false);
00074 
00075   int temp_lethal_threshold, temp_unknown_cost_value;
00076   nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
00077   nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
00078   nh.param("trinary_costmap", trinary_costmap_, true);
00079 
00080   lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
00081   unknown_cost_value_ = temp_unknown_cost_value;
00082 
00083   // Only resubscribe if topic has changed
00084   if (map_sub_.getTopic() != ros::names::resolve(map_topic))
00085   {
00086     // we'll subscribe to the latched topic that the map server uses
00087     ROS_INFO("Requesting the map...");
00088     map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);
00089     map_received_ = false;
00090     has_updated_data_ = false;
00091 
00092     ros::Rate r(10);
00093     while (!map_received_ && g_nh.ok())
00094     {
00095       ros::spinOnce();
00096       r.sleep();
00097     }
00098 
00099     ROS_INFO("Received a %d X %d map at %f m/pix", getSizeInCellsX(), getSizeInCellsY(), getResolution());
00100 
00101     if (subscribe_to_updates_)
00102     {
00103       ROS_INFO("Subscribing to updates");
00104       map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);
00105 
00106     }
00107   }
00108   else
00109   {
00110     has_updated_data_ = true;
00111   }
00112 
00113   if (dsrv_)
00114   {
00115     delete dsrv_;
00116   }
00117 
00118   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
00119   dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
00120       &StaticLayer::reconfigureCB, this, _1, _2);
00121   dsrv_->setCallback(cb);
00122 }
00123 
00124 void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
00125 {
00126   if (config.enabled != enabled_)
00127   {
00128     enabled_ = config.enabled;
00129     has_updated_data_ = true;
00130     x_ = y_ = 0;
00131     width_ = size_x_;
00132     height_ = size_y_;
00133   }
00134 }
00135 
00136 void StaticLayer::matchSize()
00137 {
00138   // If we are using rolling costmap, the static map size is
00139   //   unrelated to the size of the layered costmap
00140   if (!layered_costmap_->isRolling())
00141   {
00142     Costmap2D* master = layered_costmap_->getCostmap();
00143     resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
00144               master->getOriginX(), master->getOriginY());
00145   }
00146 }
00147 
00148 unsigned char StaticLayer::interpretValue(unsigned char value)
00149 {
00150   // check if the static value is above the unknown or lethal thresholds
00151   if (track_unknown_space_ && value == unknown_cost_value_)
00152     return NO_INFORMATION;
00153   else if (!track_unknown_space_ && value == unknown_cost_value_)
00154     return FREE_SPACE;
00155   else if (value >= lethal_threshold_)
00156     return LETHAL_OBSTACLE;
00157   else if (trinary_costmap_)
00158     return FREE_SPACE;
00159 
00160   double scale = (double) value / lethal_threshold_;
00161   return scale * LETHAL_OBSTACLE;
00162 }
00163 
00164 void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
00165 {
00166   unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
00167 
00168   ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
00169 
00170   // resize costmap if size, resolution or origin do not match
00171   Costmap2D* master = layered_costmap_->getCostmap();
00172   if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
00173       master->getSizeInCellsY() != size_y ||
00174       master->getResolution() != new_map->info.resolution ||
00175       master->getOriginX() != new_map->info.origin.position.x ||
00176       master->getOriginY() != new_map->info.origin.position.y ||
00177       !layered_costmap_->isSizeLocked()))
00178   {
00179     // Update the size of the layered costmap (and all layers, including this one)
00180     ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
00181     layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
00182                                 new_map->info.origin.position.y, true);
00183   }
00184   else if (size_x_ != size_x || size_y_ != size_y ||
00185            resolution_ != new_map->info.resolution ||
00186            origin_x_ != new_map->info.origin.position.x ||
00187            origin_y_ != new_map->info.origin.position.y)
00188   {
00189     // only update the size of the costmap stored locally in this layer
00190     ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
00191     resizeMap(size_x, size_y, new_map->info.resolution,
00192               new_map->info.origin.position.x, new_map->info.origin.position.y);
00193   }
00194 
00195   unsigned int index = 0;
00196 
00197   // initialize the costmap with static data
00198   for (unsigned int i = 0; i < size_y; ++i)
00199   {
00200     for (unsigned int j = 0; j < size_x; ++j)
00201     {
00202       unsigned char value = new_map->data[index];
00203       costmap_[index] = interpretValue(value);
00204       ++index;
00205     }
00206   }
00207   map_frame_ = new_map->header.frame_id;
00208 
00209   // we have a new map, update full size of map
00210   x_ = y_ = 0;
00211   width_ = size_x_;
00212   height_ = size_y_;
00213   map_received_ = true;
00214   has_updated_data_ = true;
00215 
00216   // shutdown the map subscrber if firt_map_only_ flag is on
00217   if (first_map_only_)
00218   {
00219     ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
00220     map_sub_.shutdown();
00221   }
00222 }
00223 
00224 void StaticLayer::incomingUpdate(const map_msgs::OccupancyGridUpdateConstPtr& update)
00225 {
00226   unsigned int di = 0;
00227   for (unsigned int y = 0; y < update->height ; y++)
00228   {
00229     unsigned int index_base = (update->y + y) * size_x_;
00230     for (unsigned int x = 0; x < update->width ; x++)
00231     {
00232       unsigned int index = index_base + x + update->x;
00233       costmap_[index] = interpretValue(update->data[di++]);
00234     }
00235   }
00236   x_ = update->x;
00237   y_ = update->y;
00238   width_ = update->width;
00239   height_ = update->height;
00240   has_updated_data_ = true;
00241 }
00242 
00243 void StaticLayer::activate()
00244 {
00245   onInitialize();
00246 }
00247 
00248 void StaticLayer::deactivate()
00249 {
00250   map_sub_.shutdown();
00251   if (subscribe_to_updates_)
00252     map_update_sub_.shutdown();
00253 }
00254 
00255 void StaticLayer::reset()
00256 {
00257   if (first_map_only_)
00258   {
00259     has_updated_data_ = true;
00260   }
00261   else
00262   {
00263     onInitialize();
00264   }
00265 }
00266 
00267 void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
00268                                double* max_x, double* max_y)
00269 {
00270 
00271   if( !layered_costmap_->isRolling() ){
00272     if (!map_received_ || !(has_updated_data_ || has_extra_bounds_))
00273       return;
00274   }
00275 
00276   useExtraBounds(min_x, min_y, max_x, max_y);
00277 
00278   double wx, wy;
00279 
00280   mapToWorld(x_, y_, wx, wy);
00281   *min_x = std::min(wx, *min_x);
00282   *min_y = std::min(wy, *min_y);
00283 
00284   mapToWorld(x_ + width_, y_ + height_, wx, wy);
00285   *max_x = std::max(wx, *max_x);
00286   *max_y = std::max(wy, *max_y);
00287 
00288   has_updated_data_ = false;
00289 }
00290 
00291 void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
00292 {
00293   if (!map_received_)
00294     return;
00295 
00296   if (!layered_costmap_->isRolling())
00297   {
00298     // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
00299     if (!use_maximum_)
00300       updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
00301     else
00302       updateWithMax(master_grid, min_i, min_j, max_i, max_j);
00303   }
00304   else
00305   {
00306     // If rolling window, the master_grid is unlikely to have same coordinates as this layer
00307     unsigned int mx, my;
00308     double wx, wy;
00309     // Might even be in a different frame
00310     tf::StampedTransform transform;
00311     try
00312     {
00313       tf_->lookupTransform(map_frame_, global_frame_, ros::Time(0), transform);
00314     }
00315     catch (tf::TransformException ex)
00316     {
00317       ROS_ERROR("%s", ex.what());
00318       return;
00319     }
00320     // Copy map data given proper transformations
00321     for (unsigned int i = min_i; i < max_i; ++i)
00322     {
00323       for (unsigned int j = min_j; j < max_j; ++j)
00324       {
00325         // Convert master_grid coordinates (i,j) into global_frame_(wx,wy) coordinates
00326         layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
00327         // Transform from global_frame_ to map_frame_
00328         tf::Point p(wx, wy, 0);
00329         p = transform(p);
00330         // Set master_grid with cell from map
00331         if (worldToMap(p.x(), p.y(), mx, my))
00332         {
00333           if (!use_maximum_)
00334             master_grid.setCost(i, j, getCost(mx, my));
00335           else
00336             master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
00337         }
00338       }
00339     }
00340   }
00341 }
00342 
00343 }  // namespace costmap_2d


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:22