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.
typename vec2D_selector< T >::type Vec2D
Definition Vec2D.h:104
An interface to the operating system, including Port based communication.