opencv_demo_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <grid_map_ros/grid_map_ros.hpp>
00003 #include <grid_map_cv/grid_map_cv.hpp>
00004 #include <cv_bridge/cv_bridge.h>
00005 #include <opencv2/highgui/highgui.hpp>
00006 
00007 using namespace grid_map;
00008 using namespace ros;
00009 
00010 int main(int argc, char** argv)
00011 {
00012   // Initialize node and publisher.
00013   init(argc, argv, "opencv_demo");
00014   NodeHandle nodeHandle("~");
00015   Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
00016   const bool useTransparency = false;
00017 
00018   // Create grid map.
00019   GridMap map({"original", "elevation"});
00020   map.setFrameId("map");
00021   map.setGeometry(Length(1.2, 2.0), 0.01);
00022   ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
00023     map.getLength().x(), map.getLength().y(),
00024     map.getSize()(0), map.getSize()(1));
00025 
00026   // Add data.
00027   if (!useTransparency) map["original"].setZero();
00028   grid_map::Polygon polygon;
00029   polygon.setFrameId(map.getFrameId());
00030   polygon.addVertex(Position( 0.480,  0.000));
00031   polygon.addVertex(Position( 0.164,  0.155));
00032   polygon.addVertex(Position( 0.116,  0.500));
00033   polygon.addVertex(Position(-0.133,  0.250));
00034   polygon.addVertex(Position(-0.480,  0.399));
00035   polygon.addVertex(Position(-0.316,  0.000));
00036   polygon.addVertex(Position(-0.480, -0.399));
00037   polygon.addVertex(Position(-0.133, -0.250));
00038   polygon.addVertex(Position( 0.116, -0.500));
00039   polygon.addVertex(Position( 0.164, -0.155));
00040   polygon.addVertex(Position( 0.480,  0.000));
00041 
00042   for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) {
00043     map.at("original", *iterator) = 0.3;
00044   }
00045 
00046   // Convert to CV image.
00047   cv::Mat originalImage;
00048   if (useTransparency) {
00049     // Note: The template parameters have to be set based on your encoding
00050     // of the image. For 8-bit images use `unsigned char`.
00051     GridMapCvConverter::toImage<unsigned short, 4>(map, "original", CV_16UC4, 0.0, 0.3, originalImage);
00052   } else {
00053     GridMapCvConverter::toImage<unsigned short, 1>(map, "original", CV_16UC1, 0.0, 0.3, originalImage);
00054   }
00055 
00056   // Create OpenCV window.
00057   cv::namedWindow("OpenCV Demo");
00058 
00059   // Work with copy of image in a loop.
00060   while (nodeHandle.ok()) {
00061 
00062     // Initialize.
00063     ros::Time time = ros::Time::now();
00064     map.setTimestamp(time.toNSec());
00065     cv::Mat modifiedImage;
00066     int blurRadius = 200 - abs((int)(200.0 * sin(time.toSec())));
00067     blurRadius = blurRadius - (blurRadius % 2) + 1;
00068 
00069     // Apply Gaussian blur.
00070     cv::GaussianBlur(originalImage, modifiedImage, cv::Size(blurRadius, blurRadius), 0.0, 0.0);
00071 
00072     // Visualize as image.
00073     cv::imshow("OpenCV Demo", modifiedImage);
00074     cv::waitKey(40);
00075 
00076     // Convert resulting image to a grid map.
00077     if (useTransparency) {
00078       GridMapCvConverter::addLayerFromImage<unsigned short, 4>(modifiedImage, "elevation", map, 0.0, 0.3, 0.3);
00079     } else {
00080       GridMapCvConverter::addLayerFromImage<unsigned short, 1>(modifiedImage, "elevation", map, 0.0, 0.3);
00081     }
00082 
00083     // Publish grid map.
00084     grid_map_msgs::GridMap message;
00085     GridMapRosConverter::toMessage(map, message);
00086     publisher.publish(message);
00087     ROS_INFO_STREAM("Published image and grid map with blur radius " << blurRadius << ".");
00088   }
00089 
00090   return 0;
00091 }


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