26 #include "../utils/rob/roboshape_colli.h" 27 #include "obstacle_map.h" 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> 64 tf_listener_(listener),
68 logger->
log_debug(
"LaserOccupancyGrid",
"(Constructor): Entering");
71 std::string cfg_prefix =
"/plugins/colli/";
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());
77 config->
get_float((cfg_prefix +
"laser_occupancy_grid/history/max_length").c_str());
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());
83 cfg_emergency_stop_beams_used_ =
84 config->
get_float((cfg_prefix +
"emergency_stopping/beams_used").c_str());
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;
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;
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_));
106 angle_range_ =
deg2rad((360 - cfg_delete_invisible_old_obstacles_angle_min_)
107 + cfg_delete_invisible_old_obstacles_angle_max_);
109 angle_min_ =
deg2rad(cfg_delete_invisible_old_obstacles_angle_min_);
111 reference_frame_ = config->
get_string((cfg_prefix +
"frame/odometry").c_str());
113 (cfg_prefix +
"frame/laser")
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());
120 if_buffer_size_ = config->
get_int((cfg_prefix +
"laser_occupancy_grid/buffer_size").c_str());
121 if_buffer_size_ = std::max(
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());
132 if_buffer_filled_.resize(if_buffer_size_);
133 std::fill(if_buffer_filled_.begin(), if_buffer_filled_.end(),
false);
137 robo_shape_.reset(
new RoboShapeColli((cfg_prefix +
"roboshape/").c_str(), logger, config));
138 old_readings_.clear();
141 logger->
log_debug(
"LaserOccupancyGrid",
"Generating obstacle map");
142 bool obstacle_shape = robo_shape_->is_angular_robot() && !cfg_force_elipse_obstacle_;
144 logger->
log_debug(
"LaserOccupancyGrid",
"Generating obstacle map done");
152 robo_shape_->get_complete_width_x() / 2.f - robo_shape_->get_robot_length_for_deg(0);
154 robo_shape_->get_complete_width_y() / 2.f - robo_shape_->get_robot_length_for_deg(90);
156 "Laser (x,y) offset from robo-center is (%f, %f)",
160 logger->
log_debug(
"LaserOccupancyGrid",
"(Constructor): Exiting");
167 obstacle_map_.reset();
174 old_readings_.clear();
175 old_readings_.reserve(initial_history_size_);
182 LaserOccupancyGrid::update_laser()
185 int if_buffer_free_pos = -1;
187 for (
int i = 0; i < if_buffer_size_; ++i) {
188 if (if_buffer_filled_[i] ==
false) {
189 if_buffer_free_pos = i;
194 if (if_buffer_free_pos < 0) {
199 int if_buffer_oldest_pos = -1;
201 for (
int i = 0; i < if_buffer_size_; ++i) {
203 if_buffer_oldest_pos = i;
207 if_buffer_free_pos = if_buffer_oldest_pos;
211 if_buffer_filled_[if_buffer_free_pos] =
true;
213 new_readings_.clear();
216 for (
int i = 0; i < if_buffer_size_; ++i) {
217 if (if_buffer_filled_[i] ==
true) {
220 if_buffer_filled_[i] =
false;
222 const Time *laser_time = if_laser_->
timestamp();
223 std::string laser_frame = if_laser_->
frame();
225 tf::StampedTransform transform;
228 tf_listener_->
lookup_transform(reference_frame_, laser_frame, laser_time, transform);
230 tf::Vector3 pos_robot_tf = transform.getOrigin();
233 double angle_inc = M_PI * 2. / 360.;
237 if (if_laser_->
distances(i) >= min_laser_length_) {
239 polar_coord_2d_t point_polar;
241 point_polar.phi = angle_inc * i;
245 polar2cart2d(point_polar.phi, point_polar.r, &point_cart.x, &point_cart.y);
248 p.setValue(point_cart.x, point_cart.y, 0.);
251 LaserOccupancyGrid::LaserPoint point;
253 point.timestamp = Time(laser_time);
255 new_readings_.push_back(point);
257 if (cfg_delete_invisible_old_obstacles_) {
259 if (angle_dist >= 0 && angle_dist <= angle_range_) {
260 validate_old_laser_points(pos_robot, point.coord);
265 }
catch (Exception &e) {
266 if_buffer_filled_[i] =
true;
267 if (cfg_write_spam_debug_) {
269 "LaserOccupancyGrid",
270 "Unable to transform %s to %s. Laser-data not used, will keeped in history.",
272 reference_frame_.c_str());
285 LaserOccupancyGrid::validate_old_laser_points(
cart_coord_2d_t pos_robot,
288 std::vector<LaserPoint> old_readings_tmp;
291 cart_coord_2d_t v_new(pos_new_laser_point.x - pos_robot.x, pos_new_laser_point.y - pos_robot.y);
295 float d_new = sqrt(v_new.x * v_new.x + v_new.y * v_new.y);
301 static const float deg_unit = M_PI / 180.f;
303 for (std::vector<LaserPoint>::iterator it = old_readings_.begin(); it != old_readings_.end();
305 v_old.x = (*it).coord.x - pos_robot.x;
306 v_old.y = (*it).coord.y - pos_robot.y;
309 float d_old = sqrt(v_old.x * v_old.x + v_old.y * v_old.y);
312 if (d_new <= d_old + obstacle_distance_) {
315 old_readings_tmp.push_back(*it);
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) {
323 old_readings_tmp.push_back(*it);
334 old_readings_.clear();
335 old_readings_.reserve(old_readings_tmp.size());
337 for (
unsigned int i = 0; i < old_readings_tmp.size(); ++i) {
338 old_readings_.push_back(old_readings_tmp[i]);
343 LaserOccupancyGrid::obstacle_in_path_distance(
float vx,
float vy)
348 float distance_min = 1000;
350 int cfg_beams = cfg_emergency_stop_beams_used_;
352 int beams_start = angle - int(cfg_beams / 2);
353 if (beams_start < 0) {
357 int beams_end = beams_start + cfg_beams;
358 if (beams_end >= 360) {
362 for (
int i = beams_start; i != beams_end; i = (i + 1) % 360) {
364 if (dist != 0 && std::isfinite(dist)) {
365 distance_min = std::min(distance_min, dist);
385 float vel = std::sqrt(vx * vx + vy * vy);
387 float next_obstacle = obstacle_in_path_distance(vx, vy);
392 for (
int y = 0; y <
width_; ++y)
393 for (
int x = 0; x <
height_; ++x)
405 "Unable to transform %s to %s. Can't put obstacles into the grid",
406 reference_frame_.c_str(),
407 laser_frame_.c_str());
411 integrate_old_readings(midX, midY, inc, vel, transform);
412 integrate_new_readings(midX, midY, inc, vel, transform);
414 return next_obstacle;
423 std::vector<LaserOccupancyGrid::LaserPoint> *
424 LaserOccupancyGrid::transform_laser_points(std::vector<LaserOccupancyGrid::LaserPoint> &laserPoints,
427 int count_points = laserPoints.size();
428 std::vector<LaserOccupancyGrid::LaserPoint> *laserPointsTransformed =
429 new std::vector<LaserOccupancyGrid::LaserPoint>();
430 laserPointsTransformed->reserve(count_points);
434 for (
int i = 0; i < count_points; ++i) {
435 p.setValue(laserPoints[i].coord.x, laserPoints[i].coord.y, 0.);
438 LaserOccupancyGrid::LaserPoint point;
440 point.timestamp = laserPoints[i].timestamp;
441 laserPointsTransformed->push_back(point);
444 return laserPointsTransformed;
463 offset_base_.
x = (int)round((offset_laser_.
x + x) * 100.f /
cell_height_);
464 offset_base_.
y = (int)round((offset_laser_.
y + y) * 100.f /
cell_width_);
477 LaserOccupancyGrid::integrate_old_readings(
int midX,
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);
488 float newpos_x, newpos_y;
491 Time history =
Time(clock) -
Time(
double(std::max(min_history_length_, max_history_length_)));
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;
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]);
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);
536 old_readings_.clear();
537 old_readings_.reserve(old_readings.size());
540 for (
unsigned int i = 0; i < old_readings.size(); i++)
541 old_readings_.push_back(old_readings[i]);
543 delete pointsTransformed;
547 LaserOccupancyGrid::integrate_new_readings(
int midX,
551 tf::StampedTransform &transform)
553 std::vector<LaserOccupancyGrid::LaserPoint> *pointsTransformed =
554 transform_laser_points(new_readings_, transform);
556 int numberOfReadings = pointsTransformed->size();
561 float oldp_x = 1000.f;
562 float oldp_y = 1000.f;
564 for (
int i = 0; i < numberOfReadings; i++) {
565 point = (*pointsTransformed)[i].coord;
567 if (sqrt(
sqr(point.x) +
sqr(point.y)) >= min_laser_length_
568 &&
distance(point.x, point.y, oldp_x, oldp_y) >= obstacle_distance_) {
571 posX = midX + (int)((point.x * 100.f) / ((float)
cell_height_));
572 posY = midY + (int)((point.y * 100.f) / ((float)
cell_width_));
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_);
578 float height = robo_shape_->get_complete_width_x();
579 height = std::max(4.f, ((height + inc) * 100.f) /
cell_height_);
581 integrate_obstacle(posX, posY, width, height);
583 old_readings_.push_back(new_readings_[i]);
587 delete pointsTransformed;
591 LaserOccupancyGrid::integrate_obstacle(
int x,
int y,
int width,
int height)
593 std::vector<int> fast_obstacle = obstacle_map_->get_obstacle(width, height, cfg_obstacle_inc_);
596 for (
unsigned int i = 0; i < fast_obstacle.size(); i += 3) {
602 int posX = x + fast_obstacle[i] + offset_base_.
x;
603 int posY = y + fast_obstacle[i + 1] + offset_base_.
y;
605 if ((posX > 0) && (posX <
height_) && (posY > 0) && (posY <
width_)
Laser360Interface Fawkes BlackBoard Interface.
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.
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.
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.
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...
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.
virtual void log_debug(const char *component, const char *format,...)
Log debug message.
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.