simple_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_msgs/GridMap.h>
00004 #include <cmath>
00005 
00006 using namespace grid_map;
00007 
00008 int main(int argc, char** argv)
00009 {
00010   // Initialize node and publisher.
00011   ros::init(argc, argv, "grid_map_simple_demo");
00012   ros::NodeHandle nh("~");
00013   ros::Publisher publisher = nh.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   // Work with grid map in a loop.
00024   ros::Rate rate(30.0);
00025   while (nh.ok()) {
00026 
00027     // Add data to grid map.
00028     ros::Time time = ros::Time::now();
00029     for (GridMapIterator it(map); !it.isPastEnd(); ++it) {
00030       Position position;
00031       map.getPosition(*it, position);
00032       map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x();
00033     }
00034 
00035     // Publish grid map.
00036     map.setTimestamp(time.toNSec());
00037     grid_map_msgs::GridMap message;
00038     GridMapRosConverter::toMessage(map, message);
00039     publisher.publish(message);
00040     ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec());
00041 
00042     // Wait for next cycle.
00043     rate.sleep();
00044   }
00045 
00046   return 0;
00047 }


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