resolution_change_demo_node.cpp
Go to the documentation of this file.
1 #include <ros/ros.h>
4 
5 using namespace grid_map;
6 using namespace ros;
7 
8 int main(int argc, char** argv)
9 {
10  // Initialize node and publisher.
11  init(argc, argv, "resolution_change_demo");
12  NodeHandle nodeHandle("~");
13  Publisher publisher = nodeHandle.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  // Add data.
24  map["elevation"].setZero(); // Un/comment this line to try with and without transparency.
25  grid_map::Polygon polygon;
26  polygon.setFrameId(map.getFrameId());
27  polygon.addVertex(Position( 0.480, 0.000));
28  polygon.addVertex(Position( 0.164, 0.155));
29  polygon.addVertex(Position( 0.116, 0.500));
30  polygon.addVertex(Position(-0.133, 0.250));
31  polygon.addVertex(Position(-0.480, 0.399));
32  polygon.addVertex(Position(-0.316, 0.000));
33  polygon.addVertex(Position(-0.480, -0.399));
34  polygon.addVertex(Position(-0.133, -0.250));
35  polygon.addVertex(Position( 0.116, -0.500));
36  polygon.addVertex(Position( 0.164, -0.155));
37  polygon.addVertex(Position( 0.480, 0.000));
38 
39  for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) {
40  map.at("elevation", *iterator) = 0.3;
41  }
42 
43  Rate rate(10.0);
44 
45  // Work in a loop.
46  while (nodeHandle.ok()) {
47 
48  // Initialize.
49  ros::Time time = ros::Time::now();
50  const double resolution = 0.05 + 0.04 * sin(time.toSec());
51 
52  // Change resoltion of grid map.
53  GridMap modifiedMap;
54  GridMapCvProcessing::changeResolution(map, modifiedMap, resolution);
55 
56  // Publish grid map.
57  grid_map_msgs::GridMap message;
58  GridMapRosConverter::toMessage(modifiedMap, message);
59  publisher.publish(message);
60  ROS_INFO_STREAM("Published grid map with " << resolution << " m/cell resolution.");
61 
62  rate.sleep();
63  }
64 
65  return 0;
66 }
void publish(const boost::shared_ptr< M > &message) const
int main(int argc, char **argv)
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)
Node for comparing different normal filters performances.
Eigen::Vector2d Position
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void addVertex(const Position &vertex)
bool sleep()
#define ROS_INFO_STREAM(args)
void setFrameId(const std::string &frameId)
static bool changeResolution(const GridMap &gridMapSource, GridMap &gridMapResult, const double resolution, const int interpolationAlgorithm=cv::INTER_CUBIC)
static Time now()
bool ok() const
void setFrameId(const std::string &frameId)
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Eigen::Array2d Length


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