costmap_2d: costmap_2d.h Source File

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   //convenient for storing x/y point pairs
00050   struct MapLocation {
00051     unsigned int x;
00052     unsigned int y;
00053   };
00054 
00059   class Costmap2D {
00060     friend class CostmapTester; //Need this for gtest to work correctly
00061     public:
00082       Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, 
00083           double resolution, double origin_x, double origin_y, double inscribed_radius = 0.0,
00084           double circumscribed_radius = 0.0, double inflation_radius = 0.0, double max_obstacle_range = 0.0,
00085           double max_obstacle_height = 0.0, double max_raytrace_range = 0.0, double weight = 25.0,
00086           const std::vector<unsigned char>& static_data = std::vector<unsigned char>(0), unsigned char lethal_threshold = 0,
00087           bool track_unknown_space = false, unsigned char unknown_cost_value = 0);
00088 
00093       Costmap2D(const Costmap2D& map);
00094 
00100       Costmap2D& operator=(const Costmap2D& map);
00101 
00110       void copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y);
00111 
00115       Costmap2D();
00116 
00120       virtual ~Costmap2D();
00121 
00129       virtual void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
00130 
00139       void reinflateWindow(double wx, double wy, double w_size_x, double w_size_y, bool clear = true);
00140 
00149       virtual void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info = false);
00150 
00156       void updateWorld(double robot_x, double robot_y, 
00157           const std::vector<Observation>& observations, const std::vector<Observation>& clearing_observations);
00158 
00165       unsigned char getCost(unsigned int mx, unsigned int my) const;
00166 
00173       void setCost(unsigned int mx, unsigned int my, unsigned char cost);
00174 
00182       void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;
00183 
00192       bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;
00193 
00202       void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;
00203 
00210       inline unsigned int getIndex(unsigned int mx, unsigned int my) const{
00211         return my * size_x_ + mx;
00212       }
00213 
00220       inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const{
00221         my = index / size_x_;
00222         mx = index - (my * size_x_);
00223       }
00224 
00229       const unsigned char* getCharMap() const;
00230 
00235       unsigned int getSizeInCellsX() const;
00236 
00241       unsigned int getSizeInCellsY() const;
00242 
00247       double getSizeInMetersX() const;
00248 
00253       double getSizeInMetersY() const;
00254 
00259       double getOriginX() const;
00260 
00265       double getOriginY() const;
00266 
00271       double getResolution() const;
00272     
00277       double getInscribedRadius() const { return inscribed_radius_; }
00278     
00283       double getCircumscribedRadius() const { return circumscribed_radius_; }
00284     
00289       double getInflationRadius() const { return inflation_radius_; }
00290 
00297       bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);
00298 
00304       void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
00305 
00311       void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);
00312 
00313 
00319       virtual void updateOrigin(double new_origin_x, double new_origin_y);
00320 
00328       bool isCircumscribedCell(unsigned int x, unsigned int y) const;
00329 
00335       inline unsigned char computeCost(double distance) const {
00336         unsigned char cost = 0;
00337         if(distance == 0)
00338           cost = LETHAL_OBSTACLE;
00339         else if(distance <= cell_inscribed_radius_)
00340           cost = INSCRIBED_INFLATED_OBSTACLE;
00341         else {
00342           //make sure cost falls off by Euclidean distance
00343           double euclidean_distance = distance * resolution_;
00344           double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
00345           cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
00346         }
00347         return cost;
00348       }
00349     
00350       /***
00351        * @brief Get the lower bound of cost for cells inside the circumscribed radius
00352        * @return the circumscribed_cost_lb_ attribute, which gets initialized to computeCost(cell_circumscribed_radius_)
00353        */
00354       inline unsigned char getCircumscribedCost() const {
00355         return circumscribed_cost_lb_;
00356       }
00357 
00362       void saveMap(std::string file_name);
00363 
00372       void updateStaticMapWindow(double win_origin_x, double win_origin_y, 
00373           unsigned int data_size_x, unsigned int data_size_y, 
00374           const std::vector<unsigned char>& static_data);
00375 
00384       void replaceFullMap(double win_origin_x, double win_origin_y,
00385           unsigned int data_size_x, unsigned int data_size_y,
00386           const std::vector<unsigned char>& static_data);
00387 
00388 
00389     protected:
00399       inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, 
00400           unsigned int src_x, unsigned int src_y, std::priority_queue<CellData>& inflation_queue){
00401         unsigned char* marked = &markers_[index];
00402         //set the cost of the cell being inserted
00403         if(*marked == 0){
00404           //we compute our distance table one cell further than the inflation radius dictates so we can make the check below
00405           double distance = distanceLookup(mx, my, src_x, src_y);
00406 
00407           //we only want to put the cell in the queue if it is within the inflation radius of the obstacle point
00408           if(distance > cell_inflation_radius_)
00409             return;
00410 
00411           //assign the cost associated with the distance from an obstacle to the cell
00412           updateCellCost(index, costLookup(mx, my, src_x, src_y));
00413 
00414           //push the cell data onto the queue and mark
00415           CellData data(distance, index, mx, my, src_x, src_y);
00416           inflation_queue.push(data);
00417           *marked = 1;
00418         }
00419       }
00420 
00435       template <typename data_type>
00436       void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x,
00437           data_type* dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x,
00438           unsigned int region_size_x, unsigned int region_size_y){
00439         //we'll first need to compute the starting points for each map
00440         data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
00441         data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);
00442 
00443         //now, we'll copy the source map into the destination map
00444         for(unsigned int i = 0; i < region_size_y; ++i){
00445           memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
00446           sm_index += sm_size_x;
00447           dm_index += dm_size_x;
00448         }
00449       }
00450 
00456       unsigned int cellDistance(double world_dist);
00457 
00461       virtual void deleteMaps();
00462 
00466       virtual void resetMaps();
00467 
00471       void deleteKernels();
00472 
00478       virtual void initMaps(unsigned int size_x, unsigned int size_y);
00479 
00485       void copyKernels(const Costmap2D& map, unsigned int cell_inflation_radius);
00486 
00495       void reshapeStaticMap(double win_origin_x, double win_origin_y,
00496           unsigned int data_size_x, unsigned int data_size_y, const std::vector<unsigned char>& static_data);
00497 
00506       void replaceStaticMapWindow(double win_origin_x, double win_origin_y, 
00507           unsigned int data_size_x, unsigned int data_size_y, 
00508           const std::vector<unsigned char>& static_data);
00509 
00510     private:
00516       virtual void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);
00517 
00522       void raytraceFreespace(const std::vector<Observation>& clearing_observations);
00523 
00528       virtual void raytraceFreespace(const Observation& clearing_observation);
00529 
00539       void resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
00540           std::priority_queue<CellData>& inflation_queue, bool clear = true );
00541 
00551       template <class ActionType>
00552         inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length = UINT_MAX){
00553           int dx = x1 - x0;
00554           int dy = y1 - y0;
00555 
00556           unsigned int abs_dx = abs(dx);
00557           unsigned int abs_dy = abs(dy);
00558 
00559           int offset_dx = sign(dx);
00560           int offset_dy = sign(dy) * size_x_;
00561 
00562           unsigned int offset = y0 * size_x_ + x0;
00563 
00564           //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
00565           double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1));
00566           double scale = std::min(1.0,  max_length / dist);
00567 
00568           //if x is dominant
00569           if(abs_dx >= abs_dy){
00570             int error_y = abs_dx / 2;
00571             bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
00572             return;
00573           }
00574 
00575           //otherwise y is dominant
00576           int error_x = abs_dy / 2;
00577           bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));
00578 
00579         }
00580 
00584       template <class ActionType>
00585         inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, 
00586             unsigned int offset, unsigned int max_length){
00587           unsigned int end = std::min(max_length, abs_da);
00588           for(unsigned int i = 0; i < end; ++i){
00589             at(offset);
00590             offset += offset_a;
00591             error_b += abs_db;
00592             if((unsigned int)error_b >= abs_da){
00593               offset += offset_b;
00594               error_b -= abs_da;
00595             }
00596           }
00597           at(offset);
00598         }
00599 
00604       void inflateObstacles(std::priority_queue<CellData>& inflation_queue);
00605 
00611       inline void updateCellCost(unsigned int index, unsigned char cost){
00612         unsigned char* cell_cost = &costmap_[index];
00613         if(*cell_cost != NO_INFORMATION)
00614           *cell_cost = std::max(cost, *cell_cost);
00615         else if(cost == LETHAL_OBSTACLE)
00616           *cell_cost = cost;
00617       }
00618 
00627       inline char costLookup(int mx, int my, int src_x, int src_y){
00628         unsigned int dx = abs(mx - src_x);
00629         unsigned int dy = abs(my - src_y);
00630         return cached_costs_[dx][dy];
00631       }
00632 
00641       inline double distanceLookup(int mx, int my, int src_x, int src_y){
00642         unsigned int dx = abs(mx - src_x);
00643         unsigned int dy = abs(my - src_y);
00644         return cached_distances_[dx][dy];
00645       }
00646 
00647       inline int sign(int x){
00648         return x > 0 ? 1.0 : -1.0;
00649       }
00650 
00651     protected:
00652       unsigned int size_x_;
00653       unsigned int size_y_;
00654       double resolution_;
00655       double origin_x_;
00656       double origin_y_;
00657       unsigned char* static_map_;
00658       unsigned char* costmap_;
00659       unsigned char* markers_;
00660       double max_obstacle_range_;
00661       double max_obstacle_height_;
00662       double max_raytrace_range_;
00663       unsigned char** cached_costs_;
00664       double** cached_distances_;
00665       double inscribed_radius_, circumscribed_radius_, inflation_radius_;
00666       unsigned int cell_inscribed_radius_, cell_circumscribed_radius_, cell_inflation_radius_;
00667       double weight_;
00668       unsigned char circumscribed_cost_lb_, lethal_threshold_;
00669       bool track_unknown_space_;
00670       unsigned char unknown_cost_value_;
00671       std::priority_queue<CellData> inflation_queue_;
00672 
00673       //functors for raytracing actions
00674       class ClearCell {
00675         public:
00676           ClearCell(unsigned char* costmap) : costmap_(costmap) {}
00677           inline void operator()(unsigned int offset){
00678             costmap_[offset] = 0;
00679           }
00680         private:
00681           unsigned char* costmap_;
00682       };
00683 
00684       class MarkCell {
00685         public:
00686           MarkCell(unsigned char* costmap) : costmap_(costmap) {}
00687           inline void operator()(unsigned int offset){
00688             costmap_[offset] = LETHAL_OBSTACLE;
00689           }
00690         private:
00691           unsigned char* costmap_;
00692       };
00693 
00694       class PolygonOutlineCells {
00695         public:
00696           PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector<MapLocation>& cells) 
00697             : costmap_(costmap), char_map_(char_map), cells_(cells){}
00698 
00699           //just push the relevant cells back onto the list
00700           inline void operator()(unsigned int offset){
00701             MapLocation loc;
00702             costmap_.indexToCells(offset, loc.x, loc.y);
00703             cells_.push_back(loc);
00704           }
00705 
00706         private:
00707           const Costmap2D& costmap_;
00708           const unsigned char* char_map_;
00709           std::vector<MapLocation>& cells_;
00710       };
00711   };
00712 };
00713 
00714 #endif