occupancy_grid_builder.cpp
Go to the documentation of this file.
00001 #include "pointmatcher_ros/get_params_from_server.h"
00002 #include "ethzasl_gridmap_2d/grid-map.h"
00003 #include "ethzasl_gridmap_2d/grid-functors.h"
00004 #include "tf/transform_listener.h"
00005 #include "sensor_msgs/LaserScan.h"
00006 
00007 using namespace std;
00008 
00009 struct OccupancyGridBuilder
00010 {
00011         ros::NodeHandle nh;
00012         tf::TransformListener tfListener;
00013         std::string mapFrame;
00014         const float maxRange;
00015         const int obsVal;
00016         const int freeVal;
00017         const float invalidDist;
00018         const float mapTFUpdatePeriod;
00019         GridMap::Group mapGroup;
00020         GridMap probMap;
00021         GridMap knownMap;
00022         ros::Publisher mapPub;
00023         ros::Subscriber laserScanSub;
00024         
00025         OccupancyGridBuilder() :
00026                 mapFrame(getParam<string>("map_frame", "map")),
00027                 maxRange(getParam<double>("max_range", 80.)),
00028                 obsVal(getParam<int>("obs_value", 8000)),
00029                 freeVal(getParam<int>("free_value", -8000)),
00030                 // Distance to clear map if range is invalid; 0 to deactivate
00031                 invalidDist(getParam<double>("invalid_distance", 1.)), 
00032                 // Timeout for the tf request
00033                 mapTFUpdatePeriod(getParam<double>("map_tf_update_period", 1.)), 
00034                 probMap(getParam<double>("resolution", 0.05), 0, &mapGroup),
00035                 knownMap(&mapGroup, -1),
00036                 mapPub(nh.advertise<nav_msgs::OccupancyGrid>("map", 2, true)),
00037                 laserScanSub(nh.subscribe("scan", 10, &OccupancyGridBuilder::scanReceived, this))
00038         {
00039         }
00040         
00041         void scanReceived(const sensor_msgs::LaserScan& scan)
00042         {
00043                 typedef GridMap::Vector Vector;
00044                 MapConstUpdater probUpdater(probMap, freeVal);
00045                 Drawer knownUpdater(knownMap);
00046 
00047                 const ros::Time stamp(scan.header.stamp);
00048                 tf::StampedTransform transform;
00049                 assert(scan.ranges.size());
00050                 float angle(scan.angle_min);
00051                 for (size_t a = 0; a < scan.ranges.size(); ++a) // angles indice
00052                 {
00053                         const float range(scan.ranges[a]);
00054                         float end = -1; // Non-acceptable value
00055                         bool validRange(false);
00056                         if (range >= scan.range_min && range <= scan.range_max)
00057                         {
00058                                 end = range;
00059                                 validRange = true;
00060                         }
00061                         else if (invalidDist > 0) // Distance to clear map if range is invalid
00062                                 end = invalidDist;
00063                         if (end != -1)
00064                         {
00065                                 const ros::Time scanStamp(stamp + ros::Duration(scan.time_increment)*a);
00066                                 // get transform
00067                                 tfListener.waitForTransform(mapFrame, scan.header.frame_id, scanStamp, ros::Duration(mapTFUpdatePeriod), ros::Duration(0.1));
00068                                 tfListener.lookupTransform(mapFrame, scan.header.frame_id, scanStamp, transform);
00069                                 // compute ray positions
00070                                 const tf::Vector3 rayStart = transform(tf::Vector3(0,0,0));
00071                                 const float dist(min(end, maxRange));
00072                                 const tf::Vector3 rayEnd = transform(tf::Vector3(cosf(angle) * dist, sinf(angle) * dist, 0));
00073                                 // update map
00074                                 probMap.lineScan(
00075                                         Vector(rayStart.x(), rayStart.y()), 
00076                                         Vector(rayEnd.x(), rayEnd.y()),
00077                                         probUpdater
00078                                 );
00079                                 if (validRange && (range == dist))
00080                                         probMap.addNearestValueSaturated(Vector(rayEnd.x(),
00081                                                         rayEnd.y()), obsVal-freeVal);
00082                                 knownMap.lineScan(
00083                                         Vector(rayStart.x(), rayStart.y()), 
00084                                         Vector(rayEnd.x(), rayEnd.y()),
00085                                         knownUpdater,
00086                                         0
00087                                 );
00088                         }
00089                         angle += scan.angle_increment;
00090                 }
00091                 
00092                 mapPub.publish(probMap.toOccupancyGrid(mapFrame, &knownMap));
00093         }
00094 };
00095 
00096 int main(int argc, char* argv[])
00097 {
00098         ros::init(argc, argv, "occupancy_grid_builder");
00099         OccupancyGridBuilder ogb;
00100         while (ros::ok())
00101         {
00102                 try
00103                 {
00104                         ros::spin();
00105                 }
00106                 catch (const tf::TransformException& e)
00107                 {
00108                         ROS_WARN_STREAM(e.what());
00109                 }
00110         }
00111         return 0;
00112 }


ethzasl_icp_mapper
Author(s): François Pomerleau and Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:16:21