move_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 
00004 using namespace grid_map;
00005 using namespace ros;
00006 
00007 int main(int argc, char** argv)
00008 {
00009   // Initialize node and publisher.
00010   init(argc, argv, "move_demo");
00011   NodeHandle nh("~");
00012   Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
00013 
00014   // Create grid map.
00015   GridMap map({"layer"});
00016   map.setFrameId("map");
00017   map.setGeometry(Length(0.7, 0.7), 0.01, Position(0.0, 0.0));
00018   ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.",
00019     map.getLength().x(), map.getLength().y(),
00020     map.getSize()(0), map.getSize()(1),
00021     map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str());
00022   map["layer"].setRandom();
00023 
00024   bool useMoveMethod = true;
00025   while (nh.ok()) {
00026 
00027     if (useMoveMethod) {
00028       ROS_INFO("Using the `move(...)` method.");
00029     } else {
00030       ROS_INFO("Using the `setPosition(...)` method.");
00031     }
00032 
00033     // Work with temporary map in a loop.
00034     GridMap tempMap(map);
00035     Rate rate(10.0);
00036     ros::Time startTime = ros::Time::now();
00037     ros::Duration duration(0.0);
00038 
00039     while (duration <= ros::Duration(10.0)) {
00040       ros::Time time = ros::Time::now();
00041       duration = time - startTime;
00042 
00043       // Change position of the map with either the `move` or `setPosition` method.
00044       const double t = duration.toSec();
00045       Position newPosition = 0.03 * t * Position(cos(t), sin(t));
00046 
00047       if (useMoveMethod) {
00048         tempMap.move(newPosition);
00049       } else {
00050         tempMap.setPosition(newPosition);
00051       }
00052 
00053       // Publish grid map.
00054       tempMap.setTimestamp(time.toNSec());
00055       grid_map_msgs::GridMap message;
00056       GridMapRosConverter::toMessage(tempMap, message);
00057       publisher.publish(message);
00058       ROS_DEBUG("Grid map (duration %f) published with new position [%f, %f].",
00059                 duration.toSec(), tempMap.getPosition().x(), tempMap.getPosition().y());
00060       rate.sleep();
00061     }
00062 
00063     useMoveMethod = !useMoveMethod;
00064   }
00065 
00066   return 0;
00067 }


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