YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
FakeLaser.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 "FakeLaser.h"
9
10#include <yarp/os/Time.h>
11#include <yarp/os/Log.h>
12#include <yarp/os/LogStream.h>
14#include <yarp/math/Vec2D.h>
15#include <iostream>
16#include <limits>
17#include <cstring>
18#include <cstdlib>
19#include <cmath>
20
21//#define LASER_DEBUG
22#ifndef DEG2RAD
23#define DEG2RAD M_PI/180.0
24#endif
25
26YARP_LOG_COMPONENT(FAKE_LASER, "yarp.devices.fakeLaser")
27
30using namespace yarp::dev::Nav2D;
31
32bool FakeLaser::open(yarp::os::Searchable& config)
33{
34 if (!this->parseParams(config)) {return false;}
35
36 m_info = "Fake Laser device for test/debugging";
37 m_device_status = DEVICE_OK_STANDBY;
38
39#ifdef LASER_DEBUG
40 yCDebug(FAKE_LASER) << "%s\n", config.toString().c_str());
41#endif
42
43 if (config.check("help"))
44 {
45 yCInfo(FAKE_LASER,"Some examples:");
46 yCInfo(FAKE_LASER,"yarpdev --device fakeLaser --help");
47 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test no_obstacles");
48 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_pattern");
49 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_constant --const_distance 0.5");
50 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_constant --const_distance 0.5 --SENSOR::resolution 0.5 --SKIP::min 0 50 --SKIP::max 10 60");
51 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map");
52 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map --localization_port /fakeLaser/location:i");
53 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map --localization_server /localizationServer");
54 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map --localization_client /fakeLaser/localizationClient --localization_server /localizationServer");
55 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_mapfile --map_context context --map_file mymap.map");
56 yCInfo(FAKE_LASER,"yarpdev --device rangefinder2D_nws_yarp --subdevice fakeLaser --period 0.01 --name /fakeLaser:o --test use_mapfile --map_file mymap.map --localization_client /fakeLaser/localizationClient --localization_server /localization2D_nws_yarp --localization_device localization2D_nwc_yarp");
57 return false;
58 }
59
60 std::string string_test_mode = m_test;
61 if (string_test_mode == "no_obstacles") { m_test_mode = NO_OBSTACLES; }
62 else if (string_test_mode == "use_pattern") { m_test_mode = USE_PATTERN; }
63 else if (string_test_mode == "use_mapfile") { m_test_mode = USE_MAPFILE; }
64 else if (string_test_mode == "use_constant") { m_test_mode = USE_CONSTANT_VALUE; }
65 else if (string_test_mode == "use_square_trap") { m_test_mode = USE_SQUARE_TRAP; }
66 else { yCError(FAKE_LASER) << "invalid/unknown value for param 'test'"; return false; }
67
68 //parse all the parameters related to the linear/angular range of the sensor
69 if (this->parseConfiguration(config) == false)
70 {
71 yCError(FAKE_LASER) << ": error parsing parameters";
72 return false;
73 }
74
75 if (m_test_mode == USE_SQUARE_TRAP)
76 {
77 m_robot_loc_x = 0;
78 m_robot_loc_y = 0;
79 m_robot_loc_t = 0;
80 m_map.m_map_name = "test_map_10x10m";
81 m_map.m_resolution = 0.01; //m/pixel
82 m_map.m_origin.setOrigin(-5,-5,0); //m
83 m_map.setSize_in_meters(10,10);
84 for (size_t y = 0; y < m_map.m_height; y++)
85 {
86 for (size_t x = 0; x < m_map.m_width; x++)
87 {
88 m_map.setOccupancyData(yarp::dev::Nav2D::XYCell(x, y),0);
89 m_map.setMapFlag(yarp::dev::Nav2D::XYCell(x, y), MapGrid2D::map_flags::MAP_CELL_FREE);
90 }
91 }
92 m_originally_loaded_map = m_map;
93 trap_the_robot(3); //3m
95 m_map.saveToFile("mio1");
96 }
97 else if (m_test_mode == USE_MAPFILE)
98 {
99 std::string map_file;
100 if (!m_MAP_MODE_map_context.empty() && !m_MAP_MODE_map_file.empty())
101 {
103 std::string tmp_filename = m_MAP_MODE_map_file;
104 std::string tmp_contextname = m_MAP_MODE_map_context;
107 bool b = rf.configure(0, nullptr);
108 if (b)
109 {
111 if (map_file == "")
112 {
113 yCWarning(FAKE_LASER, "Unable to find file: %s from context: %s\n", tmp_filename.c_str(), tmp_contextname.c_str());
114 }
115 }
116 else
117 {
118 yCWarning(FAKE_LASER, "Unable to find file: %s from context: %s\n", tmp_filename.c_str(), tmp_contextname.c_str());
119 }
120 }
121 else if (!m_MAP_MODE_map_file.empty())
122 {
123 map_file = m_MAP_MODE_map_file;
124 }
125 else
126 {
127 yCError(FAKE_LASER) << "Missing `map_file` or `map_context`+`map_file` parameters";
128 return false;
129 }
130
131 if (map_file=="")
132 {
133 yCError(FAKE_LASER) << "File not found";
134 return false;
135 }
136 bool ret = m_originally_loaded_map.loadFromFile(map_file);
137 if (ret == false)
138 {
139 yCError(FAKE_LASER) << "A problem occurred while opening:" << map_file;
140 return false;
141 }
142 m_map = m_originally_loaded_map;
143
144 if (!m_localization_port.empty())
145 {
146 std::string localization_port_name = config.check("localization_port", Value(std::string("/fakeLaser/location:i")), "name of localization input port").asString();
148 m_loc_port->open(localization_port_name);
149 yCInfo(FAKE_LASER) << "Robot localization will be obtained from port" << localization_port_name;
150 m_loc_mode = LOC_FROM_PORT;
151 }
152 else if (!m_localization_client.empty() ||
153 !m_localization_server.empty() ||
154 !m_localization_device.empty())
155 {
157 std::string localization_client_name = m_localization_client;
158 std::string localization_server_name = m_localization_server;
159 std::string localization_device_name = m_localization_device;
163 m_pLoc = new PolyDriver;
164 if (m_pLoc->open(loc_options) == false)
165 {
166 yCError(FAKE_LASER) << "Unable to open localization driver";
167 return false;
168 }
169 m_pLoc->view(m_iLoc);
170 if (m_iLoc == nullptr)
171 {
172 yCError(FAKE_LASER) << "Unable to open localization interface";
173 return false;
174 }
175 yCInfo(FAKE_LASER) << "Robot localization will be obtained from localization server: " << localization_server_name;
176 m_loc_mode = LOC_FROM_CLIENT;
177 }
178 else
179 {
180 yCInfo(FAKE_LASER) << "No localization method selected. Robot location set to 0,0,0";
181 m_loc_mode = LOC_NOT_SET;
182 }
183
184 m_robot_loc_x=0;
185 m_robot_loc_y=0;
186 m_robot_loc_t=0;
187 }
188
189 yCInfo(FAKE_LASER) << "Starting debug mode";
190 yCInfo(FAKE_LASER) << "test mode:"<< m_test_mode << " i.e. " << string_test_mode;
191
192 if (!m_rpcPort.open(m_rpc_test_port))
193 {
194 yCError(FAKE_LASER, "Failed to open port %s", m_rpc_test_port.c_str());
195 return false;
196 }
197 m_rpcPort.setReader(*this);
198
199 return PeriodicThread::start();
200}
201
203{
205
206 if (m_loc_port)
207 {
209 m_loc_port->close();
210 }
211 return true;
212}
213
215{
216 m_mutex.lock();
217 m_min_distance = min;
218 m_max_distance = max;
219 m_mutex.unlock();
220 return ReturnValue_ok;
221}
222
224{
225 m_mutex.lock();
226 m_min_angle = min;
227 m_max_angle = max;
228 m_mutex.unlock();
229 return ReturnValue_ok;
230}
231
233{
234 m_mutex.lock();
236 m_mutex.unlock();
237 return ReturnValue_ok;
238}
239
241{
242 m_mutex.lock();
243 m_GENERAL_period = (1.0 / rate);
244 m_mutex.unlock();
245 return ReturnValue_ok;
246}
247
249{
250#ifdef LASER_DEBUG
251 yCDebug(FAKE_LASER)<<"thread initialising...\n");
252 yCDebug(FAKE_LASER)<<"... done!\n");
253#endif
254
255 return true;
256}
257
258
259bool FakeLaser::LiangBarsky_clip(int edgeLeft, int edgeRight, int edgeTop, int edgeBottom,
262{
263 double t0 = 0.0; double t1 = 1.0;
264 double xdelta = double(dst.x - src.x);
265 double ydelta = double(dst.y - src.y);
266 double p, q, r;
267
268 for (int edge = 0; edge < 4; edge++)
269 {
270 if (edge == 0) { p = -xdelta; q = -(edgeLeft - src.x); }
271 if (edge == 1) { p = xdelta; q = (edgeRight - src.x); }
272 if (edge == 2) { p = -ydelta; q = -(edgeTop - src.y); }
273 if (edge == 3) { p = ydelta; q = (edgeBottom - src.y); }
274 r = q / p;
275 if (p == 0 && q < 0) {return false;} //line is outside (parallel)
276
277 if (p < 0)
278 {
279 if (r > t1) {return false;} //line is outside.
280 else if (r > t0) {t0 = r;} //line is clipped
281 }
282 else if (p > 0)
283 {
284 if (r < t0) {return false;} //line is outside.
285 else if (r < t1) {t1 = r;} //line is clipped
286 }
287 }
288
289 src_clipped.x = size_t(double(src.x) + t0 * xdelta);
290 src_clipped.y = size_t(double(src.y) + t0 * ydelta);
291 dst_clipped.x = size_t(double(src.x) + t1 * xdelta);
292 dst_clipped.y = size_t(double(src.y) + t1 * ydelta);
293
294 return true; //line is clipped
295}
296
298{
300 double t = yarp::os::Time::now();
301 static double t_orig = yarp::os::Time::now();
302 double size = (t - (t_orig));
303
304 static int test_count = 0;
305 static int test = 0;
306
308 {
309 for (size_t i = 0; i < m_sensorsNum; i++)
310 {
311 double value = 0;
312 if (test == 0)
313 {
314 value = i / 100.0;
315 }
316 else if (test == 1)
317 {
318 value = size * 2;
319 }
320 else if (test == 2)
321 {
322 if (i <= 10) { value = 1.0 + i / 20.0; }
323 else if (i >= 90 && i <= 100) { value = 2.0 + (i - 90) / 20.0; }
324 else { value = m_min_distance; }
325 }
326 m_laser_data.push_back(value);
327 }
328
329 test_count++;
330 if (test_count == 60)
331 {
332 test_count = 0; test++; if (test > 2) { test = 0; }
334 }
335 }
336 else if (m_test_mode == NO_OBSTACLES)
337 {
338 for (size_t i = 0; i < m_sensorsNum; i++)
339 {
340 m_laser_data.push_back(std::numeric_limits<double>::infinity());
341 }
342 }
344 {
345 for (size_t i = 0; i < m_sensorsNum; i++)
346 {
348 }
349 }
350 else if (m_test_mode == USE_MAPFILE)
351 {
352 if (m_loc_mode == LOC_NOT_SET)
353 {
354 //do nothing
355 }
356 else if (m_loc_mode == LOC_FROM_PORT)
357 {
358 Bottle* b = m_loc_port->read(false);
359 if (b)
360 {
361 m_robot_loc_x = b->get(0).asFloat64();
362 m_robot_loc_y = b->get(1).asFloat64();
363 m_robot_loc_t = b->get(2).asFloat64();
364 }
365 }
366 else if (m_loc_mode == LOC_FROM_CLIENT)
367 {
368 Map2DLocation loc;
369 if (m_iLoc->getCurrentPosition(loc))
370 {
371 m_robot_loc_x = loc.x;
372 m_robot_loc_y = loc.y;
373 m_robot_loc_t = loc.theta;
374 }
375 else
376 {
377 yCError(FAKE_LASER) << "Error occurred while getting current position from localization server";
378 }
379 }
380 else
381 {
382 yCDebug(FAKE_LASER) << "No localization mode selected. This branch should be not reachable.";
383 }
384
385 for (size_t i = 0; i < m_sensorsNum; i++)
386 {
387 //compute the ray in the robot reference frame
388 double ray_angle = i * m_resolution + m_min_angle;
391
392 //transforms the ray from the robot to the world reference frame
397
398 //beware! src is the robot position and it is always inside the image.
399 //dst is the end of the ray, and it can be out of the image!
400 //for this reason i am not going to call world2Cell() on dst, because that functions clips the point to the border without
401 //knowing the angular coefficient of the ray. I thus need the unclipped value, and run the LiangBarsky algorithm
402 //(which knows the angular coefficient of the ray) on it.
407 dst.x = int((+ray_world_rot.x - this->m_map.m_origin.get_x()) / this->m_map.m_resolution) + 0;
408 dst.y = int((-ray_world_rot.y + this->m_map.m_origin.get_y()) / this->m_map.m_resolution) + (int)m_map.m_height - 1;
409
412 double distance;
413 if (LiangBarsky_clip(0, (int)m_map.width(), 0, (int)m_map.height(),
414 XYCell_unbounded(src.x, src.y), dst, newsrc, newdst))
415 {
416 distance = checkStraightLine(src, newdst);
417 double simulated_noise = (*m_dis)(*m_gen);
419 }
420 else
421 {
422 //This should never happen, unless the robot is outside the map!
423 yDebug() << "Robot is outside the map?!";
424 m_laser_data.push_back(std::numeric_limits<double>::infinity());
425 }
426
427 }
428 }
429
430 return true;
431}
432
434{
436 return;
437}
438
439void FakeLaser::wall_the_robot(double siz, double dist)
440{
441 //double res;
442 //m_map.getResolution(res);
443 //size_t siz_cell = siz / res;
444 //size_t dist_cell = dist / res;
446
448 XYWorld start (0+dist, 0 - siz);
452
454 XYWorld end(0 + dist, 0 + siz);
458
459 drawStraightLine(start_cell,end_cell);
460}
461
462void FakeLaser::obst_the_robot(double siz, double dist)
463{
464 //NOT YET IMPLEMENTED
465 /*double res;
466 m_map.getResolution(res);
467 size_t siz_cell = size_t(siz / res);
468 size_t dist_cell = size_t(dist / res);
469 XYCell robot = m_map.world2Cell(XYWorld(m_robot_loc_x, m_robot_loc_y));*/
470}
471
472void FakeLaser::trap_the_robot(double siz)
473{
474 double res;
475 m_map.getResolution(res);
476 size_t siz_cell = size_t(siz / res);
478 for (size_t x= robot.x- siz_cell; x< robot.x + siz_cell; x++)
479 {
480 size_t y=robot.y- siz_cell;
482 }
483 for (size_t x = robot.x - siz_cell; x < robot.x + siz_cell; x++)
484 {
485 size_t y = robot.y + siz_cell;
487 }
488 for (size_t y = robot.y - siz_cell; y < robot.y + siz_cell; y++)
489 {
490 size_t x = robot.x - siz_cell;
492 }
493 for (size_t y = robot.y - siz_cell; y < robot.y + siz_cell; y++)
494 {
495 size_t x = robot.x + siz_cell;
497 }
498}
499
500void FakeLaser::free_the_robot()
501{
503}
504
506{
507 yarp::os::Bottle command;
508 yarp::os::Bottle reply;
509 bool ok = command.read(connection);
510 if (!ok) {
511 return false;
512 }
513 reply.clear();
514
515 if (command.get(0).asString() == "trap")
516 {
517 if (command.size() == 1)
518 {
519 trap_the_robot();
520 reply.addVocab32(VOCAB_OK);
521 }
522 else if (command.size() == 2)
523 {
524 trap_the_robot(command.get(1).asFloat64());
525 reply.addVocab32(VOCAB_OK);
526 }
527 else
528 {
529 reply.addVocab32(VOCAB_ERR);
530 }
531 }
532 else if (command.get(0).asString() == "wall")
533 {
534 if (command.size() == 1)
535 {
536 wall_the_robot(1.0, 1.0);
537 wall_the_robot(1.0, 1.05);
538 reply.addVocab32(VOCAB_OK);
539 }
540 else if (command.size() == 2)
541 {
542 wall_the_robot(command.get(1).asFloat64(), 1.0);
543 wall_the_robot(command.get(1).asFloat64(), 1.05);
544 reply.addVocab32(VOCAB_OK);
545 }
546 else if (command.size() == 3)
547 {
548 wall_the_robot(command.get(1).asFloat64(), command.get(2).asFloat64());
549 wall_the_robot(command.get(1).asFloat64(), command.get(2).asFloat64()+0.05);
550 reply.addVocab32(VOCAB_OK);
551 }
552 else
553 {
554 reply.addVocab32(VOCAB_ERR);
555 }
556 }
557 else if (command.get(0).asString() == "free")
558 {
559 free_the_robot();
560 reply.addVocab32(VOCAB_OK);
561 }
562 else if (command.get(0).asString() == "help")
563 {
564 reply.addVocab32("many");
565 reply.addString("wall <size> <distance>: creates a wall in front of the robot");
566 reply.addString("trap <size>: traps the robot in a box obstacle");
567 reply.addString("free: removes all generated obstacles");
568 }
569 else
570 {
571 yCError(FAKE_LASER) << "Invalid command";
572 reply.addVocab32(VOCAB_ERR);
573 }
574
576 if (returnToSender != nullptr)
577 {
578 reply.write(*returnToSender);
579 }
580 return true;
581}
582
583
584void FakeLaser::drawStraightLine(XYCell src, XYCell dst)
585{
586 long int x, y;
587 long int dx, dy, dx1, dy1, px, py, xe, ye, i;
588 dx = (long int)dst.x - (long int)src.x;
589 dy = (long int)dst.y - (long int)src.y;
590 dx1 = abs(dx);
591 dy1 = abs(dy);
592 px = 2 * dy1 - dx1;
593 py = 2 * dx1 - dy1;
594 if (dy1 <= dx1)
595 {
596 if (dx >= 0)
597 {
598 x = (long int)src.x;
599 y = (long int)src.y;
600 xe = (long int)dst.x;
601 }
602 else
603 {
604 x = (long int)dst.x;
605 y = (long int)dst.y;
606 xe = (long int)src.x;
607 }
609 for (i = 0; x < xe; i++)
610 {
611 x = x + 1;
612 if (px < 0)
613 {
614 px = px + 2 * dy1;
615 }
616 else
617 {
618 if ((dx < 0 && dy < 0) || (dx > 0 && dy > 0))
619 {
620 y = y + 1;
621 }
622 else
623 {
624 y = y - 1;
625 }
626 px = px + 2 * (dy1 - dx1);
627 }
629 }
630 }
631 else
632 {
633 if (dy >= 0)
634 {
635 x = (long int)src.x;
636 y = (long int)src.y;
637 ye = (long int)dst.y;
638 }
639 else
640 {
641 x = (long int)dst.x;
642 y = (long int)dst.y;
643 ye = (long int)src.y;
644 }
646 for (i = 0; y < ye; i++)
647 {
648 y = y + 1;
649 if (py <= 0)
650 {
651 py = py + 2 * dx1;
652 }
653 else
654 {
655 if ((dx < 0 && dy < 0) || (dx > 0 && dy > 0))
656 {
657 x = x + 1;
658 }
659 else
660 {
661 x = x - 1;
662 }
663 py = py + 2 * (dx1 - dy1);
664 }
666 }
667 }
668}
669
670double FakeLaser::checkStraightLine(XYCell src, XYCell dst)
671{
672 //BEWARE: src and dest must be already clipped and >0 in this function
674 test_point.x = src.x;
675 test_point.y = src.y;
676
677 //here using the fast Bresenham algorithm
678 int dx = abs(int(dst.x - src.x));
679 int dy = abs(int(dst.y - src.y));
680 int err = dx - dy;
681
682 int sx;
683 int sy;
684 if (src.x < dst.x) { sx = 1; } else { sx = -1; }
685 if (src.y < dst.y) { sy = 1; } else { sy = -1; }
686
687 while (true)
688 {
689 //the test point is going to move from src until it reaches dst OR
690 //if it reaches the boundaries of the image
691 if (test_point.x==0 || test_point.y ==0 || test_point.x>=m_map.width() || test_point.y>=m_map.height())
692 {
693 break;
694 }
695
696 //if (m_map.isFree(src) == false)
698 {
701 double dist = sqrt(pow(world_start.x - world_end.x, 2) + pow(world_start.y - world_end.y, 2));
702 return dist;
703 }
704
705 if (test_point.x == dst.x && test_point.y == dst.y)
706 {
707 break;
708 }
709
710 int e2 = err * 2;
711 if (e2 > -dy)
712 {
713 err = err - dy;
714 test_point.x += sx;
715 }
716 if (e2 < dx)
717 {
718 err = err + dx;
719 test_point.y += sy;
720 }
721 }
722 return std::numeric_limits<double>::infinity();
723}
724
726{
727#ifdef LASER_DEBUG
728 yCDebug(FAKE_LASER) << "FakeLaser Thread releasing...");
729 yCDebug(FAKE_LASER) << "... done.");
730#endif
731}
const yarp::os::LogComponent & FAKE_LASER()
Definition FakeLaser.cpp:26
constexpr yarp::conf::vocab32_t VOCAB_OK
constexpr yarp::conf::vocab32_t VOCAB_ERR
size_t size_t
#define DEG2RAD
#define yDebug(...)
Definition Log.h:275
#define ReturnValue_ok
Definition ReturnValue.h:82
fakeLaser : fake sensor device driver for testing purposes and reference for IRangefinder2D devices.
Definition FakeLaser.h:49
double m_robot_loc_y
Definition FakeLaser.h:65
double m_robot_loc_x
Definition FakeLaser.h:64
yarp::dev::ReturnValue setScanRate(double rate) override
set the scan rate (scans per seconds)
void run() override
Loop function.
yarp::dev::Nav2D::MapGrid2D m_map
Definition FakeLaser.h:58
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
double m_robot_loc_t
Definition FakeLaser.h:66
@ USE_CONSTANT_VALUE
Definition FakeLaser.h:51
@ NO_OBSTACLES
Definition FakeLaser.h:51
@ USE_MAPFILE
Definition FakeLaser.h:51
@ USE_PATTERN
Definition FakeLaser.h:51
bool acquireDataFromHW() override final
This method should be implemented by the user, and contain the logic to grab data from the hardware.
yarp::dev::Nav2D::ILocalization2D * m_iLoc
Definition FakeLaser.h:61
yarp::dev::ReturnValue setScanLimits(double min, double max) override
set the scan angular range.
localization_mode_t m_loc_mode
Definition FakeLaser.h:55
bool threadInit() override
Initialization method.
yarp::dev::ReturnValue setDistanceRange(double min, double max) override
set the device detection range.
yarp::os::BufferedPort< yarp::os::Bottle > * m_loc_port
Definition FakeLaser.h:59
test_mode_t m_test_mode
Definition FakeLaser.h:54
yarp::dev::ReturnValue setHorizontalResolution(double step) override
get the angular step between two measurements (if available)
bool close() override
Close the DeviceDriver.
void threadRelease() override
Release method.
yarp::dev::Nav2D::MapGrid2D m_originally_loaded_map
Definition FakeLaser.h:57
@ LOC_NOT_SET
Definition FakeLaser.h:52
@ LOC_FROM_CLIENT
Definition FakeLaser.h:52
@ LOC_FROM_PORT
Definition FakeLaser.h:52
virtual bool updateLidarData()
This utility method calls in sequence: grabDataFromHW(), updateTimestamp and applyLimitsOnLaserData()...
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
virtual yarp::dev::ReturnValue getCurrentPosition(yarp::dev::Nav2D::Map2DLocation &loc)=0
Gets the current position of the robot w.r.t world reference frame.
XYCell world2Cell(XYWorld world) const
XYWorld cell2World(XYCell cell) const
MapGrid2DOrigin m_origin
pose of the map frame w.r.t. the bottom left corner of the map image
size_t height() const
Retrieves the map height, expressed in cells.
bool setMapFlag(XYCell cell, map_flags flag)
Set the flag of a specific cell of the map.
size_t width() const
Retrieves the map width, expressed in cells.
bool isWall(XYCell cell) const
Checks if a specific cell of the map contains a wall.
void getResolution(double &resolution) const
Retrieves the resolution of the map, i.e.
A container for a device driver.
Definition PolyDriver.h:23
A simple collection of objects that can be described and transmitted in a portable way.
Definition Bottle.h:65
void addVocab32(yarp::conf::vocab32_t x)
Places a 32 bit vocabulary item in the bottle, at the end of the list.
Definition Bottle.cpp:164
size_type size() const
Gets the number of elements in the bottle.
Definition Bottle.cpp:257
bool read(ConnectionReader &reader) override
Set the bottle's value based on input from a network connection.
Definition Bottle.cpp:246
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:252
void clear()
Empties the bottle of any objects it contains.
Definition Bottle.cpp:121
bool write(ConnectionWriter &writer) const override
Output a representation of the bottle to a network connection.
Definition Bottle.cpp:236
void addString(const char *str)
Places a string in the bottle, at the end of the list.
Definition Bottle.cpp:176
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
void interrupt() override
Interrupt any current reads or writes attached to the port.
T * read(bool shouldWait=true) override
Read an available object from the port.
An interface for reading from a network connection.
virtual ConnectionWriter * getWriter()=0
Gets a way to reply to the message, if possible.
An interface for writing to a network connection.
bool start()
Call this to start the thread.
void step()
Call this to "step" the thread rather than starting it.
void stop()
Call this to stop the thread, this call blocks until the thread is terminated (and releaseThread() ca...
A class for storing options and configuration information.
Definition Property.h:33
Helper class for finding config files and other external resources.
bool setDefaultContext(const std::string &contextName)
Sets the context for the current ResourceFinder object.
bool configure(int argc, char *argv[], bool skipFirstArgument=true)
Sets up the ResourceFinder.
std::string findFile(const std::string &name)
Find the full path to a file.
bool setDefaultConfigFile(const std::string &fname)
Provide a default value for the configuration file (can be overridden from command line with the –fro...
A base class for nested structures that can be searched.
Definition Searchable.h:31
A single value (typically within a Bottle).
Definition Value.h:44
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:228
virtual std::string asString() const
Get string value.
Definition Value.cpp:246
void push_back(const T &elem)
Push a new element in the vector: size is changed.
Definition Vector.h:278
#define yCInfo(component,...)
#define yCError(component,...)
#define yCWarning(component,...)
#define yCDebug(component,...)
#define YARP_LOG_COMPONENT(name,...)
yarp::math::Vec2D< size_t > XYCell
Definition NavTypes.h:16
yarp::math::Vec2D< double > XYWorld
Definition NavTypes.h:17
double now()
Return the current time in seconds, relative to an arbitrary starting point.
Definition Time.cpp:121
The main, catch-all namespace for YARP.
Definition dirs.h:16