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 
38 namespace 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! */
171 void
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  */
181 void
182 LaserOccupancyGrid::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  */
284 void
285 LaserOccupancyGrid::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 
342 float
343 LaserOccupancyGrid::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  */
382 float
383 LaserOccupancyGrid::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  */
423 std::vector<LaserOccupancyGrid::LaserPoint> *
424 LaserOccupancyGrid::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  */
450 point_t
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  */
460 void
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 
476 void
477 LaserOccupancyGrid::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 
546 void
547 LaserOccupancyGrid::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 
590 void
591 LaserOccupancyGrid::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