YARP
Yet Another Robot Platform
Map2DArea.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: BSD-3-Clause
4  */
5 
6 #include <yarp/dev/api.h>
7 #include <yarp/dev/Map2DArea.h>
8 #include <yarp/os/Bottle.h>
11 #include <sstream>
12 #include <string>
13 #include <vector>
14 #include <cassert>
15 #include <yarp/os/LogStream.h>
16 #include <random>
17 #include <algorithm>
18 
19 using namespace yarp::dev;
20 using namespace yarp::dev::Nav2D;
21 using namespace yarp::sig;
22 using namespace yarp::os;
23 using namespace yarp::math;
24 using namespace std;
25 
26 int pnpoly(std::vector<yarp::math::Vec2D<double>> points, double testx, double testy)
27 {
28  size_t i, j;
29  int c = 0;
30  for (i = 0, j = points.size() - 1; i < points.size(); j = i++)
31  {
32  if (((points[i].y>testy) != (points[j].y>testy)) &&
33  (testx < (points[j].x - points[i].x) * (testy - points[i].y) / (points[j].y - points[i].y) + points[i].x))
34  {
35  c = !c;
36  }
37  }
38  return c;
39 }
40 
41 Map2DArea::Map2DArea(const std::string& map_name, const std::vector<yarp::math::Vec2D<double>> area_points)
42 {
43  map_id = map_name;
44  points = area_points;
45 }
46 
47 Map2DArea::Map2DArea(const std::string& map_name, const std::vector<Map2DLocation> area_points)
48 {
49  map_id = map_name;
50  for (auto it = area_points.begin(); it != area_points.end(); it++)
51  {
52 #if 0
53  yAssert(it->map_id == map_name);
54 #else
55  if (it->map_id != map_name)
56  {
57  map_id = "";
58  points.clear();
59  yError() << "all area_points must belong to the same map:" << map_name;
60  return;
61  }
62 #endif
63  points.push_back(yarp::math::Vec2D<double>(it->x, it->y));
64  }
65 }
66 
68 {
69  map_id = "";
70 }
71 
73 {
74  // auto-convert text mode interaction
75  connection.convertTextMode();
76 
77  //[[maybe_unused]] int32_t dummy; //@@@FIXME To be used as soon as C++17 becomes available
78  int32_t dummy;
79 
80  dummy = connection.expectInt32();
81  assert(dummy == BOTTLE_TAG_LIST);
82  dummy = connection.expectInt32();
83 
84  dummy = connection.expectInt32();
85  assert(dummy == BOTTLE_TAG_STRING);
86  int string_size = connection.expectInt32();
87  this->map_id.resize(string_size);
88  connection.expectBlock(const_cast<char*>(this->map_id.data()), string_size);
89 
90  dummy = connection.expectInt32();
91  assert(dummy == BOTTLE_TAG_INT32);
92  size_t siz = connection.expectInt32();
93 
94  this->points.clear();
95  for (size_t i = 0; i < siz; i++)
96  {
97  dummy = connection.expectInt32();
98  assert(dummy == BOTTLE_TAG_FLOAT64);
99  double x = connection.expectFloat64();
100  dummy = connection.expectInt32();
101  assert(dummy == BOTTLE_TAG_FLOAT64);
102  double y = connection.expectFloat64();
103  this->points.push_back(yarp::math::Vec2D<double>(x, y));
104  }
105 
106  return !connection.isError();
107 }
108 
110 {
111  size_t siz = this->points.size();
112 
113  connection.appendInt32(BOTTLE_TAG_LIST);
114  connection.appendInt32(2+siz*2);
115 
116  connection.appendInt32(BOTTLE_TAG_STRING);
117  connection.appendString(map_id);
118 
119  connection.appendInt32(BOTTLE_TAG_INT32);
120  connection.appendInt32(siz);
121 
122  for (size_t i = 0; i < siz; i++)
123  {
124  connection.appendInt32(BOTTLE_TAG_FLOAT64);
125  connection.appendFloat64(this->points[i].x);
126  connection.appendInt32(BOTTLE_TAG_FLOAT64);
127  connection.appendFloat64(this->points[i].y);
128  }
129 
130  connection.convertTextMode();
131  return !connection.isError();
132 }
133 
134 std::string Map2DArea::toString() const
135 {
136  std::ostringstream stringStream;
137  stringStream.precision(-1);
138  stringStream.width(-1);
139  stringStream << std::string("map_id:") << map_id << " ";
140  for (size_t i = 0; i<points.size(); i++)
141  {
142  stringStream << " point " << i << "(" << points[i].x << "," << points[i].y << ")";
143  }
144  return stringStream.str();
145 }
146 
148 {
149  if (loc.map_id != this->map_id) {
150  return false;
151  }
152  if (points.size() < 3) {
153  return false;
154  }
155  if (pnpoly(points, loc.x, loc.y) > 0) {
156  return true;
157  }
158  return false;
159 }
160 
161 bool Map2DArea::operator!=(const Map2DArea& r) const
162 {
163  if (
164  map_id != r.map_id ||
165  points != r.points
166  )
167  {
168  return true;
169  }
170  return false;
171 }
172 
173 bool Map2DArea::operator==(const Map2DArea& r) const
174 {
175  if (
176  map_id == r.map_id &&
177  points == r.points
178  )
179  {
180  return true;
181  }
182  return false;
183 }
184 
185 bool Map2DArea::isValid() const
186 {
187  if (points.size() < 3) {
188  return false;
189  }
190  if (map_id == "") {
191  return false;
192  }
193  return true;
194 }
195 
197 {
198  lt.map_id = rb.map_id = this->map_id;
199  lt.x = lt.y = std::numeric_limits<double>::max();
200  rb.x = rb.y = std::numeric_limits<double>::min();
201  if (isValid() == false) {
202  return false;
203  }
204  for (auto it = points.begin(); it != points.end(); it++)
205  {
206  if (it->x > rb.x) { rb.x = it->x; }
207  if (it->y > rb.y) { rb.y = it->y; }
208  if (it->x < lt.x) { lt.x = it->x; }
209  if (it->y < lt.y) { lt.y = it->y; }
210  }
211  return true;
212 }
213 
215 {
216  Map2DLocation lt;
217  Map2DLocation rb;
218  if (findAreaBounds(lt, rb) == false) {
219  return false;
220  }
221 
222  std::random_device rd;
223  std::mt19937 gen(rd());
224  std::uniform_real_distribution<double> dis_x(lt.x, rb.x);
225  std::uniform_real_distribution<double> dis_y(lt.y, rb.y);
226 
227  size_t count_trials = 0;
228  do
229  {
230  double rnd_x = dis_x(gen);
231  double rnd_y = dis_y(gen);
232 
233  loc.map_id = this->map_id;
234  loc.x = rnd_x;
235  loc.y = rnd_y;
236  loc.theta = 0;
237  count_trials++;
238  if (this->checkLocationInsideArea(loc)) {
239  break;
240  }
241  } while (count_trials < 20);
242 
243  if (count_trials >= 20)
244  {
245  yError() << "Problem found in Map2DArea::getRandomLocation()";
246  return false;
247  }
248 
249  return true;
250 }
251 
253 {
254  this->map_id = "";
255  this->points.clear();
256 }
257 
259 {
260  //std::vector::at() function performs bound check, throwing exception.
261  //[] operator, instead, not.
262  return points.at(index);
263 }
#define BOTTLE_TAG_FLOAT64
Definition: Bottle.h:25
#define BOTTLE_TAG_INT32
Definition: Bottle.h:21
#define BOTTLE_TAG_STRING
Definition: Bottle.h:26
#define BOTTLE_TAG_LIST
Definition: Bottle.h:28
#define yError(...)
Definition: Log.h:279
#define yAssert(x)
Definition: Log.h:294
int pnpoly(std::vector< yarp::math::Vec2D< double >> points, double testx, double testy)
Definition: Map2DArea.cpp:26
contains the definition of a Map2DArea type
void clear()
Remove all elements from the path.
Definition: Map2DArea.cpp:252
std::string toString() const
Returns text representation of the area.
Definition: Map2DArea.cpp:134
std::vector< yarp::math::Vec2D< double > > points
Definition: Map2DArea.h:111
bool operator==(const Map2DArea &r) const
Compares two Map2DArea.
Definition: Map2DArea.cpp:173
bool operator!=(const Map2DArea &r) const
Compares two Map2DAreas.
Definition: Map2DArea.cpp:161
Map2DArea()
Default constructor: the map name is empty, coordinates are set to zero.
Definition: Map2DArea.cpp:67
bool write(yarp::os::ConnectionWriter &connection) const override
Write a map2DArea to a connection.
Definition: Map2DArea.cpp:109
bool getRandomLocation(yarp::dev::Nav2D::Map2DLocation &loc)
get a random Map2DLocation inside the Map2DArea @loc the computed Map2DLocation
Definition: Map2DArea.cpp:214
bool findAreaBounds(yarp::dev::Nav2D::Map2DLocation &lt, yarp::dev::Nav2D::Map2DLocation &rb)
retrieves two Map2DLocations representing the bounding box of the Map2DArea @lt the left-top vertex @...
Definition: Map2DArea.cpp:196
bool checkLocationInsideArea(yarp::dev::Nav2D::Map2DLocation loc)
Check if a Map2DLocation is inside a Map2DArea.
Definition: Map2DArea.cpp:147
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
Definition: Map2DArea.cpp:72
yarp::math::Vec2D< double > & operator[](size_t index)
Returns a vertex of the area.
Definition: Map2DArea.cpp:258
bool isValid() const
Checks if the Map2DArea is valid return true if the Map2DArea is valid.
Definition: Map2DArea.cpp:185
An interface for reading from a network connection.
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
virtual std::int32_t expectInt32()=0
Read a 32-bit integer from the network connection.
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary.
virtual bool isError() const =0
virtual yarp::conf::float64_t expectFloat64()=0
Read a 64-bit floating point number from the network connection.
An interface for writing to a network connection.
virtual bool isError() const =0
virtual bool convertTextMode()=0
Converts a standard description in binary into a textual description, if the connection is in text-mo...
virtual void appendInt32(std::int32_t data)=0
Send a representation of a 32-bit integer to the network connection.
virtual void appendFloat64(yarp::conf::float64_t data)=0
Send a representation of a 64-bit floating point number to the network connection.
virtual void appendString(const char *str, const char terminate='\n') final
Send a character sequence to the network connection.
An interface for the device drivers.
bool isValid()
Check if time is valid (non-zero).
Definition: Time.cpp:314
An interface to the operating system, including Port based communication.
Signal processing.
Definition: Image.h:22