ImageToGridmapDemo.cpp
Go to the documentation of this file.
00001 /*
00002  * ImageToGridmapDemo.cpp
00003  *
00004  *  Created on: May 4, 2015
00005  *      Author: Péter Fankhauser
00006  *       Institute: ETH Zurich, Autonomous Systems Lab
00007  */
00008 
00009 #include "grid_map_demos/ImageToGridmapDemo.hpp"
00010 
00011 namespace grid_map_demos {
00012 
00013 ImageToGridmapDemo::ImageToGridmapDemo(ros::NodeHandle& nodeHandle)
00014     : nodeHandle_(nodeHandle),
00015       map_(grid_map::GridMap({"elevation"})),
00016       mapInitialized_(false)
00017 {
00018   readParameters();
00019   map_.setBasicLayers({"elevation"});
00020   imageSubscriber_ = nodeHandle_.subscribe(imageTopic_, 1, &ImageToGridmapDemo::imageCallback, this);
00021   gridMapPublisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
00022 }
00023 
00024 ImageToGridmapDemo::~ImageToGridmapDemo()
00025 {
00026 }
00027 
00028 bool ImageToGridmapDemo::readParameters()
00029 {
00030   nodeHandle_.param("image_topic", imageTopic_, std::string("/image"));
00031   nodeHandle_.param("resolution", resolution_, 0.03);
00032   nodeHandle_.param("min_height", minHeight_, 0.0);
00033   nodeHandle_.param("max_height", maxHeight_, 1.0);
00034   return true;
00035 }
00036 
00037 void ImageToGridmapDemo::imageCallback(const sensor_msgs::Image& msg)
00038 {
00039   if (!mapInitialized_) {
00040     grid_map::GridMapRosConverter::initializeFromImage(msg, resolution_, map_);
00041     ROS_INFO("Initialized map with size %f x %f m (%i x %i cells).", map_.getLength().x(),
00042              map_.getLength().y(), map_.getSize()(0), map_.getSize()(1));
00043     mapInitialized_ = true;
00044   }
00045   grid_map::GridMapRosConverter::addLayerFromImage(msg, "elevation", map_, minHeight_, maxHeight_);
00046   grid_map::GridMapRosConverter::addColorLayerFromImage(msg, "color", map_);
00047 
00048   // Publish as grid map.
00049   grid_map_msgs::GridMap mapMessage;
00050   grid_map::GridMapRosConverter::toMessage(map_, mapMessage);
00051   gridMapPublisher_.publish(mapMessage);
00052 }
00053 
00054 } /* namespace */


grid_map_demos
Author(s): Péter Fankhauser
autogenerated on Mon Oct 9 2017 03:09:38