demo.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 #include <ros/ros.h>
38 #include <nav_msgs/OccupancyGrid.h>
40 #include <vector>
41 
42 template<class Iterator>
44 {
45 public:
46  InfiniteIterator(Iterator it, unsigned char active_value, unsigned char seen_value)
47  : it_(it), active_(active_value), seen_(seen_value)
48  {
49  }
50 
52  {
53  // Note that this demo assumes that each iterator has at least one point on the grid
54  grid.setValue(*it_, seen_);
55  ++it_;
56  if (it_ == it_.end())
57  {
58  it_ = it_.begin();
59  }
60  grid.setValue(*it_, active_);
61  }
62 
63 private:
64  Iterator it_;
65  unsigned char active_, seen_;
66 };
67 
68 int main(int argc, char** argv)
69 {
70  ros::init(argc, argv, "iterator_demo");
73  sleep(5.0);
74  info.width = 75;
75  info.height = 60;
76  info.resolution = 0.1;
77  grid.setInfo(info);
78 
79  ros::NodeHandle nh("~");
80  ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 1);
81  ros::Rate r(33);
82 
83  nav_2d_msgs::Polygon2D triangle;
84  triangle.points.resize(3);
85  triangle.points[0].x = 1.9;
86  triangle.points[0].y = 0.5;
87  triangle.points[1].x = 3.5;
88  triangle.points[1].y = 1.0;
89  triangle.points[2].x = 2.0;
90  triangle.points[2].y = 2.4;
91 
92  nav_2d_msgs::Polygon2D diamond;
93  diamond.points.resize(4);
94  diamond.points[0].x = 4.5;
95  diamond.points[0].y = 0.0;
96  diamond.points[1].x = 5.3;
97  diamond.points[1].y = 1.25;
98  diamond.points[2].x = 4.5;
99  diamond.points[2].y = 2.5;
100  diamond.points[3].x = 3.7;
101  diamond.points[3].y = 1.25;
102 
103  nav_2d_msgs::Polygon2D diamond2;
104  diamond2.points.resize(4);
105  diamond2.points[0].x = 6.5;
106  diamond2.points[0].y = 0.0;
107  diamond2.points[1].x = 7.3;
108  diamond2.points[1].y = 1.25;
109  diamond2.points[2].x = 6.5;
110  diamond2.points[2].y = 2.5;
111  diamond2.points[3].x = 5.7;
112  diamond2.points[3].y = 1.25;
113 
115  InfiniteIterator<nav_grid_iterators::SubGrid> sub_grid(nav_grid_iterators::SubGrid(&info, 3, 5, 14, 15), 99, 101);
116  InfiniteIterator<nav_grid_iterators::Line> line(nav_grid_iterators::Line(&info, 7.3, 2.8, 0.2, 2.5), 126, 200);
117  InfiniteIterator<nav_grid_iterators::Line> line2(nav_grid_iterators::Line(&info, 7.3, 3.3, 0.2, 3.0, true, false),
118  126, 200);
120  254, 255);
121  InfiniteIterator<nav_grid_iterators::Spiral> spiral(nav_grid_iterators::Spiral(&info, 3.75, 4.8, 1.0), 254, 126);
123  254, 99);
127  0, 1);
128 
129  nav_msgs::OccupancyGrid ogrid;
130  ogrid.header.frame_id = info.frame_id;
131  ogrid.info = nav_2d_utils::infoToInfo(info);
132  ogrid.data.resize(info.width * info.height);
133 
134  while (ros::ok())
135  {
136  whole_grid.iterate(grid);
137  sub_grid.iterate(grid);
138  line.iterate(grid);
139  line2.iterate(grid);
140  circle_fill.iterate(grid);
141  spiral.iterate(grid);
142  circle_o.iterate(grid);
143  poly_f.iterate(grid);
144  poly_o.iterate(grid);
145  poly_r.iterate(grid);
146 
147  // Manaully creating OccupancyGrid (rather than use nav_grid_pub_sub) to avoid circular dependency
148  ogrid.header.stamp = ros::Time::now();
149 
150  unsigned int data_index = 0;
151  for (const nav_grid::Index& index : nav_grid_iterators::WholeGrid(info))
152  {
153  ogrid.data[data_index++] = grid(index);
154  }
155  pub.publish(ogrid);
156  r.sleep();
157  }
158 
159  return 0;
160 }
void iterate(nav_grid::NavGrid< unsigned char > &grid)
Definition: demo.cpp:51
virtual void setValue(const unsigned int x, const unsigned int y, const T &value)=0
void publish(const boost::shared_ptr< M > &message) const
InfiniteIterator(Iterator it, unsigned char active_value, unsigned char seen_value)
Definition: demo.cpp:46
int main(int argc, char **argv)
Definition: demo.cpp:68
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Iterates over all of the valid indexes on the outline of a polygon.
Iterates over all of the valid indexes that lie within a circle from the center out.
Definition: spiral.h:48
Iterates over all of the valid indexes that lie within a circle in row major order.
Definition: circle_fill.h:48
void setInfo(const NavGridInfo &new_info) override
ROSCPP_DECL bool ok()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
Iterates over the valid indexes that lie on the outline of a circle.
Iterates over all of the valid indexes of a line.
Definition: line.h:48
Iterator it_
Definition: demo.cpp:64
Iterates over all of the valid indexes that lie within an arbitrary polygon in row major order...
Definition: polygon_fill.h:48
Iterator for looping through every index within an aligned rectangular portion of the grid...
Definition: sub_grid.h:47
static Time now()
unsigned char seen_
Definition: demo.cpp:65
nav_grid::NavGridInfo infoToInfo(const nav_msgs::MapMetaData &metadata)
unsigned char active_
Definition: demo.cpp:65


nav_grid_iterators
Author(s):
autogenerated on Wed Jun 26 2019 20:06:20