costmap_2d: costmap_2d: costmap

00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2008, 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 the Willow Garage 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 #ifndef COSTMAP_COSTMAP_2D_H_
00038 #define COSTMAP_COSTMAP_2D_H_
00039 
00040 #include <vector>
00041 #include <queue>
00042 #include <costmap_2d/observation.h>
00043 #include <costmap_2d/cell_data.h>
00044 #include <costmap_2d/cost_values.h>
00045 #include <sensor_msgs/PointCloud2.h>
00046 #include <boost/thread.hpp>
00047 
00048 namespace costmap_2d {
00049   class Costmap2DConfig;
00050 
00051   //convenient for storing x/y point pairs
00052   struct MapLocation {
00053     unsigned int x;
00054     unsigned int y;
00055   };
00056 
00061   class Costmap2D {
00062     friend class CostmapTester; //Need this for gtest to work correctly
00063     public:
00084       Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, 
00085           double resolution, double origin_x, double origin_y, double inscribed_radius = 0.0,
00086           double circumscribed_radius = 0.0, double inflation_radius = 0.0, double max_obstacle_range = 0.0,
00087           double max_obstacle_height = 0.0, double max_raytrace_range = 0.0, double weight = 25.0,
00088           const std::vector<unsigned char>& static_data = std::vector<unsigned char>(0), unsigned char lethal_threshold = 0,
00089           bool track_unknown_space = false, unsigned char unknown_cost_value = 0);
00090 
00095       Costmap2D(const Costmap2D& map);
00096 
00102       Costmap2D& operator=(const Costmap2D& map);
00103 
00112       void copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y);
00113 
00117       Costmap2D();
00118 
00122       virtual ~Costmap2D();
00123 
00124       void reconfigure(costmap_2d::Costmap2DConfig &config, const costmap_2d::Costmap2DConfig& last_config);
00125 
00126       virtual void finishConfiguration(costmap_2d::Costmap2DConfig &config);
00127 
00135       virtual void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
00136 
00145       void reinflateWindow(double wx, double wy, double w_size_x, double w_size_y, bool clear = true);
00146 
00155       virtual void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info = false);
00156 
00162       void updateWorld(double robot_x, double robot_y, 
00163           const std::vector<Observation>& observations, const std::vector<Observation>& clearing_observations);
00164 
00171       unsigned char getCost(unsigned int mx, unsigned int my) const;
00172 
00179       void setCost(unsigned int mx, unsigned int my, unsigned char cost);
00180 
00188       void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
00189 
00198       bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
00199 
00208       void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
00209 
00216       inline unsigned int getIndex(unsigned int mx, unsigned int my) const{
00217         return my * size_x_ + mx;
00218       }
00219 
00226       inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const{
00227         my = index / size_x_;
00228         mx = index - (my * size_x_);
00229       }
00230 
00235       const unsigned char* getCharMap() const;
00236 
00241       unsigned int getSizeInCellsX() const;
00242 
00247       unsigned int getSizeInCellsY() const;
00248 
00253       double getSizeInMetersX() const;
00254 
00259       double getSizeInMetersY() const;
00260 
00265       double getOriginX() const;
00266 
00271       double getOriginY() const;
00272 
00277       double getResolution() const;
00278     
00283       double getInscribedRadius() const { return inscribed_radius_; }
00284     
00289       double getCircumscribedRadius() const { return circumscribed_radius_; }
00290     
00295       double getInflationRadius() const { return inflation_radius_; }
00296 
00303       bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
00304 
00310       void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
00311 
00317       void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
00318 
00319 
00325       virtual void updateOrigin(double new_origin_x, double new_origin_y);
00326 
00334       bool isCircumscribedCell(unsigned int x, unsigned int y) const;
00335 
00341       inline unsigned char computeCost(double distance) const {
00342         unsigned char cost = 0;
00343         if(distance == 0)
00344           cost = LETHAL_OBSTACLE;
00345         else if(distance <= cell_inscribed_radius_)
00346           cost = INSCRIBED_INFLATED_OBSTACLE;
00347         else {
00348           //make sure cost falls off by Euclidean distance
00349           double euclidean_distance = distance * resolution_;
00350           double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
00351           cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
00352         }
00353         return cost;
00354       }
00355     
00356       /***
00357        * @brief Get the lower bound of cost for cells inside the circumscribed radius
00358        * @return the circumscribed_cost_lb_ attribute, which gets initialized to computeCost(cell_circumscribed_radius_)
00359        */
00360       inline unsigned char getCircumscribedCost() const {
00361         return circumscribed_cost_lb_;
00362       }
00363 
00368       void saveMap(std::string file_name);
00369 
00378       void updateStaticMapWindow(double win_origin_x, double win_origin_y, 
00379           unsigned int data_size_x, unsigned int data_size_y, 
00380           const std::vector<unsigned char>& static_data);
00381 
00390       void replaceFullMap(double win_origin_x, double win_origin_y,
00391           unsigned int data_size_x, unsigned int data_size_y,
00392           const std::vector<unsigned char>& static_data);
00393 
00394 
00395     protected:
00405       inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, 
00406           unsigned int src_x, unsigned int src_y, std::priority_queue<CellData>& inflation_queue){
00407         unsigned char* marked = &markers_[index];
00408         //set the cost of the cell being inserted
00409         if(*marked == 0){
00410           //we compute our distance table one cell further than the inflation radius dictates so we can make the check below
00411           double distance = distanceLookup(mx, my, src_x, src_y);
00412 
00413           //we only want to put the cell in the queue if it is within the inflation radius of the obstacle point
00414           if(distance > cell_inflation_radius_)
00415             return;
00416 
00417           //assign the cost associated with the distance from an obstacle to the cell
00418           updateCellCost(index, costLookup(mx, my, src_x, src_y));
00419 
00420           //push the cell data onto the queue and mark
00421           CellData data(distance, index, mx, my, src_x, src_y);
00422           inflation_queue.push(data);
00423           *marked = 1;
00424         }
00425       }
00426 
00441       template <typename data_type>
00442       void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x,
00443           data_type* dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x,
00444           unsigned int region_size_x, unsigned int region_size_y){
00445         //we'll first need to compute the starting points for each map
00446         data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
00447         data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
00448 
00449         //now, we'll copy the source map into the destination map
00450         for(unsigned int i = 0; i < region_size_y; ++i){
00451           memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
00452           sm_index += sm_size_x;
00453           dm_index += dm_size_x;
00454         }
00455       }
00456 
00462       unsigned int cellDistance(double world_dist);
00463 
00467       virtual void deleteMaps();
00468 
00472       virtual void resetMaps();
00473 
00477       void deleteKernels();
00478 
00484       virtual void initMaps(unsigned int size_x, unsigned int size_y);
00485 
00491       void copyKernels(const Costmap2D& map, unsigned int cell_inflation_radius);
00492 
00501       void reshapeStaticMap(double win_origin_x, double win_origin_y,
00502           unsigned int data_size_x, unsigned int data_size_y, const std::vector<unsigned char>& static_data);
00503 
00512       void replaceStaticMapWindow(double win_origin_x, double win_origin_y, 
00513           unsigned int data_size_x, unsigned int data_size_y, 
00514           const std::vector<unsigned char>& static_data);
00515 
00516     private:
00522       virtual void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);
00523 
00528       void raytraceFreespace(const std::vector<Observation>& clearing_observations);
00529 
00534       virtual void raytraceFreespace(const Observation& clearing_observation);
00535 
00545       void resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
00546           std::priority_queue<CellData>& inflation_queue, bool clear = true );
00547 
00551       void computeCaches();
00552 
00562       template <class ActionType>
00563         inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length = UINT_MAX){
00564           int dx = x1 - x0;
00565           int dy = y1 - y0;
00566 
00567           unsigned int abs_dx = abs(dx);
00568           unsigned int abs_dy = abs(dy);
00569 
00570           int offset_dx = sign(dx);
00571           int offset_dy = sign(dy) * size_x_;
00572 
00573           unsigned int offset = y0 * size_x_ + x0;
00574 
00575           //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
00576           double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1));
00577           double scale = std::min(1.0,  max_length / dist);
00578 
00579           //if x is dominant
00580           if(abs_dx >= abs_dy){
00581             int error_y = abs_dx / 2;
00582             bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
00583             return;
00584           }
00585 
00586           //otherwise y is dominant
00587           int error_x = abs_dy / 2;
00588           bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
00589 
00590         }
00591 
00595       template <class ActionType>
00596         inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, 
00597             unsigned int offset, unsigned int max_length){
00598           unsigned int end = std::min(max_length, abs_da);
00599           for(unsigned int i = 0; i < end; ++i){
00600             at(offset);
00601             offset += offset_a;
00602             error_b += abs_db;
00603             if((unsigned int)error_b >= abs_da){
00604               offset += offset_b;
00605               error_b -= abs_da;
00606             }
00607           }
00608           at(offset);
00609         }
00610 
00615       void inflateObstacles(std::priority_queue<CellData>& inflation_queue);
00616 
00622       inline void updateCellCost(unsigned int index, unsigned char cost){
00623         unsigned char* cell_cost = &costmap_[index];
00624         if(*cell_cost != NO_INFORMATION)
00625           *cell_cost = std::max(cost, *cell_cost);
00626         else if(cost == LETHAL_OBSTACLE)
00627           *cell_cost = cost;
00628       }
00629 
00638       inline char costLookup(int mx, int my, int src_x, int src_y){
00639         unsigned int dx = abs(mx - src_x);
00640         unsigned int dy = abs(my - src_y);
00641         return cached_costs_[dx][dy];
00642       }
00643 
00652       inline double distanceLookup(int mx, int my, int src_x, int src_y){
00653         unsigned int dx = abs(mx - src_x);
00654         unsigned int dy = abs(my - src_y);
00655         return cached_distances_[dx][dy];
00656       }
00657 
00658       inline int sign(int x){
00659         return x > 0 ? 1.0 : -1.0;
00660       }
00661 
00662       boost::recursive_mutex configuration_mutex_;
00663     protected:
00664       unsigned int size_x_;
00665       unsigned int size_y_;
00666       double resolution_;
00667       double origin_x_;
00668       double origin_y_;
00669       unsigned char* static_map_;
00670       unsigned char* costmap_;
00671       unsigned char* markers_;
00672       double max_obstacle_range_;
00673       double max_obstacle_height_;
00674       double max_raytrace_range_;
00675       unsigned char** cached_costs_;
00676       double** cached_distances_;
00677       double inscribed_radius_, circumscribed_radius_, inflation_radius_;
00678       unsigned int cell_inscribed_radius_, cell_circumscribed_radius_, cell_inflation_radius_;
00679       double weight_;
00680       unsigned char circumscribed_cost_lb_, lethal_threshold_;
00681       bool track_unknown_space_;
00682       unsigned char unknown_cost_value_;
00683       std::priority_queue<CellData> inflation_queue_;
00684 
00685       //functors for raytracing actions
00686       class ClearCell {
00687         public:
00688           ClearCell(unsigned char* costmap) : costmap_(costmap) {}
00689           inline void operator()(unsigned int offset){
00690             costmap_[offset] = 0;
00691           }
00692         private:
00693           unsigned char* costmap_;
00694       };
00695 
00696       class MarkCell {
00697         public:
00698           MarkCell(unsigned char* costmap) : costmap_(costmap) {}
00699           inline void operator()(unsigned int offset){
00700             costmap_[offset] = LETHAL_OBSTACLE;
00701           }
00702         private:
00703           unsigned char* costmap_;
00704       };
00705 
00706       class PolygonOutlineCells {
00707         public:
00708           PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector<MapLocation>& cells) 
00709             : costmap_(costmap), char_map_(char_map), cells_(cells){}
00710 
00711           //just push the relevant cells back onto the list
00712           inline void operator()(unsigned int offset){
00713             MapLocation loc;
00714             costmap_.indexToCells(offset, loc.x, loc.y);
00715             cells_.push_back(loc);
00716           }
00717 
00718         private:
00719           const Costmap2D& costmap_;
00720           const unsigned char* char_map_;
00721           std::vector<MapLocation>& cells_;
00722       };
00723   };
00724 };
00725 
00726 #endif
costmap_2d.h
