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