ThreadGrid.cpp
Go to the documentation of this file.
00001 #include "ThreadGrid.h"
00002 #include "SlamNode.h"
00003 
00004 #include <string>
00005 
00006 #include "obvision/reconstruct/grid/RayCastAxisAligned2D.h"
00007 #include "obcore/base/Logger.h"
00008 
00009 
00010 namespace ohm_tsd_slam
00011 {
00012 ThreadGrid::ThreadGrid(obvious::TsdGrid* grid, ros::NodeHandle* const nh, const double xOffset, const double yOffset):
00013         ThreadSLAM(*grid),
00014         _occGrid(new nav_msgs::OccupancyGrid),
00015         _occGridContent(new char[grid->getCellsX() * grid->getCellsY()]),
00016         _gridCoords(new double[grid->getCellsX() * grid->getCellsY()]),
00017         _width(grid->getCellsX()),
00018         _height(grid->getCellsY()),
00019         _cellSize(grid->getCellSize())
00020 
00021 {
00022   for(unsigned int i = 0; i < _grid.getCellsX() * _grid.getCellsY(); ++i)
00023     _occGridContent[i] = -1;
00024 
00025   _occGrid->info.resolution           = static_cast<double>(_grid.getCellSize());
00026   _occGrid->info.width                = _grid.getCellsX();
00027   _occGrid->info.height               = _grid.getCellsY();
00028   _occGrid->info.origin.orientation.w = 0.0;
00029   _occGrid->info.origin.orientation.x = 0.0;
00030   _occGrid->info.origin.orientation.y = 0.0;
00031   _occGrid->info.origin.orientation.z = 0.0;
00032   _occGrid->info.origin.position.x    = 0.0 - (_grid.getCellsX() * _grid.getCellSize() * 0.5 + xOffset);
00033   _occGrid->info.origin.position.y    = 0.0 - (_grid.getCellsY() * _grid.getCellSize() * 0.5 + yOffset);
00034   _occGrid->info.origin.position.z    = 0.0;
00035   _occGrid->data.resize(_grid.getCellsX() * _grid.getCellsY());
00036 
00037   ros::NodeHandle prvNh("~");
00038   std::string mapTopic;
00039   std::string mapFrame;
00040   std::string getMapTopic;
00041   int intVar         = 0;
00042 
00043   prvNh.param("map_topic", mapTopic, std::string("map"));
00044   prvNh.param("get_map_topic", getMapTopic, std::string("map"));
00045   prvNh.param<int>("object_inflation_factor", intVar, 2);
00046   prvNh.param<bool>("use_object_inflation", _objectInflation, false);
00047   
00048   prvNh.param("map_frame", mapFrame, std::string("map"));
00049   _occGrid->header.frame_id           = mapFrame;
00050 
00051   _gridPub          = nh->advertise<nav_msgs::OccupancyGrid>(mapTopic, 1);
00052   _getMapServ       = nh->advertiseService(getMapTopic, &ThreadGrid::getMapServCallBack, this);
00053   _objInflateFactor = static_cast<unsigned int>(intVar);
00054 }
00055 
00056 ThreadGrid::~ThreadGrid()
00057 {
00058   _stayActive = false;
00059   _thread->join();
00060   delete _occGrid;
00061   delete _occGridContent;
00062   delete _gridCoords;
00063 }
00064 
00065 void ThreadGrid::eventLoop(void)
00066 {
00067   static unsigned int frameId = 0;
00068   while(_stayActive)
00069   {
00070     _sleepCond.wait(_sleepMutex);
00071     unsigned int mapSize = 0;
00072     obvious::RayCastAxisAligned2D raycasterMap;
00073     raycasterMap.calcCoords(&_grid, _gridCoords, NULL, &mapSize, _occGridContent);
00074     if(mapSize == 0)
00075       ROS_INFO_STREAM("OccupancyGridThread: Warning! Raycasting returned with no coordinates, map contains no data yet!\n");
00076 
00077     _occGrid->header.stamp       = ros::Time::now();
00078     _occGrid->header.seq         = frameId++;
00079     _occGrid->info.map_load_time = ros::Time::now();
00080     const unsigned int gridSize        = _width * _height;
00081 
00082     for(unsigned int i = 0; i < gridSize ; ++i)
00083       _occGrid->data[i] = _occGridContent[i];
00084 
00085     for(unsigned int i = 0; i < mapSize / 2; i++)
00086     {
00087       double x       = _gridCoords[2*i];
00088       double y       = _gridCoords[2*i+1];
00089       unsigned int u = static_cast<unsigned int>(round(x / _cellSize));
00090       unsigned int v = static_cast<unsigned int>(round(y / _cellSize));
00091       if(u > 0 && u < _width && v > 0 && v < _height)
00092       {
00093         _occGrid->data[v * _width + u] = 100;               //set grid cell to occupied
00094         if(_objectInflation)
00095         {
00096           for(unsigned int i = v-_objInflateFactor; i < v + _objInflateFactor; i++)
00097           {
00098             for(unsigned int j = u - _objInflateFactor; j < u + _objInflateFactor; j++)
00099             {
00100               if((u >= _width) || (v >= _height))
00101                 continue;
00102               _occGrid->data[i * _width + j] = 100;
00103             }
00104           }
00105         }
00106       }
00107     }
00108     _gridPub.publish(*_occGrid);
00109   }
00110 }
00111 
00112 bool ThreadGrid::getMapServCallBack(nav_msgs::GetMap::Request& req, nav_msgs::GetMap::Response& res)
00113 {
00114   static unsigned int frameId = 0;
00115   res.map = *_occGrid;
00116   res.map.header.stamp = ros::Time::now();
00117   _occGrid->header.seq = frameId++;
00118   _occGrid->info.map_load_time = ros::Time::now();
00119   return(true);
00120 }
00121 
00122 } /* namespace */


ohm_tsd_slam
Author(s): Philipp Koch, Stefan May, Markus Kühn
autogenerated on Thu Jun 6 2019 17:41:12