25 #include "obstacle_map.h" 27 #include "../utils/rob/roboshape_colli.h" 29 #include <utils/time/clock.h> 30 #include <utils/math/angle.h> 31 #include <utils/math/coord.h> 33 #include <logging/logger.h> 34 #include <config/config.h> 36 #include <interfaces/Laser360Interface.h> 62 int width,
int height,
int cell_width,
int cell_height)
64 tf_listener_(listener ),
68 logger->
log_debug(
"LaserOccupancyGrid",
"(Constructor): Entering");
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());
79 cfg_emergency_stop_beams_used_ = config->
get_float((cfg_prefix +
"emergency_stopping/beams_used").c_str());
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;
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;
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_));
96 angle_range_ =
deg2rad((360 - cfg_delete_invisible_old_obstacles_angle_min_) + cfg_delete_invisible_old_obstacles_angle_max_);
98 angle_min_ =
deg2rad( cfg_delete_invisible_old_obstacles_angle_min_ );
101 reference_frame_ = config->
get_string((cfg_prefix +
"frame/odometry").c_str());
102 laser_frame_ = config->
get_string((cfg_prefix +
"frame/laser").c_str());
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());
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);
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());
116 if_buffer_filled_.resize(if_buffer_size_);
117 std::fill(if_buffer_filled_.begin(), if_buffer_filled_.end(),
false);
121 robo_shape_ =
new RoboShapeColli( (cfg_prefix +
"roboshape/").c_str(), logger, config );
122 old_readings_.clear();
125 logger->
log_debug(
"LaserOccupancyGrid",
"Generating obstacle map");
126 bool obstacle_shape = robo_shape_->
is_angular_robot() && ! cfg_force_elipse_obstacle_;
128 logger->
log_debug(
"LaserOccupancyGrid",
"Generating obstacle map done");
137 logger->
log_debug(
"LaserOccupancyGrid",
"Laser (x,y) offset from robo-center is (%f, %f)",
138 offset_laser_.
x, offset_laser_.
y);
141 logger->
log_debug(
"LaserOccupancyGrid",
"(Constructor): Exiting");
154 old_readings_.clear();
155 old_readings_.reserve( initial_history_size_ );
162 LaserOccupancyGrid::update_laser()
165 int if_buffer_free_pos = -1;
167 for (
int i = 0; i < if_buffer_size_; ++i) {
168 if (if_buffer_filled_[i] ==
false) {
169 if_buffer_free_pos = i;
174 if ( if_buffer_free_pos < 0 ) {
179 int if_buffer_oldest_pos = -1;
181 for (
int i = 0; i < if_buffer_size_; ++i) {
183 if_buffer_oldest_pos = i;
187 if_buffer_free_pos = if_buffer_oldest_pos;
191 if_buffer_filled_[ if_buffer_free_pos ] =
true;
193 new_readings_.clear();
196 for (
int i = 0; i < if_buffer_size_; ++i) {
197 if (if_buffer_filled_[i] ==
true) {
200 if_buffer_filled_[i] =
false;
203 std::string laser_frame = if_laser_->
frame();
208 tf_listener_->
lookup_transform(reference_frame_, laser_frame, laser_time, transform);
210 tf::Vector3 pos_robot_tf = transform.getOrigin();
213 double angle_inc = M_PI * 2. / 360.;
217 if (if_laser_->
distances(i) >= min_laser_length_) {
221 point_polar.
phi = angle_inc * i;
228 p.setValue(point_cart.
x, point_cart.
y, 0.);
231 LaserOccupancyGrid::LaserPoint point;
233 point.timestamp =
Time(laser_time);
235 new_readings_.push_back(point);
237 if ( cfg_delete_invisible_old_obstacles_ ) {
239 if ( angle_dist >= 0 && angle_dist <= angle_range_ ) {
240 validate_old_laser_points(pos_robot, point.coord);
246 if_buffer_filled_[i] =
true;
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());
264 std::vector< LaserPoint > old_readings_tmp;
267 cart_coord_2d_t v_new(pos_new_laser_point.
x - pos_robot.
x , pos_new_laser_point.
y - pos_robot.
y);
271 float d_new = sqrt(v_new.
x*v_new.
x + v_new.
y*v_new.
y);
278 static const float deg_unit = M_PI / 180.f;
280 for ( std::vector< LaserPoint >::iterator it = old_readings_.begin();
281 it != old_readings_.end(); ++it ) {
283 v_old.
x = (*it).coord.x - pos_robot.
x;
284 v_old.
y = (*it).coord.y - pos_robot.
y;
287 d_old = sqrt(v_old.
x*v_old.
x + v_old.
y*v_old.
y);
290 if( d_new <= d_old + obstacle_distance_ ) {
293 old_readings_tmp.push_back( *it );
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 ) {
301 old_readings_tmp.push_back( *it );
312 old_readings_.clear();
313 old_readings_.reserve( old_readings_tmp.size() );
315 for (
unsigned int i = 0; i < old_readings_tmp.size(); ++i) {
316 old_readings_.push_back( old_readings_tmp[i] );
321 LaserOccupancyGrid::obstacle_in_path_distance(
float vx,
float vy )
326 float distance_min = 1000;
328 int cfg_beams = cfg_emergency_stop_beams_used_;
330 int beams_start = angle - int( cfg_beams / 2 );
331 if ( beams_start < 0 ) { beams_start += 360; }
333 int beams_end = beams_start + cfg_beams;
334 if ( beams_end >= 360 ) { beams_end -= 360; }
336 for (
int i = beams_start; i != beams_end; i = (i+1) % 360 ) {
338 if ( dist != 0 && std::isfinite(dist) ) {
339 distance_min = std::min( distance_min, dist );
359 float vel = std::sqrt(vx*vx + vy*vy);
361 float next_obstacle = obstacle_in_path_distance( vx, vy );
366 for (
int y = 0; y <
width_; ++y )
367 for (
int x = 0; x <
height_; ++x )
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());
383 integrate_old_readings( midX, midY, inc, vel, transform );
384 integrate_new_readings( midX, midY, inc, vel, transform );
386 return next_obstacle;
395 std::vector< LaserOccupancyGrid::LaserPoint >*
396 LaserOccupancyGrid::transform_laser_points(std::vector< LaserOccupancyGrid::LaserPoint >& laserPoints,
tf::StampedTransform& transform)
398 int count_points = laserPoints.size();
399 std::vector< LaserOccupancyGrid::LaserPoint >* laserPointsTransformed =
new std::vector< LaserOccupancyGrid::LaserPoint >();
400 laserPointsTransformed->reserve( count_points );
404 for (
int i = 0; i < count_points; ++i) {
405 p.setValue(laserPoints[i].coord.x, laserPoints[i].coord.y, 0.);
408 LaserOccupancyGrid::LaserPoint point;
410 point.timestamp = laserPoints[i].timestamp;
411 laserPointsTransformed->push_back( point );
414 return laserPointsTransformed;
433 offset_base_.
x = (int)round( (offset_laser_.
x + x)*100.f/
cell_height_ );
434 offset_base_.
y = (int)round( (offset_laser_.
y + y)*100.f/
cell_width_ );
448 LaserOccupancyGrid::integrate_old_readings(
int midX,
int midY,
float inc,
float vel,
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);
455 float newpos_x, newpos_y;
458 Time history =
Time(clock) -
Time(
double(std::max( min_history_length_, max_history_length_)));
461 for (
unsigned int i = 0; i < pointsTransformed->size(); ++i ) {
463 if ( (*pointsTransformed)[i].timestamp.in_sec() >= history.
in_sec() ) {
465 newpos_x = (*pointsTransformed)[i].coord.x;
466 newpos_y = (*pointsTransformed)[i].coord.y;
481 bool SollEintragen =
true;
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 )
494 old_readings.push_back( old_readings_[i] );
499 width = std::max( 4.f, ((width + inc)*100.f)/
cell_width_ );
501 height = std::max( 4.f, ((height + inc)*100.f)/
cell_height_ );
502 integrate_obstacle( posX, posY, width, height );
509 old_readings_.clear();
510 old_readings_.reserve( old_readings.size() );
513 for (
unsigned int i = 0; i < old_readings.size(); i++ )
514 old_readings_.push_back( old_readings[i] );
516 delete pointsTransformed;
521 LaserOccupancyGrid::integrate_new_readings(
int midX,
int midY,
float inc,
float vel,
524 std::vector< LaserOccupancyGrid::LaserPoint >* pointsTransformed = transform_laser_points(new_readings_, transform);
526 int numberOfReadings = pointsTransformed->size();
531 float oldp_x = 1000.f;
532 float oldp_y = 1000.f;
534 for (
int i = 0; i < numberOfReadings; i++ ) {
535 point = (*pointsTransformed)[i].coord;
537 if( sqrt(
sqr(point.
x) +
sqr(point.
y)) >= min_laser_length_
538 &&
distance(point.
x, point.
y, oldp_x, oldp_y) >= obstacle_distance_)
542 posX = midX + (int)((point.
x*100.f) / ((float)
cell_height_ ));
543 posY = midY + (int)((point.
y*100.f) / ((float)
cell_width_ ));
545 if ( !( posX <= 5 || posX >=
height_-6 || posY <= 5 || posY >=
width_-6 ) ) {
548 width = std::max( 4.f, ((width + inc)*100.f)/
cell_width_ );
552 height = std::max( 4.f, ((height + inc)*100.f)/
cell_height_ );
554 integrate_obstacle( posX, posY, width, height );
556 old_readings_.push_back( new_readings_[i] );
560 delete pointsTransformed;
564 LaserOccupancyGrid::integrate_obstacle(
int x,
int y,
int width,
int height )
566 std::vector< int > fast_obstacle = obstacle_map->
get_obstacle( width, height, cfg_obstacle_inc_ );
572 for(
unsigned int i = 0; i < fast_obstacle.size(); i+=3 ) {
578 posX = x + fast_obstacle[i] + offset_base_.
x;
579 posY = y + fast_obstacle[i+1] + offset_base_.
y;
581 if( (posX > 0) && (posX <
height_)
582 && (posY > 0) && (posY <
width_)
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.
double in_sec() const
Convet time to seconds.
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
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.
static Clock * instance()
Clock initializer.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
Cartesian coordinates (2D).
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
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.
This is supposed to be the central clock in Fawkes.
point_t get_laser_position()
Get the laser's position in the grid.
A class for handling time.
colli_cell_cost_t get_cell_costs() const
Get cell costs.
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.
int cell_height_
Cell height in cm.
float angle_distance_signed(float angle_from, float angle_to)
Determines the signed distance between from "angle_from" to "angle_to" provided as radians...
unsigned int far
The cost for a cell near an obstacle (distance="near")
double sqr(double x)
Fast square multiplication.
Costs of occupancy-grid cells.
Time buffer_timestamp(unsigned int buffer)
Get time of a buffer.
void reset_old()
Reset all old readings and forget about the world state!
int width_
Width of the grid in # cells.
Time now() const
Get the current time.
int cell_width_
Cell width in cm.
void read_from_buffer(unsigned int buffer)
Copy data from buffer to private memory.
Base class for exceptions in Fawkes.
float get_complete_width_x()
Returns the complete x width of the angular robot.
int height_
Height of the grid in # cells.
void read()
Read from BlackBoard into local copy.
Occupancy Grid class for general use.
~LaserOccupancyGrid()
Descturctor.
unsigned int free
The cost for a free cell.
unsigned int occ
The cost for an occupied cell.
void copy_shared_to_buffer(unsigned int buffer)
Copy data from private memory to buffer.
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.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
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.
Point with cartesian coordinates as signed integers.
This is an implementation of a collection of fast obstacles.
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.
bool is_angular_robot()
Returns if the robot is angular.
unsigned int mid
The cost for a cell near an obstacle (distance="near")
float * distances() const
Get distances value.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
char * frame() const
Get frame value.
std::vector< std::vector< Probability > > occupancy_probs_
The occupancy probability of the cells in a 2D array.
Interface for configuration handling.
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")
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.