YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
INavigation2DTest.h
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#ifndef INAVIGATION2DTEST_H
7#define INAVIGATION2DTEST_H
8
9#include <yarp/dev/IMap2D.h>
11#include <catch2/catch_amalgamated.hpp>
12#include "TestUtils.h"
13
14using namespace yarp::dev;
15using namespace yarp::dev::Nav2D;
16using namespace yarp::sig;
17using namespace yarp::os;
18
19namespace yarp::dev::tests
20{
21 inline void test_similar_angles(INavigation2D* inav, double angle1, double angle2)
22 {
23 bool b0, b1;
24 b0 = inav->setInitialPose(Map2DLocation("map_1", 10.2, 20.1, angle1)); CHECK(b0);
25 wait_safe(); // Allow some time for the command to take effect
26
27 INFO("Testing angle" + std::to_string(angle1) + " is similar to:" + std::to_string(angle2));
28 bool is_near;
29 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, angle2), is_near, 0.1, 10.0); CHECK(b1); CHECK(is_near);
30 //yInfo() << "Testing angle" << angle1 << " is similar to:" << angle2 + 5.0;
31 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, angle2 + 5.0), is_near,0.1, 10.0); CHECK(b1); CHECK(is_near);
32 //yInfo() << "Testing angle" << angle1 << " is different from:" << angle2 + 20.0;
33 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, angle2 + 20.0), is_near,0.1, 10.0); CHECK(b1); CHECK(!is_near);
34 //yInfo() << "Testing angle" << angle1 << " is similar to:" << angle2 - 5.0;
35 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, angle2 - 5.0), is_near,0.1, 10.0); CHECK(b1); CHECK(is_near);
36 //yInfo() << "Testing angle" << angle1 << " is different from:" << angle2 - 20.0;
37 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, angle2 - 20.0), is_near,0.1, 10.0); CHECK(b1); CHECK(!is_near);
38 }
39
41 {
43 Map2DLocation loc_test = Map2DLocation("map_1", 10.0, 20.0, 15);
45 Map2DLocation my_current_loc = Map2DLocation("map_1", 10.2, 20.1, 15.5);
46 Map2DLocation my_current_loc_wrong = Map2DLocation("map_1", 10000, 10000, 10000);
48 Map2DArea area_test("map_1", std::vector<Map2DLocation> {Map2DLocation("map_1", -10, -10, 0),
49 Map2DLocation("map_1", -10, +10, 0),
50 Map2DLocation("map_1", +10, +10, 0),
51 Map2DLocation("map_1", +10, -10, 0)}, "this is a test area");
52 bool b0, b1;
53 b0 = imap->storeArea("area_test", area_test); CHECK(b0);
54 b0 = imap->storeLocation("loc_test", loc_test); CHECK(b0);
55
56 {
57 bool is_inside;
58 b0 = inav->setInitialPose(my_current_loc); CHECK(b0);
59 wait_safe(); // Allow some time for the command to take effect
61 b1 = inav->checkInsideArea("area_test",is_inside); CHECK(b1); CHECK(!is_inside);
62 b1 = inav->checkInsideArea(area_test,is_inside); CHECK(b1); CHECK(!is_inside);
63 b0 = inav->setInitialPose(Map2DLocation("map_1", 0, 0, 0)); CHECK(b0);
64 wait_safe(); // Allow some time for the command to take effect
65 b0 = inav->getCurrentPosition(loc_to_be_tested); CHECK(loc_to_be_tested == Map2DLocation("map_1", 0, 0, 0));
66 b1 = inav->checkInsideArea("area_test",is_inside); CHECK(b1); CHECK(is_inside);
67 b1 = inav->checkInsideArea(area_test,is_inside); CHECK(b1); CHECK(is_inside);
68 }
69
70 {
71 std::string area_name;
73
74 b0 = inav->setInitialPose(my_current_loc_zero); CHECK(b0);
75 wait_safe(); // Allow some time for the command to take effect
76
77 b0 = inav->findCurrentArea(area_name, area_data); CHECK(b0);
78 CHECK(area_name == "area_test");
80
81 b0 = inav->setInitialPose(my_current_loc_wrong); CHECK(b0);
82 wait_safe(); // Allow some time for the command to take effect
83
84 b0 = inav->findCurrentArea(area_name, area_data); CHECK(b0);
85 CHECK(area_name == "");
87 }
88
89 {
90 double lin_tol = 1.0; //m
91 double ang_tol = 1.0; //deg
92 b0 = inav->setInitialPose(my_current_loc); CHECK(b0);
93 wait_safe(); // Allow some time for the command to take effect
94 bool is_near;
95 b1 = inav->checkNearToLocation("loc_test", is_near, lin_tol, ang_tol); CHECK(b1); CHECK(is_near);
96 b1 = inav->checkNearToLocation(loc_test, is_near, lin_tol, ang_tol); CHECK(b1); CHECK(is_near);
97 }
98 {
99 double lin_tol = 0.0001; //m
100 double ang_tol = 0.0001; //deg
101 b0 = inav->setInitialPose(my_current_loc); CHECK(b0);
102 wait_safe(); // Allow some time for the command to take effect
103 bool is_near;
104 b1 = inav->checkNearToLocation("loc_test", is_near, lin_tol, ang_tol); CHECK(b1); CHECK(!is_near);
105 b1 = inav->checkNearToLocation(loc_test, is_near, lin_tol, ang_tol); CHECK(b1); CHECK(!is_near);
106 }
107 {
108 double lin_tol = 1.0; //m
110 b0 = inav->setInitialPose(my_current_loc2); CHECK(b0);
111 wait_safe(); // Allow some time for the command to take effect
112 bool is_near;
113 b1 = inav->checkNearToLocation("loc_test", is_near, lin_tol); CHECK(b1); CHECK(is_near);
114 b1 = inav->checkNearToLocation(loc_test, is_near, lin_tol); CHECK(b1); CHECK(is_near);
115 }
116 {
117 double lin_tol = 0.1; //m
118 double ang_tol = 0.1; //deg
119 bool is_near;
120 b0 = inav->setInitialPose(my_current_loc);
121 CHECK(b0);
122 wait_safe(); // Allow some time for the command to take effect
123 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, 15.5), is_near, lin_tol, ang_tol); CHECK(b1);
124 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, 15.5 + 180), is_near, lin_tol, ang_tol); CHECK(b1); CHECK(!is_near);
125 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, 15.5 + 360), is_near, lin_tol, ang_tol); CHECK(b1); CHECK(is_near);
126 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, 15.5 + 720), is_near, lin_tol, ang_tol); CHECK(b1); CHECK(is_near);
127 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, 15.5 - 180), is_near, lin_tol, ang_tol); CHECK(b1); CHECK(!is_near);
128 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, 15.5 - 360), is_near, lin_tol, ang_tol); CHECK(b1); CHECK(is_near);
129 b1 = inav->checkNearToLocation(Map2DLocation("map_1", 10.2, 20.1, 15.5 - 720), is_near, lin_tol, ang_tol); CHECK(b1); CHECK(is_near);
130
131 //in the following tests, the tolerance is set to 10.0 deg
132 test_similar_angles(inav, +2.0, +2.0);
133 test_similar_angles(inav, -2.0, +2.0);
134 test_similar_angles(inav, +2.0, -2.0);
135 test_similar_angles(inav, -2.0, -2.0);
136
137 test_similar_angles(inav, +182.0, +182.0);
138 test_similar_angles(inav, -182.0, +182.0);
139 test_similar_angles(inav, +182.0, -182.0);
140 test_similar_angles(inav, -182.0, -182.0);
141
142 test_similar_angles(inav, 2.0, 358.0);
143 test_similar_angles(inav, -2.0, 358.0);
144 test_similar_angles(inav, 2.0, -358.0);
145 test_similar_angles(inav, -2.0, -358.0);
146
147 test_similar_angles(inav, +2.0, 718.0);
148 test_similar_angles(inav, -2.0, 718.0);
149 test_similar_angles(inav, +2.0, -718.0);
150 test_similar_angles(inav, -2.0, -718.0);
151 }
152 {
153 b0 = inav->setInitialPose(my_current_loc);
154 CHECK(b0);
155 wait_safe(); // Allow some time for the command to take effect
157 b1 = inav->getEstimatedOdometry(my_current_odom); CHECK(b1);
158 INFO("Current position is:" + my_current_loc.toString());
159 INFO("Estimated Odometry is:" + my_current_odom.toString());
160 bool bodom = fabs(my_current_loc.x - my_current_odom.odom_x) < 0.0000001 &&
161 fabs(my_current_loc.y - my_current_odom.odom_y) < 0.0000001 &&
162 fabs(my_current_loc.theta - my_current_odom.odom_theta) < 0.0000001;
163 CHECK(bodom);
164 }
165 }
166
168 {
170 bool b0, b1, b2;
171 Map2DLocation tloc("test", 1, 2, 3);
174 std::string tname("testLoc");
175 std::string gname1;
177
178 b0 = inav->storeLocation(tname, tloc); CHECK(b0);
179
180 //going to a location by absolute value
181 b1 = inav->stopNavigation(); CHECK(b1);
182 b1 = inav->getNavigationStatus(gstat1); CHECK(b1); CHECK(gstat1 == NavigationStatusEnum::navigation_status_idle);
183
184 b1 = inav->getAbsoluteLocationOfCurrentTarget(gloc1); CHECK(b1); CHECK(gloc1 == gloc_empty);
185 b1 = inav->getNameOfCurrentTarget(gname1); CHECK(b1); CHECK(gname1 == "");
186 b1 = inav->gotoTargetByAbsoluteLocation(tloc); CHECK(b1);
187 b1 = inav->getAbsoluteLocationOfCurrentTarget(gloc1); CHECK(b1); CHECK(gloc1 == tloc);
188 b1 = inav->getNameOfCurrentTarget(gname1); CHECK(b1); CHECK(gname1 == "");
189
190 //going to an existing location by name
191 b1 = inav->stopNavigation(); CHECK(b1);
192 b1 = inav->getNavigationStatus(gstat1); CHECK(b1); CHECK(gstat1 == NavigationStatusEnum::navigation_status_idle);
193
194 b1 = inav->getAbsoluteLocationOfCurrentTarget(gloc1); CHECK(b1); CHECK(gloc1 == gloc_empty);
195 b1 = inav->getNameOfCurrentTarget(gname1); CHECK(b1); CHECK(gname1 == "");
196
197 b1 = inav->gotoTargetByLocationName(tname); CHECK(b1);
198 b1 = inav->getAbsoluteLocationOfCurrentTarget(gloc1); CHECK(b1); CHECK(gloc1 == tloc);
199 b1 = inav->getNameOfCurrentTarget(gname1); CHECK(b1); CHECK(gname1 == tname);
200
201 //stop must clear navigation target name
202 b1 = inav->stopNavigation(); CHECK(b1);
203 b1 = inav->getNavigationStatus(gstat1); CHECK(b1); CHECK(gstat1 == NavigationStatusEnum::navigation_status_idle);
204 b1 = inav->getNameOfCurrentTarget(gname1); CHECK(b1); CHECK(gname1 == "");
205
206 //trying to goto to a non-existing location by name, target name must be not set
207 b1 = inav->gotoTargetByLocationName("non-existing-loc"); CHECK(b1 == false);
208 b1 = inav->getAbsoluteLocationOfCurrentTarget(gloc1); CHECK(b1); CHECK(gloc1 == gloc_empty);
209 b1 = inav->getNameOfCurrentTarget(gname1); CHECK(b1); CHECK(gname1 == "");
210
211 //mix of last two tests. A non existing location must clear a previously set target name
212 b1 = inav->gotoTargetByLocationName(tname); CHECK(b1);
213 b1 = inav->getAbsoluteLocationOfCurrentTarget(gloc1); CHECK(b1); CHECK(gloc1 == tloc);
214 b1 = inav->gotoTargetByLocationName("non-existing-loc"); CHECK(b1 == false);
215 b1 = inav->getAbsoluteLocationOfCurrentTarget(gloc1); CHECK(b1); CHECK(gloc1 == gloc_empty);
216 }
217
219 {
221 bool b;
222 Map2DLocation loc("test", 1, 2, 3);
224
225 b = inav_ctl->getNavigationStatus(status); CHECK(b);
227
228 b = inav_trgt->gotoTargetByAbsoluteLocation(loc); CHECK(b);
229 b = inav_ctl->getNavigationStatus(status); CHECK(b);
231
232 size_t count=0;
233 do
234 {
235 b = inav_ctl->getNavigationStatus(status); CHECK(b);
237 {
238 break;
239 }
240 wait_safe();
241 count++;
242 if (count>200) {CHECK(0); break; }
243 }
244 while(1);
245
246 count = 0;
247 do
248 {
249 b = inav_ctl->getNavigationStatus(status); CHECK(b);
251 {
252 break;
253 }
254 wait_safe();
255 count++;
256 if (count > 200) { CHECK(0); break; }
257 } while (1);
258
259 //test complete
260 }
261}
262
263#endif
IMap2D Interface.
Definition IMap2D.h:31
An interface to control the navigation of a mobile robot in a 2D environment.
A mini-server for performing network communication in the background.
void exec_iNav2D_test_1(INavigation2D *inav, IMap2D *imap)
void exec_iNav2D_test_3(INavigation2DTargetActions *inav_trgt, INavigation2DControlActions *inav_ctl)
void exec_iNav2D_test_2(INavigation2D *inav, IMap2D *imap)
void wait_safe(const double seconds)
Definition TestUtils.cpp:17
void test_similar_angles(INavigation2D *inav, double angle1, double angle2)
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
std::string INFO
Definition ymanager.cpp:54