simple_demo_node.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
3 #include <grid_map_msgs/GridMap.h>
4 #include <cmath>
5 
6 using namespace grid_map;
7 
8 int main(int argc, char** argv)
9 {
10  // Initialize node and publisher.
11  ros::init(argc, argv, "grid_map_simple_demo");
12  ros::NodeHandle nh("~");
13  ros::Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
14 
15  // Create grid map.
16  GridMap map({"elevation"});
17  map.setFrameId("map");
18  map.setGeometry(Length(1.2, 2.0), 0.03);
19  ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
20  map.getLength().x(), map.getLength().y(),
21  map.getSize()(0), map.getSize()(1));
22 
23  // Work with grid map in a loop.
24  ros::Rate rate(30.0);
25  while (nh.ok()) {
26 
27  // Add data to grid map.
28  ros::Time time = ros::Time::now();
29  for (GridMapIterator it(map); !it.isPastEnd(); ++it) {
30  Position position;
31  map.getPosition(*it, position);
32  map.at("elevation", *it) = -0.04 + 0.2 * std::sin(3.0 * time.toSec() + 5.0 * position.y()) * position.x();
33  }
34 
35  // Publish grid map.
36  map.setTimestamp(time.toNSec());
37  grid_map_msgs::GridMap message;
38  GridMapRosConverter::toMessage(map, message);
39  publisher.publish(message);
40  ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", message.info.header.stamp.toSec());
41 
42  // Wait for next cycle.
43  rate.sleep();
44  }
45 
46  return 0;
47 }
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
static void toMessage(const grid_map::GridMap &gridMap, grid_map_msgs::GridMap &message)
int main(int argc, char **argv)
Node for comparing different normal filters performances.
Eigen::Vector2d Position
#define ROS_INFO(...)
uint64_t toNSec() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
static Time now()
#define ROS_INFO_THROTTLE(rate,...)
bool ok() const
void setFrameId(const std::string &frameId)
Eigen::Array2d Length


grid_map_demos
Author(s): Péter Fankhauser
autogenerated on Tue Jun 1 2021 02:13:55