YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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>
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
19using namespace yarp::dev;
20using namespace yarp::dev::Nav2D;
21using namespace yarp::sig;
22using namespace yarp::os;
23using namespace yarp::math;
24
25int pnpoly(std::vector<yarp::math::Vec2D<double>> points, double testx, double testy)
26{
27 size_t i, j;
28 int c = 0;
29 for (i = 0, j = points.size() - 1; i < points.size(); j = i++)
30 {
31 if (((points[i].y>testy) != (points[j].y>testy)) &&
32 (testx < (points[j].x - points[i].x) * (testy - points[i].y) / (points[j].y - points[i].y) + points[i].x))
33 {
34 c = !c;
35 }
36 }
37 return c;
38}
39
40Map2DArea::Map2DArea(const std::string& map_name, const std::vector<yarp::math::Vec2D<double>>& area_points, const std::string& desc)
41{
42 map_id = map_name;
44 description = desc;
45}
46
47Map2DArea::Map2DArea(const std::string& map_name, const std::vector<Map2DLocation>& area_points, const std::string& desc)
48{
49 map_id = map_name;
50 description = desc;
51 for (auto it = area_points.begin(); it != area_points.end(); it++)
52 {
53#if 0
54 yAssert(it->map_id == map_name);
55#else
56 if (it->map_id != map_name)
57 {
58 map_id = "";
59 points.clear();
60 yError() << "all area_points must belong to the same map:" << map_name;
61 return;
62 }
63#endif
64 points.push_back(yarp::math::Vec2D<double>(it->x, it->y));
65 }
66}
67
69{
70 map_id = "";
71}
72
73std::string Map2DArea::toString() const
74{
75 std::ostringstream stringStream;
76 stringStream.precision(-1);
77 stringStream.width(-1);
78 stringStream << std::string("map_id:") << map_id << " ";
79 for (size_t i = 0; i<points.size(); i++)
80 {
81 stringStream << " point " << i << "(" << points[i].x << "," << points[i].y << ")";
82 }
83 stringStream << std::string("desc:") << description;
84 return stringStream.str();
85}
86
88{
89 if (loc.map_id != this->map_id) {
90 return false;
91 }
92 if (points.size() < 3) {
93 return false;
94 }
95 if (pnpoly(points, loc.x, loc.y) > 0) {
96 return true;
97 }
98 return false;
99}
100
102{
103 if (obj.map_id != this->map_id) {
104 return false;
105 }
106 if (points.size() < 3) {
107 return false;
108 }
109 if (pnpoly(points, obj.x, obj.y) > 0) {
110 return true;
111 }
112 return false;
113}
114
116{
117 if (
118 map_id != r.map_id ||
119 points != r.points
120 )
121 {
122 return true;
123 }
124 return false;
125}
126
128{
129 if (
130 map_id == r.map_id &&
131 points == r.points
132 )
133 {
134 return true;
135 }
136 return false;
137}
138
140{
141 if (points.size() < 3) {
142 return false;
143 }
144 if (map_id == "") {
145 return false;
146 }
147 return true;
148}
149
151{
154 bool ret = findAreaBounds (lt,rb);
155 if (ret == true)
156 {
157 cent.map_id=this->map_id;
158 cent.x = (rb.x - lt.x) / 2.0;
159 cent.y = (rb.y - lt.y) / 2.0;
160 return true;
161 }
162 return false;
163}
164
166{
167 lt.map_id = rb.map_id = this->map_id;
168 lt.x = lt.y = std::numeric_limits<double>::max();
169 rb.x = rb.y = std::numeric_limits<double>::min();
170 if (isValid() == false) {
171 return false;
172 }
173 for (auto it = points.begin(); it != points.end(); it++)
174 {
175 if (it->x > rb.x) { rb.x = it->x; }
176 if (it->y > rb.y) { rb.y = it->y; }
177 if (it->x < lt.x) { lt.x = it->x; }
178 if (it->y < lt.y) { lt.y = it->y; }
179 }
180 return true;
181}
182
184{
187 if (findAreaBounds(lt, rb) == false) {
188 return false;
189 }
190
191 std::random_device rd;
192 std::mt19937 gen(rd());
193 std::uniform_real_distribution<double> dis_x(lt.x, rb.x);
194 std::uniform_real_distribution<double> dis_y(lt.y, rb.y);
195
196 size_t count_trials = 0;
197 do
198 {
199 double rnd_x = dis_x(gen);
200 double rnd_y = dis_y(gen);
201
202 loc.map_id = this->map_id;
203 loc.x = rnd_x;
204 loc.y = rnd_y;
205 loc.theta = 0;
206 count_trials++;
207 if (this->checkLocationInsideArea(loc)) {
208 break;
209 }
210 } while (count_trials < 20);
211
212 if (count_trials >= 20)
213 {
214 yError() << "Problem found in Map2DArea::getRandomLocation()";
215 return false;
216 }
217
218 return true;
219}
220
222{
223 this->description="";
224 this->map_id = "";
225 this->points.clear();
226}
227
229{
230 //std::vector::at() function performs bound check, throwing exception.
231 //[] operator, instead, not.
232 return points.at(index);
233}
#define yError(...)
Definition Log.h:361
#define yAssert(x)
Definition Log.h:388
int pnpoly(std::vector< yarp::math::Vec2D< double > > points, double testx, double testy)
Definition Map2DArea.cpp:25
contains the definition of a Map2DArea type
std::string description
user defined string
std::string map_id
name of the map
std::vector< yarp::math::Vec2D< double > > points
list of points which define the vertices of the area
std::string map_id
name of the map
double theta
orientation [deg] in the map reference frame
double y
y position of the location [m], expressed in the map reference frame
double x
x position of the location [m], expressed in the map reference frame
double y
y position of the location [m], expressed in the map reference frame
std::string map_id
name of the map
double x
x position of the location [m], expressed in the map reference frame
void clear()
Remove all elements from the path.
std::string toString() const
Returns text representation of the area.
Definition Map2DArea.cpp:73
bool checkObjectInsideArea(yarp::dev::Nav2D::Map2DObject obj)
Check if a Map2DObject is inside a Map2DArea.
bool operator==(const Map2DArea &r) const
Compares two Map2DArea.
bool operator!=(const Map2DArea &r) const
Compares two Map2DAreas.
Map2DArea()
Default constructor: the map name is empty, coordinates are set to zero.
Definition Map2DArea.cpp:68
bool getRandomLocation(yarp::dev::Nav2D::Map2DLocation &loc)
get a random Map2DLocation inside the Map2DArea @loc the computed Map2DLocation
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 @...
bool checkLocationInsideArea(yarp::dev::Nav2D::Map2DLocation loc)
Check if a Map2DLocation is inside a Map2DArea.
Definition Map2DArea.cpp:87
bool getCentroid(yarp::dev::Nav2D::Map2DLocation &cent)
retrieves the centroid of the area @cent the centroid
yarp::math::Vec2D< double > & operator[](size_t index)
Returns a vertex of the area.
bool isValid() const
Checks if the Map2DArea is valid return true if the Map2DArea is valid.
A mini-server for performing network communication in the background.
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
Definition OSTestMsg.cpp:13