Fawkes API Fawkes Development Version
og_laser.cpp
1
2/***************************************************************************
3 * og_laser.cpp - Occupancy grid for colli's A* search
4 *
5 * Created: Fri Oct 18 15:16:23 2013
6 * Copyright 2002 Stefan Jacobs
7 * 2013-2014 Bahram Maleki-Fard
8 * 2014 Tobias Neumann
9 ****************************************************************************/
10
11/* This program is free software; you can redistribute it and/or modify
12 * it under the terms of the GNU General Public License as published by
13 * the Free Software Foundation; either version 2 of the License, or
14 * (at your option) any later version.
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL file in the doc directory.
22 */
23
24#include "og_laser.h"
25
26#include "../utils/rob/roboshape_colli.h"
27#include "obstacle_map.h"
28
29#include <config/config.h>
30#include <interfaces/Laser360Interface.h>
31#include <logging/logger.h>
32#include <utils/math/angle.h>
33#include <utils/math/coord.h>
34#include <utils/time/clock.h>
35
36#include <cmath>
37
38namespace fawkes {
39
40/** @class LaserOccupancyGrid <plugins/colli/search/og_laser.h>
41 * This OccGrid is derived by the Occupancy Grid originally from Andreas Strack,
42 * but modified for speed purposes.
43 */
44
45/** Constructor.
46 * @param laser The Laser object
47 * @param logger The fawkes logger
48 * @param config The fawkes configuration
49 * @param listener The tf::Transformer
50 * @param width The width of the grid (in m)
51 * @param height The height of the grid (in m)
52 * @param cell_width The width of a cell (in cm)
53 * @param cell_height The height of a cell (in cm)
54 */
56 Logger * logger,
57 Configuration * config,
58 tf::Transformer * listener,
59 int width,
60 int height,
61 int cell_width,
62 int cell_height)
63: OccupancyGrid(width, height, cell_width, cell_height),
64 tf_listener_(listener),
65 logger_(logger),
66 if_laser_(laser)
67{
68 logger->log_debug("LaserOccupancyGrid", "(Constructor): Entering");
69
70 //read config
71 std::string cfg_prefix = "/plugins/colli/";
72 obstacle_distance_ =
73 config->get_float((cfg_prefix + "laser_occupancy_grid/distance_account").c_str());
74 initial_history_size_ =
75 3 * config->get_int((cfg_prefix + "laser_occupancy_grid/history/initial_size").c_str());
76 max_history_length_ =
77 config->get_float((cfg_prefix + "laser_occupancy_grid/history/max_length").c_str());
78 min_history_length_ =
79 config->get_float((cfg_prefix + "laser_occupancy_grid/history/min_length").c_str());
80 min_laser_length_ = config->get_float((cfg_prefix + "laser/min_reading_length").c_str());
81 cfg_write_spam_debug_ = config->get_bool((cfg_prefix + "write_spam_debug").c_str());
82
83 cfg_emergency_stop_beams_used_ =
84 config->get_float((cfg_prefix + "emergency_stopping/beams_used").c_str());
85
86 cfg_delete_invisible_old_obstacles_ = config->get_bool(
87 (cfg_prefix + "laser_occupancy_grid/history/delete_invisible_old_obstacles/enable").c_str());
88 cfg_delete_invisible_old_obstacles_angle_min_ = config->get_int(
89 (cfg_prefix + "laser_occupancy_grid/history/delete_invisible_old_obstacles/angle_min").c_str());
90 cfg_delete_invisible_old_obstacles_angle_max_ = config->get_int(
91 (cfg_prefix + "laser_occupancy_grid/history/delete_invisible_old_obstacles/angle_max").c_str());
92 if (cfg_delete_invisible_old_obstacles_angle_min_ >= 360) {
93 logger_->log_warn("LaserOccupancyGrid", "Min angle out of bounce, use 0");
94 cfg_delete_invisible_old_obstacles_angle_min_ = 0;
95 }
96 if (cfg_delete_invisible_old_obstacles_angle_min_ >= 360) {
97 logger_->log_warn("LaserOccupancyGrid", "Max angle out of bounce, use 360");
98 cfg_delete_invisible_old_obstacles_angle_min_ = 360;
99 }
100
101 if (cfg_delete_invisible_old_obstacles_angle_max_
102 > cfg_delete_invisible_old_obstacles_angle_min_) {
103 angle_range_ = deg2rad((unsigned int)abs(cfg_delete_invisible_old_obstacles_angle_max_
104 - cfg_delete_invisible_old_obstacles_angle_min_));
105 } else {
106 angle_range_ = deg2rad((360 - cfg_delete_invisible_old_obstacles_angle_min_)
107 + cfg_delete_invisible_old_obstacles_angle_max_);
108 }
109 angle_min_ = deg2rad(cfg_delete_invisible_old_obstacles_angle_min_);
110
111 reference_frame_ = config->get_string((cfg_prefix + "frame/odometry").c_str());
112 laser_frame_ = config->get_string(
113 (cfg_prefix + "frame/laser")
114 .c_str()); //TODO change to base_link => search in base_link instead base_laser
115
116 cfg_obstacle_inc_ = config->get_bool((cfg_prefix + "obstacle_increasement").c_str());
117 cfg_force_elipse_obstacle_ =
118 config->get_bool((cfg_prefix + "laser_occupancy_grid/force_ellipse_obstacle").c_str());
119
120 if_buffer_size_ = config->get_int((cfg_prefix + "laser_occupancy_grid/buffer_size").c_str());
121 if_buffer_size_ = std::max(
122 if_buffer_size_,
123 1); //needs to be >= 1, because the data is always wrote into the buffer (instead of read())
124
125 cell_costs_.occ =
126 config->get_int((cfg_prefix + "laser_occupancy_grid/cell_cost/occupied").c_str());
127 cell_costs_.near = config->get_int((cfg_prefix + "laser_occupancy_grid/cell_cost/near").c_str());
128 cell_costs_.mid = config->get_int((cfg_prefix + "laser_occupancy_grid/cell_cost/mid").c_str());
129 cell_costs_.far = config->get_int((cfg_prefix + "laser_occupancy_grid/cell_cost/far").c_str());
130 cell_costs_.free = config->get_int((cfg_prefix + "laser_occupancy_grid/cell_cost/free").c_str());
131
132 if_buffer_filled_.resize(if_buffer_size_);
133 std::fill(if_buffer_filled_.begin(), if_buffer_filled_.end(), false);
134
135 if_laser_->resize_buffers(if_buffer_size_);
136
137 robo_shape_.reset(new RoboShapeColli((cfg_prefix + "roboshape/").c_str(), logger, config));
138 old_readings_.clear();
139 init_grid();
140
141 logger->log_debug("LaserOccupancyGrid", "Generating obstacle map");
142 bool obstacle_shape = robo_shape_->is_angular_robot() && !cfg_force_elipse_obstacle_;
143 obstacle_map_.reset(new ColliObstacleMap(cell_costs_, obstacle_shape));
144 logger->log_debug("LaserOccupancyGrid", "Generating obstacle map done");
145
146 laser_pos_ = point_t(0, 0);
147
148 // calculate laser offset from robot center
149 offset_base_.x = 0;
150 offset_base_.y = 0;
151 offset_laser_.x =
152 robo_shape_->get_complete_width_x() / 2.f - robo_shape_->get_robot_length_for_deg(0);
153 offset_laser_.y =
154 robo_shape_->get_complete_width_y() / 2.f - robo_shape_->get_robot_length_for_deg(90);
155 logger->log_debug("LaserOccupancyGrid",
156 "Laser (x,y) offset from robo-center is (%f, %f)",
157 offset_laser_.x,
158 offset_laser_.y);
159
160 logger->log_debug("LaserOccupancyGrid", "(Constructor): Exiting");
161}
162
163/** Descturctor. */
165{
166 robo_shape_.reset();
167 obstacle_map_.reset();
168}
169
170/** Reset all old readings and forget about the world state! */
171void
173{
174 old_readings_.clear();
175 old_readings_.reserve(initial_history_size_);
176}
177
178/**
179 * Gets data from laser (does! read it) and transforms them into the reference-frame (odom)
180 */
181void
182LaserOccupancyGrid::update_laser()
183{
184 //check for free pos in buffer
185 int if_buffer_free_pos = -1;
186
187 for (int i = 0; i < if_buffer_size_; ++i) { //for all buffer possition
188 if (if_buffer_filled_[i] == false) { //if free (used == false)
189 if_buffer_free_pos = i; //use this buffer
190 //stop loop
191 }
192 }
193 //write BB date into buffer (instead of read())
194 if (if_buffer_free_pos < 0) { //if there is no free buffer
195 //logger_->log_error("LaserOccupancyGrid", "if_laser buffer is full empty oldest");
196
197 //search for the oldest buffer and uses this
198 double if_buffer_oldest_time = Clock::instance()->now().in_sec() + 1000;
199 int if_buffer_oldest_pos = -1;
200
201 for (int i = 0; i < if_buffer_size_; ++i) {
202 if (if_laser_->buffer_timestamp(i).in_sec() < if_buffer_oldest_time) {
203 if_buffer_oldest_pos = i;
204 if_buffer_oldest_time = if_laser_->buffer_timestamp(i).in_sec();
205 }
206 }
207 if_buffer_free_pos = if_buffer_oldest_pos;
208 }
209
210 if_laser_->copy_shared_to_buffer(if_buffer_free_pos); //read new laser data
211 if_buffer_filled_[if_buffer_free_pos] = true; //set buffer used
212
213 new_readings_.clear();
214 new_readings_.reserve(if_laser_->maxlenof_distances() * if_buffer_size_);
215 //for all buffer: try to transform and save in grid
216 for (int i = 0; i < if_buffer_size_; ++i) {
217 if (if_buffer_filled_[i] == true) { //if is filled
218
219 if_laser_->read_from_buffer(i); //read buffer
220 if_buffer_filled_[i] = false; //show buffer is not used
221 //TODO just if there are new data
222 const Time *laser_time = if_laser_->timestamp();
223 std::string laser_frame = if_laser_->frame();
224
225 tf::StampedTransform transform;
226
227 try {
228 tf_listener_->lookup_transform(reference_frame_, laser_frame, laser_time, transform);
229
230 tf::Vector3 pos_robot_tf = transform.getOrigin();
231 cart_coord_2d_t pos_robot(pos_robot_tf.getX(), pos_robot_tf.getY());
232
233 double angle_inc = M_PI * 2. / 360.;
234 tf::Point p;
235 //Save all Points in refernce Frame
236 for (unsigned int i = 0; i < if_laser_->maxlenof_distances(); ++i) {
237 if (if_laser_->distances(i) >= min_laser_length_) {
238 //Save as polar coordinaten
239 polar_coord_2d_t point_polar;
240 point_polar.r = if_laser_->distances(i);
241 point_polar.phi = angle_inc * i;
242
243 //Calculate as cartesien
244 cart_coord_2d_t point_cart;
245 polar2cart2d(point_polar.phi, point_polar.r, &point_cart.x, &point_cart.y);
246
247 //transform into odom
248 p.setValue(point_cart.x, point_cart.y, 0.);
249 p = transform * p;
250
251 LaserOccupancyGrid::LaserPoint point;
252 point.coord = cart_coord_2d_t(p.getX(), p.getY());
253 point.timestamp = Time(laser_time);
254
255 new_readings_.push_back(point);
256
257 if (cfg_delete_invisible_old_obstacles_) {
258 float angle_dist = angle_distance_signed(angle_min_, point_polar.phi);
259 if (angle_dist >= 0 && angle_dist <= angle_range_) {
260 validate_old_laser_points(pos_robot, point.coord);
261 }
262 }
263 }
264 }
265 } catch (Exception &e) {
266 if_buffer_filled_[i] = true; //show buffer still needs to be there
267 if (cfg_write_spam_debug_) {
268 logger_->log_debug(
269 "LaserOccupancyGrid",
270 "Unable to transform %s to %s. Laser-data not used, will keeped in history.",
271 laser_frame.c_str(),
272 reference_frame_.c_str());
273 }
274 }
275 }
276 }
277}
278
279/**
280 * compare the given point with all old points to delete old-wrong-obstacles
281 * @param pos_robot the robot pose where the point to compare with where taken
282 * @param pos_new_laser_point the position of the point to compare with
283 */
284void
285LaserOccupancyGrid::validate_old_laser_points(cart_coord_2d_t pos_robot,
286 cart_coord_2d_t pos_new_laser_point)
287{
288 std::vector<LaserPoint> old_readings_tmp;
289
290 // vectors from robot to new and old laser-points
291 cart_coord_2d_t v_new(pos_new_laser_point.x - pos_robot.x, pos_new_laser_point.y - pos_robot.y);
292 cart_coord_2d_t v_old;
293
294 // distances from robot to new and old laser-points (i.e. length of v_new and v_old)
295 float d_new = sqrt(v_new.x * v_new.x + v_new.y * v_new.y);
296
297 // angle between the two vectors v_new and v_old. Use to determine whether they
298 // belong to the same laser-beam
299 float angle = 0.f;
300
301 static const float deg_unit = M_PI / 180.f; // 1 degree
302
303 for (std::vector<LaserPoint>::iterator it = old_readings_.begin(); it != old_readings_.end();
304 ++it) {
305 v_old.x = (*it).coord.x - pos_robot.x;
306 v_old.y = (*it).coord.y - pos_robot.y;
307
308 // need to calculate distance here, needed for angle calculation
309 float d_old = sqrt(v_old.x * v_old.x + v_old.y * v_old.y);
310
311 // we already have the distances, so already make the distance-check here
312 if (d_new <= d_old + obstacle_distance_) {
313 // in case both points belonged to the same laser-beam, p_old
314 // would be in shadow of p_new => keep p_old anyway
315 old_readings_tmp.push_back(*it);
316 continue;
317 }
318
319 // angle a between to vectors v,w: cos(a) = dot(v,w) / (|v|*|w|)
320 angle = acos((v_old.x * v_new.x + v_old.y * v_new.y) / (d_new * d_old));
321 if (std::isnan(angle) || angle > deg_unit) {
322 // p_old is not the range of this laser-beam. Keep it.
323 old_readings_tmp.push_back(*it);
324
325 /* No "else" here. It would mean that p_old is in the range of the
326 * same laser beam. And we already know that
327 * "d_new > d_old + obstacle_distance_" => this laser beam can see
328 * through p_old => discard p_old. In other words, do not add to
329 * old_readings_tmp.
330 */
331 }
332 }
333
334 old_readings_.clear();
335 old_readings_.reserve(old_readings_tmp.size());
336
337 for (unsigned int i = 0; i < old_readings_tmp.size(); ++i) {
338 old_readings_.push_back(old_readings_tmp[i]);
339 }
340}
341
342float
343LaserOccupancyGrid::obstacle_in_path_distance(float vx, float vy)
344{
345 if_laser_->read();
346 int angle = roundf(rad2deg(normalize_rad(atan2f(vy, vx))));
347
348 float distance_min = 1000;
349
350 int cfg_beams = cfg_emergency_stop_beams_used_;
351
352 int beams_start = angle - int(cfg_beams / 2);
353 if (beams_start < 0) {
354 beams_start += 360;
355 }
356
357 int beams_end = beams_start + cfg_beams;
358 if (beams_end >= 360) {
359 beams_end -= 360;
360 }
361
362 for (int i = beams_start; i != beams_end; i = (i + 1) % 360) {
363 float dist = if_laser_->distances(i);
364 if (dist != 0 && std::isfinite(dist)) {
365 distance_min = std::min(distance_min, dist);
366 }
367 }
368
369 return distance_min;
370}
371
372/** Put the laser readings in the occupancy grid
373 * Also, every reading gets a radius according to the relative direction
374 * of this reading to the robot.
375 * @param midX is the current x position of the robot in the grid.
376 * @param midY is the current y position of the robot in the grid.
377 * @param inc is the current constant to increase the obstacles.
378 * @param vx Translation x velocity of the motor
379 * @param vy Translation y velocity of the motor
380 * @return distance to next obstacle in pathdirection
381 */
382float
383LaserOccupancyGrid::update_occ_grid(int midX, int midY, float inc, float vx, float vy)
384{
385 float vel = std::sqrt(vx * vx + vy * vy);
386
387 float next_obstacle = obstacle_in_path_distance(vx, vy);
388
389 laser_pos_.x = midX;
390 laser_pos_.y = midY;
391
392 for (int y = 0; y < width_; ++y)
393 for (int x = 0; x < height_; ++x)
394 occupancy_probs_[x][y] = cell_costs_.free;
395
396 update_laser();
397
398 tf::StampedTransform transform;
399
400 try {
401 tf_listener_->lookup_transform(laser_frame_, reference_frame_, Time(0, 0), transform);
402
403 } catch (Exception &e) {
404 logger_->log_error("LaserOccupancyGrid",
405 "Unable to transform %s to %s. Can't put obstacles into the grid",
406 reference_frame_.c_str(),
407 laser_frame_.c_str());
408 return 0.;
409 }
410
411 integrate_old_readings(midX, midY, inc, vel, transform);
412 integrate_new_readings(midX, midY, inc, vel, transform);
413
414 return next_obstacle;
415}
416
417/**
418 * Transforms all given points with the given transform
419 * @param laserPoints vector of LaserPoint, that contains the points to transform
420 * @param transform stamped transform, the transform to transform with
421 * @return the transformed laserPoints
422 */
423std::vector<LaserOccupancyGrid::LaserPoint> *
424LaserOccupancyGrid::transform_laser_points(std::vector<LaserOccupancyGrid::LaserPoint> &laserPoints,
425 tf::StampedTransform & transform)
426{
427 int count_points = laserPoints.size();
428 std::vector<LaserOccupancyGrid::LaserPoint> *laserPointsTransformed =
429 new std::vector<LaserOccupancyGrid::LaserPoint>();
430 laserPointsTransformed->reserve(count_points);
431
432 tf::Point p;
433
434 for (int i = 0; i < count_points; ++i) {
435 p.setValue(laserPoints[i].coord.x, laserPoints[i].coord.y, 0.);
436 p = transform * p;
437
438 LaserOccupancyGrid::LaserPoint point;
439 point.coord = cart_coord_2d_struct(p.getX(), p.getY());
440 point.timestamp = laserPoints[i].timestamp;
441 laserPointsTransformed->push_back(point);
442 }
443
444 return laserPointsTransformed;
445}
446
447/** Get the laser's position in the grid
448 * @return point_t structure containing the laser's position in the grid
449 */
452{
453 return laser_pos_;
454}
455
456/** Set the offset of base_link from laser.
457 * @param x offset in x-direction (in meters)
458 * @param y offset in y-direction (in meters)
459 */
460void
462{
463 offset_base_.x = (int)round((offset_laser_.x + x) * 100.f / cell_height_); // # in grid-cells
464 offset_base_.y = (int)round((offset_laser_.y + y) * 100.f / cell_width_);
465}
466
467/** Get cell costs.
468 * @return struct that contains all the cost values for the occgrid cells
469 */
472{
473 return cell_costs_;
474}
475
476void
477LaserOccupancyGrid::integrate_old_readings(int midX,
478 int midY,
479 float inc,
480 float vel,
481 tf::StampedTransform &transform)
482{
483 std::vector<LaserOccupancyGrid::LaserPoint> old_readings;
484 old_readings.reserve(old_readings_.size());
485 std::vector<LaserOccupancyGrid::LaserPoint> *pointsTransformed =
486 transform_laser_points(old_readings_, transform);
487
488 float newpos_x, newpos_y;
489
490 Clock *clock = Clock::instance();
491 Time history = Time(clock) - Time(double(std::max(min_history_length_, max_history_length_)));
492
493 // update all old readings
494 for (unsigned int i = 0; i < pointsTransformed->size(); ++i) {
495 if ((*pointsTransformed)[i].timestamp.in_sec() >= history.in_sec()) {
496 newpos_x = (*pointsTransformed)[i].coord.x;
497 newpos_y = (*pointsTransformed)[i].coord.y;
498
499 //newpos_x = old_readings_[i].coord.x + xref;
500 //newpos_y = old_readings_[i].coord.y + yref;
501
502 //float angle_to_old_reading = atan2( newpos_y, newpos_x );
503 //float sqr_distance_to_old_reading = sqr( newpos_x ) + sqr( newpos_y );
504
505 //int number_of_old_reading = (int)rad2deg(
506 // normalize_rad(360.0/m_pLaser->GetNumberOfReadings() * angle_to_old_reading) );
507 // This was RCSoftX, now ported to fawkes:
508 //int number_of_old_reading = (int) (normalize_degree( ( 360.0/(m_pLaser->GetNumberOfReadings()) ) *
509 // rad2deg(angle_to_old_reading) ) );
510
511 //bool SollEintragen = true;
512
513 // do not insert if current reading at that angle deviates more than 30cm from old reading
514 // TODO. make those 30cm configurable
515 //if ( sqr( m_pLaser->GetReadingLength( number_of_old_reading ) - 0.3 ) > sqr_distance_to_old_reading )
516 // SollEintragen = false;
517
518 //if ( SollEintragen == true ) {
519 int posX = midX + (int)((newpos_x * 100.f) / ((float)cell_height_));
520 int posY = midY + (int)((newpos_y * 100.f) / ((float)cell_width_));
521 if (posX > 4 && posX < height_ - 5 && posY > 4 && posY < width_ - 5) {
522 old_readings.push_back(old_readings_[i]);
523
524 // 25 cm's in my opinion, that are here: 0.25*100/cell_width_
525 //int size = (int)(((0.25f+inc)*100.f)/(float)cell_width_);
526 float width = robo_shape_->get_complete_width_y();
527 width = std::max(4.f, ((width + inc) * 100.f) / cell_width_);
528 float height = robo_shape_->get_complete_width_x();
529 height = std::max(4.f, ((height + inc) * 100.f) / cell_height_);
530 integrate_obstacle(posX, posY, width, height);
531 }
532 //}
533 }
534 }
535
536 old_readings_.clear();
537 old_readings_.reserve(old_readings.size());
538
539 // integrate the new calculated old readings
540 for (unsigned int i = 0; i < old_readings.size(); i++)
541 old_readings_.push_back(old_readings[i]);
542
543 delete pointsTransformed;
544}
545
546void
547LaserOccupancyGrid::integrate_new_readings(int midX,
548 int midY,
549 float inc,
550 float vel,
551 tf::StampedTransform &transform)
552{
553 std::vector<LaserOccupancyGrid::LaserPoint> *pointsTransformed =
554 transform_laser_points(new_readings_, transform);
555
556 int numberOfReadings = pointsTransformed->size();
557 //TODO resize, reserve??
558
559 int posX, posY;
560 cart_coord_2d_t point;
561 float oldp_x = 1000.f;
562 float oldp_y = 1000.f;
563
564 for (int i = 0; i < numberOfReadings; i++) {
565 point = (*pointsTransformed)[i].coord;
566
567 if (sqrt(sqr(point.x) + sqr(point.y)) >= min_laser_length_
568 && distance(point.x, point.y, oldp_x, oldp_y) >= obstacle_distance_) {
569 oldp_x = point.x;
570 oldp_y = point.y;
571 posX = midX + (int)((point.x * 100.f) / ((float)cell_height_));
572 posY = midY + (int)((point.y * 100.f) / ((float)cell_width_));
573
574 if (!(posX <= 5 || posX >= height_ - 6 || posY <= 5 || posY >= width_ - 6)) {
575 float width = robo_shape_->get_complete_width_y();
576 width = std::max(4.f, ((width + inc) * 100.f) / cell_width_);
577
578 float height = robo_shape_->get_complete_width_x();
579 height = std::max(4.f, ((height + inc) * 100.f) / cell_height_);
580
581 integrate_obstacle(posX, posY, width, height);
582
583 old_readings_.push_back(new_readings_[i]);
584 }
585 }
586 }
587 delete pointsTransformed;
588}
589
590void
591LaserOccupancyGrid::integrate_obstacle(int x, int y, int width, int height)
592{
593 std::vector<int> fast_obstacle = obstacle_map_->get_obstacle(width, height, cfg_obstacle_inc_);
594
595 // i = x offset, i+1 = y offset, i+2 is cost
596 for (unsigned int i = 0; i < fast_obstacle.size(); i += 3) {
597 /* On the laser-points, we draw obstacles based on base_link. The obstacle has the robot-shape,
598 * which means that we need to rotate the shape 180° around base_link and move that rotation-
599 * point onto the laser-point on the grid. That's the same as adding the center_to_base_offset
600 * to the calculated position of the obstacle-center ("x + fast_obstacle[i]" and "y" respectively).
601 */
602 int posX = x + fast_obstacle[i] + offset_base_.x;
603 int posY = y + fast_obstacle[i + 1] + offset_base_.y;
604
605 if ((posX > 0) && (posX < height_) && (posY > 0) && (posY < width_)
606 && (occupancy_probs_[posX][posY] < fast_obstacle[i + 2])) {
607 occupancy_probs_[posX][posY] = fast_obstacle[i + 2];
608 }
609 }
610}
611
612} // namespace fawkes
This is supposed to be the central clock in Fawkes.
Definition: clock.h:35
Time now() const
Get the current time.
Definition: clock.cpp:242
static Clock * instance()
Clock initializer.
Definition: clock.cpp:63
This is an implementation of a collection of fast obstacles.
Definition: obstacle_map.h:39
Interface for configuration handling.
Definition: config.h:68
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual int get_int(const char *path)=0
Get value from configuration which is of type int.
Base class for exceptions in Fawkes.
Definition: exception.h:36
void copy_shared_to_buffer(unsigned int buffer)
Copy data from private memory to buffer.
Definition: interface.cpp:1296
void read_from_buffer(unsigned int buffer)
Copy data from buffer to private memory.
Definition: interface.cpp:1338
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:714
Time buffer_timestamp(unsigned int buffer)
Get time of a buffer.
Definition: interface.cpp:1379
void resize_buffers(unsigned int num_buffers)
Resize buffer array.
Definition: interface.cpp:1261
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
Laser360Interface Fawkes BlackBoard Interface.
float * distances() const
Get distances value.
char * frame() const
Get frame value.
size_t maxlenof_distances() const
Get maximum length of distances value.
float update_occ_grid(int mid_x, int mid_y, float inc, float vx, float vy)
Put the laser readings in the occupancy grid.
Definition: og_laser.cpp:383
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
Definition: og_laser.cpp:461
~LaserOccupancyGrid()
Descturctor.
Definition: og_laser.cpp:164
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:471
void reset_old()
Reset all old readings and forget about the world state!
Definition: og_laser.cpp:172
LaserOccupancyGrid(Laser360Interface *laser, Logger *logger, Configuration *config, tf::Transformer *listener, int width=150, int height=150, int cell_width=5, int cell_height=5)
Constructor.
Definition: og_laser.cpp:55
point_t get_laser_position()
Get the laser's position in the grid.
Definition: og_laser.cpp:451
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_debug(const char *component, const char *format,...)
Log debug message.
Definition: multi.cpp:174
Occupancy Grid class for general use.
Definition: occupancygrid.h:36
int cell_height_
Cell height in cm.
Definition: occupancygrid.h:85
void init_grid()
Init a new empty grid with the predefined parameters *‍/.
int height_
Height of the grid in # cells.
Definition: occupancygrid.h:87
std::vector< std::vector< Probability > > occupancy_probs_
The occupancy probability of the cells in a 2D array.
Definition: occupancygrid.h:81
int width_
Width of the grid in # cells.
Definition: occupancygrid.h:86
int cell_width_
Cell width in cm.
Definition: occupancygrid.h:84
This class is mainly the same as the basic class with the difference that all data is precalculated o...
A class for handling time.
Definition: time.h:93
double in_sec() const
Convet time to seconds.
Definition: time.cpp:219
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
void lookup_transform(const std::string &target_frame, const std::string &source_frame, const fawkes::Time &time, StampedTransform &transform) const
Lookup transform.
Fawkes library namespace.
double sqr(double x)
Fast square multiplication.
Definition: common.h:37
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
Definition: astar.h:39
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
Definition: angle.h:59
float deg2rad(float deg)
Convert an angle given in degrees to radians.
Definition: angle.h:36
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
Definition: angle.h:90
float angle_distance_signed(float angle_from, float angle_to)
Determines the signed distance between from "angle_from" to "angle_to" provided as radians.
Definition: angle.h:134
float rad2deg(float rad)
Convert an angle given in radians to degrees.
Definition: angle.h:46
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
Definition: coord.h:72
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
Cartesian coordinates (2D).
Definition: types.h:65
float y
y coordinate
Definition: types.h:67
float x
x coordinate
Definition: types.h:66
Costs of occupancy-grid cells.
Definition: types.h:50
unsigned int far
The cost for a cell near an obstacle (distance="near")
Definition: types.h:54
unsigned int occ
The cost for an occupied cell.
Definition: types.h:51
unsigned int mid
The cost for a cell near an obstacle (distance="near")
Definition: types.h:53
unsigned int near
The cost for a cell near an obstacle (distance="near")
Definition: types.h:52
unsigned int free
The cost for a free cell.
Definition: types.h:55
int x
x coordinate
Definition: types.h:43
int y
y coordinate
Definition: types.h:44