24 #ifndef _PLUGINS_COLLI_SEARCH_OG_LASER_H_
25 #define _PLUGINS_COLLI_SEARCH_OG_LASER_H_
27 #include "../common/types.h"
28 #include "../utils/occupancygrid/occupancygrid.h"
30 #include <tf/transformer.h>
31 #include <utils/math/types.h>
32 #include <utils/time/time.h>
39 class Laser360Interface;
41 class ColliObstacleMap;
46 class LaserOccupancyGrid :
public OccupancyGrid
51 Configuration * config,
52 tf::Transformer * listener,
60 float update_occ_grid(
int mid_x,
int mid_y,
float inc,
float vx,
float vy);
97 float obstacle_in_path_distance(
float vx,
float vy);
101 std::vector<LaserPoint> *transform_laser_points(std::vector<LaserPoint> &laser_points,
102 tf::StampedTransform & transform);
105 void integrate_old_readings(
int mid_x,
109 tf::StampedTransform &transform);
112 void integrate_new_readings(
int mid_x,
116 tf::StampedTransform &transform);
124 void integrate_obstacle(
int x,
int y,
int width,
int height);
126 tf::Transformer *tf_listener_;
127 std::string reference_frame_;
128 std::string laser_frame_;
129 bool cfg_write_spam_debug_;
132 Laser360Interface * if_laser_;
133 std::shared_ptr<RoboShapeColli> robo_shape_;
134 std::shared_ptr<ColliObstacleMap> obstacle_map_;
136 std::vector<LaserPoint> new_readings_;
137 std::vector<LaserPoint> old_readings_;
142 colli_cell_cost_t cell_costs_;
146 std::vector<bool> if_buffer_filled_;
149 float max_history_length_, min_history_length_;
150 int initial_history_size_;
153 float min_laser_length_;
154 float obstacle_distance_;
157 cfg_emergency_stop_beams_used_;
159 bool cfg_obstacle_inc_;
160 bool cfg_force_elipse_obstacle_;
162 bool cfg_delete_invisible_old_obstacles_;
163 int cfg_delete_invisible_old_obstacles_angle_min_;
164 int cfg_delete_invisible_old_obstacles_angle_max_;