map_builder.h

Go to the documentation of this file.

#ifndef _LOCAL_MAP_MAP_BUILDER_H_
#define _LOCAL_MAP_MAP_BUILDER_H_

#include <cmath> // For std::exp.
#include <exception>
#include <string>
#include <vector>

#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include <angles/angles.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/OccupancyGrid.h>

#include <map_ray_caster/map_ray_caster.h>

namespace local_map
{

using std::vector;
using std::abs;
using std::max;

class MapBuilder
{
  public:

    MapBuilder(int width, int height, double resolution);

    bool saveMap(const std::string& name) const;  

    void grow(const sensor_msgs::LaserScan& scan);

    nav_msgs::OccupancyGrid getMap() const {return map_;}

  private:

    bool updateMap(const sensor_msgs::LaserScan& scan, long int dx, long int dy, double theta);
    bool getRayCastToObstacle(const nav_msgs::OccupancyGrid& map, double angle, double range, vector<size_t>& raycast);
    void updatePointOccupancy(bool occupied, size_t idx, vector<int8_t>& occupancy, vector<double>& log_odds) const;

    inline void updatePointsOccupancy(bool occupied, const vector<size_t>& indexes, vector<int8_t>& occupancy, vector<double>& log_odds)
    {
      vector<size_t>::const_iterator idx = indexes.begin();
      for (; idx != indexes.end(); ++idx)
      {
        updatePointOccupancy(occupied, *idx, occupancy, log_odds);
      }
    }

    // ROS parameters.
    double angle_resolution_;  

    double p_occupied_when_laser_;  


    double p_occupied_when_no_laser_ ;  



    double large_log_odds_;  


    double max_log_odds_for_belief_;  





    // Internals.
    tf::TransformListener tf_listerner_;
    std::string world_frame_id_; 
    tf::TransformBroadcaster tr_broadcaster_;  
    bool has_frame_id_;  
    std::string map_frame_id_;  
    double xinit_;  
    double yinit_;  
    long int last_xmap_;  
    long int last_ymap_;  

    nav_msgs::OccupancyGrid map_; 
    std::vector<double> log_odds_;  

    map_ray_caster::MapRayCaster ray_caster_;  
};

/* Return the offset from row and column number for a row-major array
 *
 * offset, row and column can be out of the map range.
 */
inline int offsetFromRowColNoRangeCheck(int row, int col, size_t ncol)
{
  return (row * ncol) + col;
}

template <typename T>
void moveAndCopyImage(int fill, int dx, int dy, unsigned int ncol, std::vector<T>& map);

} // namespace local_map

#endif // # ifndef _LOCAL_MAP_MAP_BUILDER_H_