24 #ifndef __PLUGINS_COLLI_SEARCH_OG_LASER_H_ 25 #define __PLUGINS_COLLI_SEARCH_OG_LASER_H_ 27 #include "../utils/occupancygrid/occupancygrid.h" 28 #include "../common/types.h" 30 #include <utils/math/types.h> 31 #include <utils/time/time.h> 34 #include <tf/transformer.h> 42 class Laser360Interface;
44 class ColliObstacleMap;
53 int width = 150,
int height = 150,
int cell_width = 5,
int cell_height = 5);
57 float update_occ_grid(
int mid_x,
int mid_y,
float inc,
float vx,
float vy );
91 float obstacle_in_path_distance(
float vx,
float vy );
95 std::vector< LaserPoint >* transform_laser_points(std::vector< LaserPoint >& laser_points,
tf::StampedTransform& transform);
98 void integrate_old_readings(
int mid_x,
int mid_y,
float inc,
float vel,
102 void integrate_new_readings(
int mid_x,
int mid_y,
float inc,
float vel,
111 void integrate_obstacle(
int x,
int y,
int width,
int height );
114 std::string reference_frame_;
115 std::string laser_frame_;
116 bool cfg_write_spam_debug_;
123 std::vector< LaserPoint > new_readings_;
124 std::vector< LaserPoint > old_readings_;
133 std::vector<bool> if_buffer_filled_;
136 float max_history_length_, min_history_length_;
137 int initial_history_size_;
140 float min_laser_length_;
141 float obstacle_distance_;
143 int cfg_emergency_stop_beams_used_;
145 bool cfg_obstacle_inc_ ;
146 bool cfg_force_elipse_obstacle_;
148 bool cfg_delete_invisible_old_obstacles_;
149 int cfg_delete_invisible_old_obstacles_angle_min_ ;
150 int cfg_delete_invisible_old_obstacles_angle_max_ ;
Laser360Interface Fawkes BlackBoard Interface.
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.
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
Cartesian coordinates (2D).
Fawkes library namespace.
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.
float update_occ_grid(int mid_x, int mid_y, float inc, float vx, float vy)
Put the laser readings in the occupancy grid.
Costs of occupancy-grid cells.
void reset_old()
Reset all old readings and forget about the world state!
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Occupancy Grid class for general use.
~LaserOccupancyGrid()
Descturctor.
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...
Interface for configuration handling.