IteratorsDemo.cpp
Go to the documentation of this file.
1 /*
2  * IteratorsDemo.cpp
3  *
4  * Created on: Nov 4, 2014
5  * Author: Péter Fankhauser
6  * Institute: ETH Zurich, ANYbotics
7  */
8 
10 
11 // ROS
12 #include <geometry_msgs/PolygonStamped.h>
13 
14 using namespace std;
15 using namespace ros;
16 using namespace grid_map;
17 
18 namespace grid_map_demos {
19 
20 IteratorsDemo::IteratorsDemo(ros::NodeHandle& nodeHandle)
21  : nodeHandle_(nodeHandle),
22  map_(vector<string>({"type"}))
23 {
24  ROS_INFO("Grid map iterators demo node started.");
25  gridMapPublisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
26  polygonPublisher_ = nodeHandle_.advertise<geometry_msgs::PolygonStamped>("polygon", 1, true);
27 
28  // Setting up map.
29  map_.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));
30  map_.setFrameId("map");
31 
32  publish();
34  duration.sleep();
35 
44 }
45 
47 
49 {
50  ROS_INFO("Running grid map iterator demo.");
51  map_.clearAll();
52  publish();
53 
54  // Note: In this example we locally store a reference to the map data
55  // for improved performance. See `iterator_benchmark.cpp` and the
56  // README.md for a comparison and more information.
57  grid_map::Matrix& data = map_["type"];
58  for (grid_map::GridMapIterator iterator(map_); !iterator.isPastEnd(); ++iterator) {
59  const int i = iterator.getLinearIndex();
60  data(i) = 1.0;
61  publish();
62  ros::Duration duration(0.01);
63  duration.sleep();
64  }
65 
67  duration.sleep();
68 }
69 
71 {
72  ROS_INFO("Running submap iterator demo.");
73  map_.clearAll();
74  publish();
75 
76  Index submapStartIndex(3, 5);
77  Index submapBufferSize(12, 7);
78 
79  for (grid_map::SubmapIterator iterator(map_, submapStartIndex, submapBufferSize);
80  !iterator.isPastEnd(); ++iterator) {
81  map_.at("type", *iterator) = 1.0;
82  publish();
83  ros::Duration duration(0.02);
84  duration.sleep();
85  }
86 
88  duration.sleep();
89 }
90 
92 {
93  ROS_INFO("Running circle iterator demo.");
94  map_.clearAll();
95  publish();
96 
97  Position center(0.0, -0.15);
98  double radius = 0.4;
99 
100  for (grid_map::CircleIterator iterator(map_, center, radius);
101  !iterator.isPastEnd(); ++iterator) {
102  map_.at("type", *iterator) = 1.0;
103  publish();
104  ros::Duration duration(0.02);
105  duration.sleep();
106  }
107 
108  ros::Duration duration(1.0);
109  duration.sleep();
110 }
111 
113 {
114  ROS_INFO("Running ellipse iterator demo.");
115  map_.clearAll();
116  publish();
117 
118  Position center(0.0, -0.15);
119  Length length(0.45, 0.9);
120 
121  for (grid_map::EllipseIterator iterator(map_, center, length, M_PI_4);
122  !iterator.isPastEnd(); ++iterator) {
123  map_.at("type", *iterator) = 1.0;
124  publish();
125  ros::Duration duration(0.02);
126  duration.sleep();
127  }
128 
129  ros::Duration duration(1.0);
130  duration.sleep();
131 }
132 
134 {
135  ROS_INFO("Running spiral iterator demo.");
136  map_.clearAll();
137  publish();
138 
139  Position center(0.0, -0.15);
140  double radius = 0.4;
141 
142  for (grid_map::SpiralIterator iterator(map_, center, radius);
143  !iterator.isPastEnd(); ++iterator) {
144  map_.at("type", *iterator) = 1.0;
145  publish();
146  ros::Duration duration(0.02);
147  duration.sleep();
148  }
149 
150  ros::Duration duration(1.0);
151  duration.sleep();
152 }
153 
155 {
156  ROS_INFO("Running line iterator demo.");
157  map_.clearAll();
158  publish();
159 
160  Index start(18, 2);
161  Index end(2, 13);
162 
163  for (grid_map::LineIterator iterator(map_, start, end);
164  !iterator.isPastEnd(); ++iterator) {
165  map_.at("type", *iterator) = 1.0;
166  publish();
167  ros::Duration duration(0.02);
168  duration.sleep();
169  }
170 
171  ros::Duration duration(1.0);
172  duration.sleep();
173 }
174 
175 void IteratorsDemo::demoPolygonIterator(const bool prepareForOtherDemos)
176 {
177  ROS_INFO("Running polygon iterator demo.");
178  map_.clearAll();
179  if (prepareForOtherDemos) map_["type"].setZero();
180  publish();
181 
182  grid_map::Polygon polygon;
183  polygon.setFrameId(map_.getFrameId());
184  polygon.addVertex(Position( 0.480, 0.000));
185  polygon.addVertex(Position( 0.164, 0.155));
186  polygon.addVertex(Position( 0.116, 0.500));
187  polygon.addVertex(Position(-0.133, 0.250));
188  polygon.addVertex(Position(-0.480, 0.399));
189  polygon.addVertex(Position(-0.316, 0.000));
190  polygon.addVertex(Position(-0.480, -0.399));
191  polygon.addVertex(Position(-0.133, -0.250));
192  polygon.addVertex(Position( 0.116, -0.500));
193  polygon.addVertex(Position( 0.164, -0.155));
194  polygon.addVertex(Position( 0.480, 0.000));
195 
196  geometry_msgs::PolygonStamped message;
198  polygonPublisher_.publish(message);
199 
200  for (grid_map::PolygonIterator iterator(map_, polygon);
201  !iterator.isPastEnd(); ++iterator) {
202  map_.at("type", *iterator) = 1.0;
203  if (!prepareForOtherDemos) {
204  publish();
205  ros::Duration duration(0.02);
206  duration.sleep();
207  }
208  }
209 
210  if (!prepareForOtherDemos) {
211  ros::Duration duration(1.0);
212  duration.sleep();
213  }
214 }
215 
217 {
218  ROS_INFO("Running sliding window iterator demo.");
219  demoPolygonIterator(true);
220  publish();
221  const size_t windowSize = 3;
223  map_.add("copy", map_["type"]);
224 
225  for (grid_map::SlidingWindowIterator iterator(map_, "copy", edgeHandling, windowSize);
226  !iterator.isPastEnd(); ++iterator) {
227  map_.at("type", *iterator) = iterator.getData().meanOfFinites(); // Blurring.
228  publish();
229 
230  // Visualize sliding window as polygon.
231  grid_map::Polygon polygon;
232  Position center;
233  map_.getPosition(*iterator, center);
234  const Length windowHalfLength(Length::Constant(0.5 * (double) windowSize * map_.getResolution()));
235  polygon.addVertex(center + (Eigen::Array2d(-1.0,-1.0) * windowHalfLength).matrix());
236  polygon.addVertex(center + (Eigen::Array2d(-1.0, 1.0) * windowHalfLength).matrix());
237  polygon.addVertex(center + (Eigen::Array2d( 1.0, 1.0) * windowHalfLength).matrix());
238  polygon.addVertex(center + (Eigen::Array2d( 1.0,-1.0) * windowHalfLength).matrix());
239  polygon.setFrameId(map_.getFrameId());
240  geometry_msgs::PolygonStamped message;
242  polygonPublisher_.publish(message);
243 
244  ros::Duration duration(0.02);
245  duration.sleep();
246  }
247 }
248 
250 {
251  map_.setTimestamp(ros::Time::now().toNSec());
252  grid_map_msgs::GridMap message;
254  gridMapPublisher_.publish(message);
255  ROS_DEBUG("Grid map (timestamp %f) published.", message.info.header.stamp.toSec());
256 }
257 
258 } /* namespace */
Eigen::Array2i Index
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
grid_map::GridMap map_
Grid map data.
ros::Publisher polygonPublisher_
Polygon publisher.
ROSCPP_DECL void start()
void publish(const boost::shared_ptr< M > &message) const
ros::Publisher gridMapPublisher_
Grid map publisher.
bool getPosition(const Index &index, Position &position) const
const std::string & getFrameId() const
Eigen::MatrixXf Matrix
bool sleep() const
static void toMessage(const grid_map::GridMap &gridMap, grid_map_msgs::GridMap &message)
#define duration(a)
ros::NodeHandle & nodeHandle_
ROS nodehandle.
Node for comparing different normal filters performances.
double getResolution() const
Eigen::Vector2d Position
#define ROS_INFO(...)
static void toMessage(const grid_map::Polygon &polygon, geometry_msgs::PolygonStamped &message)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void addVertex(const Position &vertex)
float & at(const std::string &layer, const Index &index)
void add(const std::string &layer, const double value=NAN)
void demoPolygonIterator(const bool prepareForOtherDemos=false)
void setFrameId(const std::string &frameId)
static Time now()
void setFrameId(const std::string &frameId)
void setTimestamp(const Time timestamp)
Eigen::Array2d Length
#define ROS_DEBUG(...)
bool isPastEnd() const


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