Fawkes API  Fawkes Development Version
og_laser.h
1 
2 /***************************************************************************
3  * og_laser.h - Occupancy grid for colli's A* search
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 Stefan Jacobs
7  * 2013-2014 Bahram Maleki-Fard
8  * 2014 Tobias Neumann
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #ifndef __PLUGINS_COLLI_SEARCH_OG_LASER_H_
25 #define __PLUGINS_COLLI_SEARCH_OG_LASER_H_
26 
27 #include "../utils/occupancygrid/occupancygrid.h"
28 #include "../common/types.h"
29 
30 #include <utils/math/types.h>
31 #include <utils/time/time.h>
32 #include <string>
33 
34 #include <tf/transformer.h>
35 
36 namespace fawkes
37 {
38 #if 0 /* just to make Emacs auto-indent happy */
39 }
40 #endif
41 
42 class Laser360Interface;
43 class RoboShapeColli;
44 class ColliObstacleMap;
45 
46 class Logger;
47 class Configuration;
48 
50 {
51  public:
52  LaserOccupancyGrid( Laser360Interface * laser, Logger* logger, Configuration* config, tf::Transformer* listener,
53  int width = 150, int height = 150, int cell_width = 5, int cell_height = 5);
55 
56  ///\brief Put the laser readings in the occupancy grid
57  float update_occ_grid( int mid_x, int mid_y, float inc, float vx, float vy );
58 
59  ///\brief Reset all old readings and forget about the world state!
60  void reset_old();
61 
62  ///\brief Get the laser's position in the grid
64 
65  ///\brief Set the offset of base_link from laser
66  void set_base_offset(float x, float y);
67 
68  ///\brief Get cell costs
70 
71  private:
72  class LaserPoint {
73  public:
74  cart_coord_2d_t coord;
75  Time timestamp;
76 
77  LaserPoint() { }
78 // LaserPoint(LaserPoint& src) {
79 // coord = src.coord;
80 // timestamp = src.timestamp;
81 // }
82 // LaserPoint operator=(LaserPoint src) {
83 // coord = src.coord;
84 // timestamp = src.timestamp;
85 // return src;
86 // }
87  };
88 
89  void update_laser();
90 
91  float obstacle_in_path_distance( float vx, float vy );
92 
93  void validate_old_laser_points(cart_coord_2d_t pos_robot, cart_coord_2d_t pos_new_laser_point);
94 
95  std::vector< LaserPoint >* transform_laser_points(std::vector< LaserPoint >& laser_points, tf::StampedTransform& transform);
96 
97  /** Integrate historical readings to the current occgrid. */
98  void integrate_old_readings( int mid_x, int mid_y, float inc, float vel,
99  tf::StampedTransform& transform );
100 
101  /** Integrate the current readings to the current occgrid. */
102  void integrate_new_readings( int mid_x, int mid_y, float inc, float vel,
103  tf::StampedTransform& transform );
104 
105  /** Integrate a single obstacle
106  * @param x x coordinate of obstacle center
107  * @param y y coordinate of obstacle center
108  * @param width total width of obstacle
109  * @param height total height of obstacle
110  */
111  void integrate_obstacle( int x, int y, int width, int height );
112 
113  tf::Transformer* tf_listener_;
114  std::string reference_frame_;
115  std::string laser_frame_;
116  bool cfg_write_spam_debug_;
117 
118  Logger* logger_;
119  Laser360Interface* if_laser_;
120  RoboShapeColli* robo_shape_; /**< my roboshape */
121  ColliObstacleMap* obstacle_map; /**< fast obstacle map */
122 
123  std::vector< LaserPoint > new_readings_;
124  std::vector< LaserPoint > old_readings_; /**< readings history */
125 
126  point_t laser_pos_; /**< the laser's position in the grid */
127 
128  /** Costs for the cells in grid */
129  colli_cell_cost_t cell_costs_;
130 
131  /* interface buffer history */
132  int if_buffer_size_;
133  std::vector<bool> if_buffer_filled_;
134 
135  /** History concerned constants */
136  float max_history_length_, min_history_length_;
137  int initial_history_size_;
138 
139  /** Laser concerned settings */
140  float min_laser_length_;
141  float obstacle_distance_;
142 
143  int cfg_emergency_stop_beams_used_; /**< number of beams that are used to calculate the min distance to obstacle */
144 
145  bool cfg_obstacle_inc_ ; /**< increasing obstacles or not */
146  bool cfg_force_elipse_obstacle_; /**< the used shape for obstacles */
147 
148  bool cfg_delete_invisible_old_obstacles_; /**< delete old invalid obstables or not */
149  int cfg_delete_invisible_old_obstacles_angle_min_ ; /**< the min angle for old obstacles */
150  int cfg_delete_invisible_old_obstacles_angle_max_ ; /**< the max angle for old obstacles */
151  float angle_min_; /**< the angle min in rad */
152  float angle_range_; /**< the angle range from min - max */
153 
154  /** Offsets to robot center */
155  cart_coord_2d_t offset_laser_; /**< in meters */
156  point_t offset_base_; /**< in grid cells */
157 };
158 
159 } // namespace fawkes
160 
161 #endif
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.
Definition: og_laser.cpp:61
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
Definition: og_laser.cpp:431
Cartesian coordinates (2D).
Definition: types.h:59
Fawkes library namespace.
point_t get_laser_position()
Get the laser&#39;s position in the grid.
Definition: og_laser.cpp:421
A class for handling time.
Definition: time.h:91
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:442
float update_occ_grid(int mid_x, int mid_y, float inc, float vx, float vy)
Put the laser readings in the occupancy grid.
Definition: og_laser.cpp:357
Costs of occupancy-grid cells.
Definition: types.h:51
void reset_old()
Reset all old readings and forget about the world state!
Definition: og_laser.cpp:152
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:49
Occupancy Grid class for general use.
Definition: occupancygrid.h:39
~LaserOccupancyGrid()
Descturctor.
Definition: og_laser.cpp:145
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
Point with cartesian coordinates as signed integers.
Definition: types.h:40
This is an implementation of a collection of fast obstacles.
Definition: obstacle_map.h:42
This class is mainly the same as the basic class with the difference that all data is precalculated o...
Coordinate transforms between any two frames in a system.
Definition: transformer.h:68
Interface for configuration handling.
Definition: config.h:67
Interface for logging.
Definition: logger.h:34