costmap_2d.h

Go to the documentation of this file.

/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef COSTMAP_COSTMAP_2D_H_
#define COSTMAP_COSTMAP_2D_H_

#include <vector>
#include <queue>
#include <cmath>
#include <costmap_2d/observation.h>
#include <costmap_2d/cell_data.h>
#include <costmap_2d/cost_values.h>
#include <boost/thread.hpp>

namespace costmap_2d {
  class Costmap2DConfig;

  //convenient for storing x/y point pairs
  struct MapLocation {
    unsigned int x;
    unsigned int y;
  };

  class Costmap2D {
    friend class CostmapTester; //Need this for gtest to work correctly
    public:
      Costmap2D(unsigned int cells_size_x, unsigned int cells_size_y, 
          double resolution, double origin_x, double origin_y, double inscribed_radius = 0.0,
          double circumscribed_radius = 0.0, double inflation_radius = 0.0, double max_obstacle_range = 0.0,
          double max_obstacle_height = 0.0, double max_raytrace_range = 0.0, double weight = 25.0,
          const std::vector<unsigned char>& static_data = std::vector<unsigned char>(0), unsigned char lethal_threshold = 0,
          bool track_unknown_space = false, unsigned char unknown_cost_value = 0);

      Costmap2D(const Costmap2D& map);

      Costmap2D& operator=(const Costmap2D& map);

      void copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y);

      Costmap2D();

      virtual ~Costmap2D();

      void reconfigure(costmap_2d::Costmap2DConfig &config, const costmap_2d::Costmap2DConfig& last_config);

      virtual void finishConfiguration(costmap_2d::Costmap2DConfig &config);

      virtual void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);

      void reinflateWindow(double wx, double wy, double w_size_x, double w_size_y, bool clear = true);

      virtual void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info = false);

      void updateWorld(double robot_x, double robot_y, 
          const std::vector<Observation>& observations, const std::vector<Observation>& clearing_observations);

      unsigned char getCost(unsigned int mx, unsigned int my) const;

      void setCost(unsigned int mx, unsigned int my, unsigned char cost);

      void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const;

      bool worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const;

      void worldToMapNoBounds(double wx, double wy, int& mx, int& my) const;

      inline unsigned int getIndex(unsigned int mx, unsigned int my) const{
        return my * size_x_ + mx;
      }

      inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const{
        my = index / size_x_;
        mx = index - (my * size_x_);
      }

      const unsigned char* getCharMap() const;

      unsigned int getSizeInCellsX() const;

      unsigned int getSizeInCellsY() const;

      double getSizeInMetersX() const;

      double getSizeInMetersY() const;

      double getOriginX() const;

      double getOriginY() const;

      double getResolution() const;
    
      double getInscribedRadius() const { return inscribed_radius_; }
    
      double getCircumscribedRadius() const { return circumscribed_radius_; }
    
      double getInflationRadius() const { return inflation_radius_; }

      bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);

      void polygonOutlineCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);

      void convexFillCells(const std::vector<MapLocation>& polygon, std::vector<MapLocation>& polygon_cells);


      virtual void updateOrigin(double new_origin_x, double new_origin_y);

      bool isCircumscribedCell(unsigned int x, unsigned int y) const;

      inline unsigned char computeCost(double distance) const {
        unsigned char cost = 0;
        if(distance == 0)
          cost = LETHAL_OBSTACLE;
        else if(distance <= cell_inscribed_radius_)
          cost = INSCRIBED_INFLATED_OBSTACLE;
        else {
          //make sure cost falls off by Euclidean distance
          double euclidean_distance = distance * resolution_;
          double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
          cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
        }
        return cost;
      }
    
      /***
       * @brief Get the lower bound of cost for cells inside the circumscribed radius
       * @return the circumscribed_cost_lb_ attribute, which gets initialized to computeCost(cell_circumscribed_radius_)
       */
      inline unsigned char getCircumscribedCost() const {
        return circumscribed_cost_lb_;
      }

      void saveMap(std::string file_name);

      void saveRawMap(std::string file_name);

      void updateStaticMapWindow(double win_origin_x, double win_origin_y, 
          unsigned int data_size_x, unsigned int data_size_y, 
          const std::vector<unsigned char>& static_data);

      void replaceFullMap(double win_origin_x, double win_origin_y,
          unsigned int data_size_x, unsigned int data_size_y,
          const std::vector<unsigned char>& static_data);


    protected:
      inline void enqueue(unsigned int index, unsigned int mx, unsigned int my, 
          unsigned int src_x, unsigned int src_y, std::priority_queue<CellData>& inflation_queue){
        unsigned char* marked = &markers_[index];
        //set the cost of the cell being inserted
        if(*marked == 0){
          //we compute our distance table one cell further than the inflation radius dictates so we can make the check below
          double distance = distanceLookup(mx, my, src_x, src_y);

          //we only want to put the cell in the queue if it is within the inflation radius of the obstacle point
          if(distance > cell_inflation_radius_)
            return;

          //assign the cost associated with the distance from an obstacle to the cell
          updateCellCost(index, costLookup(mx, my, src_x, src_y));

          //push the cell data onto the queue and mark
          CellData data(distance, index, mx, my, src_x, src_y);
          inflation_queue.push(data);
          *marked = 1;
        }
      }

      template <typename data_type>
      void copyMapRegion(data_type* source_map, unsigned int sm_lower_left_x, unsigned int sm_lower_left_y, unsigned int sm_size_x,
          data_type* dest_map, unsigned int dm_lower_left_x, unsigned int dm_lower_left_y, unsigned int dm_size_x,
          unsigned int region_size_x, unsigned int region_size_y){
        //we'll first need to compute the starting points for each map
        data_type* sm_index = source_map + (sm_lower_left_y * sm_size_x + sm_lower_left_x);
        data_type* dm_index = dest_map + (dm_lower_left_y * dm_size_x + dm_lower_left_x);

        //now, we'll copy the source map into the destination map
        for(unsigned int i = 0; i < region_size_y; ++i){
          memcpy(dm_index, sm_index, region_size_x * sizeof(data_type));
          sm_index += sm_size_x;
          dm_index += dm_size_x;
        }
      }

      unsigned int cellDistance(double world_dist);

      virtual void deleteMaps();

      virtual void resetMaps();

      void deleteKernels();

      virtual void initMaps(unsigned int size_x, unsigned int size_y);

      void copyKernels(const Costmap2D& map, unsigned int cell_inflation_radius);

      void reshapeStaticMap(double win_origin_x, double win_origin_y,
          unsigned int data_size_x, unsigned int data_size_y, const std::vector<unsigned char>& static_data);

      void replaceStaticMapWindow(double win_origin_x, double win_origin_y, 
          unsigned int data_size_x, unsigned int data_size_y, 
          const std::vector<unsigned char>& static_data);

    private:
      virtual void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);

      void raytraceFreespace(const std::vector<Observation>& clearing_observations);

      virtual void raytraceFreespace(const Observation& clearing_observation);

      void resetInflationWindow(double wx, double wy, double w_size_x, double w_size_y,
          std::priority_queue<CellData>& inflation_queue, bool clear = true );

      void computeCaches();

      template <class ActionType>
        inline void raytraceLine(ActionType at, unsigned int x0, unsigned int y0, unsigned int x1, unsigned int y1, unsigned int max_length = UINT_MAX){
          int dx = x1 - x0;
          int dy = y1 - y0;

          unsigned int abs_dx = abs(dx);
          unsigned int abs_dy = abs(dy);

          int offset_dx = sign(dx);
          int offset_dy = sign(dy) * size_x_;

          unsigned int offset = y0 * size_x_ + x0;

          //we need to chose how much to scale our dominant dimension, based on the maximum length of the line
          double dist = sqrt((x0 - x1) * (x0 - x1) + (y0 - y1) * (y0 - y1));
          double scale = std::min(1.0,  max_length / dist);

          //if x is dominant
          if(abs_dx >= abs_dy){
            int error_y = abs_dx / 2;
            bresenham2D(at, abs_dx, abs_dy, error_y, offset_dx, offset_dy, offset, (unsigned int)(scale * abs_dx));
            return;
          }

          //otherwise y is dominant
          int error_x = abs_dy / 2;
          bresenham2D(at, abs_dy, abs_dx, error_x, offset_dy, offset_dx, offset, (unsigned int)(scale * abs_dy));

        }

      template <class ActionType>
        inline void bresenham2D(ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, 
            unsigned int offset, unsigned int max_length){
          unsigned int end = std::min(max_length, abs_da);
          for(unsigned int i = 0; i < end; ++i){
            at(offset);
            offset += offset_a;
            error_b += abs_db;
            if((unsigned int)error_b >= abs_da){
              offset += offset_b;
              error_b -= abs_da;
            }
          }
          at(offset);
        }

      void inflateObstacles(std::priority_queue<CellData>& inflation_queue);

      inline void updateCellCost(unsigned int index, unsigned char cost){
        unsigned char* cell_cost = &costmap_[index];
        if(*cell_cost != NO_INFORMATION)
          *cell_cost = std::max(cost, *cell_cost);
        else if(cost == LETHAL_OBSTACLE)
          *cell_cost = cost;
      }

      inline char costLookup(int mx, int my, int src_x, int src_y){
        unsigned int dx = abs(mx - src_x);
        unsigned int dy = abs(my - src_y);
        return cached_costs_[dx][dy];
      }

      inline double distanceLookup(int mx, int my, int src_x, int src_y){
        unsigned int dx = abs(mx - src_x);
        unsigned int dy = abs(my - src_y);
        return cached_distances_[dx][dy];
      }

      inline int sign(int x){
        return x > 0 ? 1.0 : -1.0;
      }

      boost::recursive_mutex configuration_mutex_;
    protected:
      unsigned int size_x_;
      unsigned int size_y_;
      double resolution_;
      double origin_x_;
      double origin_y_;
      unsigned char* static_map_;
      unsigned char* costmap_;
      unsigned char* markers_;
      double max_obstacle_range_;
      double max_obstacle_height_;
      double max_raytrace_range_;
      unsigned char** cached_costs_;
      double** cached_distances_;
      double inscribed_radius_, circumscribed_radius_, inflation_radius_;
      double cell_inscribed_radius_;
      unsigned int cell_circumscribed_radius_, cell_inflation_radius_;
      double weight_;
      unsigned char circumscribed_cost_lb_, lethal_threshold_;
      bool track_unknown_space_;
      unsigned char unknown_cost_value_;
      std::priority_queue<CellData> inflation_queue_;

      //functors for raytracing actions
      class ClearCell {
        public:
          ClearCell(unsigned char* costmap) : costmap_(costmap) {}
          inline void operator()(unsigned int offset){
            costmap_[offset] = 0;
          }
        private:
          unsigned char* costmap_;
      };

      class MarkCell {
        public:
          MarkCell(unsigned char* costmap) : costmap_(costmap) {}
          inline void operator()(unsigned int offset){
            costmap_[offset] = LETHAL_OBSTACLE;
          }
        private:
          unsigned char* costmap_;
      };

      class PolygonOutlineCells {
        public:
          PolygonOutlineCells(const Costmap2D& costmap, const unsigned char* char_map, std::vector<MapLocation>& cells) 
            : costmap_(costmap), char_map_(char_map), cells_(cells){}

          //just push the relevant cells back onto the list
          inline void operator()(unsigned int offset){
            MapLocation loc;
            costmap_.indexToCells(offset, loc.x, loc.y);
            cells_.push_back(loc);
          }

        private:
          const Costmap2D& costmap_;
          const unsigned char* char_map_;
          std::vector<MapLocation>& cells_;
      };
  };
};

#endif