YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
MapGrid2DInfo.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#define _USE_MATH_DEFINES
7
8#include <cmath>
10
11using namespace yarp::dev;
12using namespace yarp::dev::Nav2D;
13using namespace yarp::sig;
14using namespace yarp::os;
15using namespace yarp::math;
16
17#ifndef DEG2RAD
18#define DEG2RAD M_PI/180.0
19#endif
20
22{
23 x = 0;
24 y = 0;
25 theta = 0;
26}
27
28void MapGrid2DOrigin::setOrigin(double x_init, double y_init, double t_init)
29{
30 x = x_init;
31 y = y_init;
32 theta = fmod(t_init, 360.0);
33 sa = sin (theta * DEG2RAD);
34 ca = cos (theta * DEG2RAD);
35}
36
41
43{
44 if (x != other.x) {
45 return true;
46 }
47 if (y != other.y) {
48 return true;
49 }
50 if (theta != other.theta) {
51 return true; //should I check for 360 wrap?
52 }
53 return false;
54}
55
56//--------------------------------------------------------------------
57
59{
60 m_map_name = "";
61 m_resolution = 0;
62 m_origin = MapGrid2DOrigin(0, 0, 0);
63 m_width = 0;
64 m_height = 0;
65}
66
68{
69 //convert a cell (from the upper-left corner) to the map reference frame (located in m_origin, measured in meters)
70 //beware: the location of m_origin is referred to the lower-left corner (ROS convention)
71 XYWorld v1;
72 XYWorld v2;
73 v1.x = double(cell.x) * this->m_resolution;
74 v1.y = double(cell.y) * this->m_resolution;
75 v1.x = +v1.x + m_origin.get_x() + 0 * this->m_resolution;
76 v1.y = -v1.y + m_origin.get_y() + (m_height - 1) * this->m_resolution;
77 //note that in the following operation, we are using -get_sa()
78 //this is an optimization, since sin(-a)=-sin(a) and cos(-a)=cos(a)
79 //we need -a instead of a because of we are using the inverse tranformation respect to world2cell
80 v2.x = v1.x * m_origin.get_ca() - v1.y * -m_origin.get_sa();
81 v2.y = v1.x * -m_origin.get_sa() + v1.y * m_origin.get_ca();
82 return v2;
83}
84
86{
87 //convert a world location (wrt the map reference frame located in m_origin, measured in meters), to a cell from the upper-left corner.
88 //beware: the location of m_origin is referred to the lower-left corner (ROS convention)
92 int x = int((+world2.x - this->m_origin.get_x()) / this->m_resolution) + 0;
93 int y = int((-world2.y + this->m_origin.get_y()) / this->m_resolution) + m_height - 1;
94 XYCell c;
95 c.x = (x < 0) ? 0 : x;
96 c.y = (y < 0) ? 0 : y;
97 c.x = (c.x >= m_width) ? m_width-1 : c.x;
98 c.y = (c.y >= m_height) ? m_height-1 : c.y;
99 return c;
100}
101
103{
104 //convert a world location (wrt the map reference frame located in m_origin, measured in meters), to a cell from the upper-left corner.
105 //beware: the location of m_origin is referred to the lower-left corner (ROS convention)
107 world2.x = world.x * m_origin.get_ca() - world.y * m_origin.get_sa();
108 world2.y = world.x * m_origin.get_sa() + world.y * m_origin.get_ca();
109 XYCell c;
110 c.x = int((+world2.x - this->m_origin.get_x()) / this->m_resolution) + 0;
111 c.y = int((-world2.y + this->m_origin.get_y()) / this->m_resolution) + m_height - 1;
112 return c;
113}
114
120
122{
123 //if (cell.x < 0) return false;
124 //if (cell.y < 0) return false;
125 if (cell.x >= m_width) {
126 return false;
127 }
128 if (cell.y >= m_height) {
129 return false;
130 }
131 return true;
132}
133
139
144
151
#define DEG2RAD
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
XYCell world2Cell(XYWorld world) const
bool isInsideMap(XYCell cell) const
Checks if a cell is inside the map.
XYWorld toXYWorld(yarp::dev::Nav2D::Map2DLocation loc) const
XYWorld cell2World(XYCell cell) const
yarp::dev::Nav2D::Map2DLocation toLocation(XYCell cell) const
XYCell toXYCell(yarp::dev::Nav2D::Map2DLocation loc) const
MapGrid2DOrigin m_origin
pose of the map frame w.r.t. the bottom left corner of the map image
XYCell world2Cell_unsafeFast(XYWorld world) const
double m_resolution
meters/pixel
void setOrigin(double x_init, double y_init, double t_init)
bool operator!=(const MapGrid2DOrigin &other) const
A mini-server for performing network communication in the background.
yarp::math::Vec2D< double > XYWorld
Definition NavTypes.h:17
For streams capable of holding different kinds of content, check what they actually have.
Definition jointData.cpp:13
An interface to the operating system, including Port based communication.