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_)
This is supposed to be the central clock in Fawkes.
Time now() const
Get the current time.
static Clock * instance()
Clock initializer.
This is an implementation of a collection of fast obstacles.
Interface for configuration handling.
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.
void copy_shared_to_buffer(unsigned int buffer)
Copy data from private memory to buffer.
void read_from_buffer(unsigned int buffer)
Copy data from buffer to private memory.
const Time * timestamp() const
Get timestamp of last write.
Time buffer_timestamp(unsigned int buffer)
Get time of a buffer.
void resize_buffers(unsigned int num_buffers)
Resize buffer array.
void read()
Read from BlackBoard into local copy.
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.
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
~LaserOccupancyGrid()
Descturctor.
colli_cell_cost_t get_cell_costs() const
Get cell costs.
void reset_old()
Reset all old readings and forget about the world state!
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.
point_t get_laser_position()
Get the laser's position in the grid.
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.
Occupancy Grid class for general use.
int cell_height_
Cell height in cm.
void init_grid()
Init a new empty grid with the predefined parameters */.
int height_
Height of the grid in # cells.
std::vector< std::vector< Probability > > occupancy_probs_
The occupancy probability of the cells in a 2D array.
int width_
Width of the grid in # cells.
int cell_width_
Cell width in cm.
This class is mainly the same as the basic class with the difference that all data is precalculated o...
A class for handling time.
double in_sec() const
Convet time to seconds.
Fawkes library namespace.
double sqr(double x)
Fast square multiplication.
struct fawkes::point_struct point_t
Point with cartesian coordinates as signed integers.
float distance(float x1, float y1, float x2, float y2)
Get distance between two 2D cartesian coordinates.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
float normalize_rad(float angle_rad)
Normalize angle in radian between 0 (inclusive) and 2*PI (exclusive).
float angle_distance_signed(float angle_from, float angle_to)
Determines the signed distance between from "angle_from" to "angle_to" provided as radians.
float rad2deg(float rad)
Convert an angle given in radians to degrees.
void polar2cart2d(float polar_phi, float polar_dist, float *cart_x, float *cart_y)
Convert a 2D polar coordinate to a 2D cartesian coordinate.
struct fawkes::cart_coord_2d_struct cart_coord_2d_t
Cartesian coordinates (2D).
Cartesian coordinates (2D).
Costs of occupancy-grid cells.
unsigned int far
The cost for a cell near an obstacle (distance="near")
unsigned int occ
The cost for an occupied cell.
unsigned int mid
The cost for a cell near an obstacle (distance="near")
unsigned int near
The cost for a cell near an obstacle (distance="near")
unsigned int free
The cost for a free cell.