inflation_layer.h
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  *  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  *         David V. Lu!!
00037  *********************************************************************/
00038 #ifndef COSTMAP_2D_INFLATION_LAYER_H_
00039 #define COSTMAP_2D_INFLATION_LAYER_H_
00040 
00041 #include <ros/ros.h>
00042 #include <costmap_2d/layer.h>
00043 #include <costmap_2d/layered_costmap.h>
00044 #include <costmap_2d/InflationPluginConfig.h>
00045 #include <dynamic_reconfigure/server.h>
00046 #include <boost/thread.hpp>
00047 #include <queue>
00048 
00049 namespace costmap_2d
00050 {
00055 class CellData
00056 {
00057 public:
00068   CellData(double d, double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
00069       distance_(d), index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
00070   {
00071   }
00072   double distance_;
00073   unsigned int index_;
00074   unsigned int x_, y_;
00075   unsigned int src_x_, src_y_;
00076 };
00077 
00082 inline bool operator<(const CellData &a, const CellData &b)
00083 {
00084   return a.distance_ > b.distance_;
00085 }
00086 
00087 class InflationLayer : public Layer
00088 {
00089 public:
00090   InflationLayer();
00091 
00092   virtual ~InflationLayer()
00093   {
00094     deleteKernels();
00095     if (dsrv_)
00096         delete dsrv_;
00097   }
00098 
00099   virtual void onInitialize();
00100   virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
00101                             double* max_x, double* max_y);
00102   virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
00103   virtual bool isDiscretized()
00104   {
00105     return true;
00106   }
00107   virtual void matchSize();
00108 
00109   virtual void reset() { onInitialize(); }
00110 
00114   inline unsigned char computeCost(double distance) const
00115   {
00116     unsigned char cost = 0;
00117     if (distance == 0)
00118       cost = LETHAL_OBSTACLE;
00119     else if (distance * resolution_ <= inscribed_radius_)
00120       cost = INSCRIBED_INFLATED_OBSTACLE;
00121     else
00122     {
00123       // make sure cost falls off by Euclidean distance
00124       double euclidean_distance = distance * resolution_;
00125       double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
00126       cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
00127     }
00128     return cost;
00129   }
00130 
00136   void setInflationParameters(double inflation_radius, double cost_scaling_factor);
00137 
00138 protected:
00139   virtual void onFootprintChanged();
00140   boost::recursive_mutex* inflation_access_;
00141 
00142 private:
00151   inline double distanceLookup(int mx, int my, int src_x, int src_y)
00152   {
00153     unsigned int dx = abs(mx - src_x);
00154     unsigned int dy = abs(my - src_y);
00155     return cached_distances_[dx][dy];
00156   }
00157 
00166   inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
00167   {
00168     unsigned int dx = abs(mx - src_x);
00169     unsigned int dy = abs(my - src_y);
00170     return cached_costs_[dx][dy];
00171   }
00172 
00173   void computeCaches();
00174   void deleteKernels();
00175   void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
00176 
00177   unsigned int cellDistance(double world_dist)
00178   {
00179     return layered_costmap_->getCostmap()->cellDistance(world_dist);
00180   }
00181 
00182   inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
00183                       unsigned int src_x, unsigned int src_y);
00184 
00185   double inflation_radius_, inscribed_radius_, weight_;
00186   unsigned int cell_inflation_radius_;
00187   unsigned int cached_cell_inflation_radius_;
00188   std::priority_queue<CellData> inflation_queue_;
00189 
00190   double resolution_;
00191 
00192   bool* seen_;
00193   int seen_size_;
00194 
00195   unsigned char** cached_costs_;
00196   double** cached_distances_;
00197   double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
00198 
00199   dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig> *dsrv_;
00200   void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level);
00201 
00202   bool need_reinflation_;  
00203 };
00204 
00205 }  // namespace costmap_2d
00206 
00207 #endif  // COSTMAP_2D_INFLATION_LAYER_H_


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