resolution_change_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 
00005 using namespace grid_map;
00006 using namespace ros;
00007 
00008 int main(int argc, char** argv)
00009 {
00010   // Initialize node and publisher.
00011   init(argc, argv, "resolution_change_demo");
00012   NodeHandle nodeHandle("~");
00013   Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
00014 
00015   // Create grid map.
00016   GridMap map({"elevation"});
00017   map.setFrameId("map");
00018   map.setGeometry(Length(1.2, 2.0), 0.03);
00019   ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
00020     map.getLength().x(), map.getLength().y(),
00021     map.getSize()(0), map.getSize()(1));
00022 
00023   // Add data.
00024   map["elevation"].setZero(); // Un/comment this line to try with and without transparency.
00025   grid_map::Polygon polygon;
00026   polygon.setFrameId(map.getFrameId());
00027   polygon.addVertex(Position( 0.480,  0.000));
00028   polygon.addVertex(Position( 0.164,  0.155));
00029   polygon.addVertex(Position( 0.116,  0.500));
00030   polygon.addVertex(Position(-0.133,  0.250));
00031   polygon.addVertex(Position(-0.480,  0.399));
00032   polygon.addVertex(Position(-0.316,  0.000));
00033   polygon.addVertex(Position(-0.480, -0.399));
00034   polygon.addVertex(Position(-0.133, -0.250));
00035   polygon.addVertex(Position( 0.116, -0.500));
00036   polygon.addVertex(Position( 0.164, -0.155));
00037   polygon.addVertex(Position( 0.480,  0.000));
00038 
00039   for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) {
00040     map.at("elevation", *iterator) = 0.3;
00041   }
00042 
00043   Rate rate(10.0);
00044 
00045   // Work in a loop.
00046   while (nodeHandle.ok()) {
00047 
00048     // Initialize.
00049     ros::Time time = ros::Time::now();
00050     const double resolution = 0.05 + 0.04 * sin(time.toSec());
00051 
00052     // Change resoltion of grid map.
00053     GridMap modifiedMap;
00054     GridMapCvProcessing::changeResolution(map, modifiedMap, resolution);
00055 
00056     // Publish grid map.
00057     grid_map_msgs::GridMap message;
00058     GridMapRosConverter::toMessage(modifiedMap, message);
00059     publisher.publish(message);
00060     ROS_INFO_STREAM("Published grid map with " << resolution << " m/cell resolution.");
00061 
00062     rate.sleep();
00063   }
00064 
00065   return 0;
00066 }


grid_map_demos
Author(s): P├ęter Fankhauser
autogenerated on Tue Jul 9 2019 05:06:58