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 "../common/types.h"
28 #include "../utils/occupancygrid/occupancygrid.h"
29 
30 #include <tf/transformer.h>
31 #include <utils/math/types.h>
32 #include <utils/time/time.h>
33 
34 #include <memory>
35 #include <string>
36 
37 namespace fawkes {
38 
39 class Laser360Interface;
40 class RoboShapeColli;
41 class ColliObstacleMap;
42 
43 class Logger;
44 class Configuration;
45 
47 {
48 public:
50  Logger * logger,
51  Configuration * config,
52  tf::Transformer * listener,
53  int width = 150,
54  int height = 150,
55  int cell_width = 5,
56  int cell_height = 5);
58 
59  ///\brief Put the laser readings in the occupancy grid
60  float update_occ_grid(int mid_x, int mid_y, float inc, float vx, float vy);
61 
62  ///\brief Reset all old readings and forget about the world state!
63  void reset_old();
64 
65  ///\brief Get the laser's position in the grid
67 
68  ///\brief Set the offset of base_link from laser
69  void set_base_offset(float x, float y);
70 
71  ///\brief Get cell costs
73 
74 private:
75  class LaserPoint
76  {
77  public:
78  cart_coord_2d_t coord;
79  Time timestamp;
80 
81  LaserPoint()
82  {
83  }
84  // LaserPoint(LaserPoint& src) {
85  // coord = src.coord;
86  // timestamp = src.timestamp;
87  // }
88  // LaserPoint operator=(LaserPoint src) {
89  // coord = src.coord;
90  // timestamp = src.timestamp;
91  // return src;
92  // }
93  };
94 
95  void update_laser();
96 
97  float obstacle_in_path_distance(float vx, float vy);
98 
99  void validate_old_laser_points(cart_coord_2d_t pos_robot, cart_coord_2d_t pos_new_laser_point);
100 
101  std::vector<LaserPoint> *transform_laser_points(std::vector<LaserPoint> &laser_points,
102  tf::StampedTransform & transform);
103 
104  /** Integrate historical readings to the current occgrid. */
105  void integrate_old_readings(int mid_x,
106  int mid_y,
107  float inc,
108  float vel,
109  tf::StampedTransform &transform);
110 
111  /** Integrate the current readings to the current occgrid. */
112  void integrate_new_readings(int mid_x,
113  int mid_y,
114  float inc,
115  float vel,
116  tf::StampedTransform &transform);
117 
118  /** Integrate a single obstacle
119  * @param x x coordinate of obstacle center
120  * @param y y coordinate of obstacle center
121  * @param width total width of obstacle
122  * @param height total height of obstacle
123  */
124  void integrate_obstacle(int x, int y, int width, int height);
125 
126  tf::Transformer *tf_listener_;
127  std::string reference_frame_;
128  std::string laser_frame_;
129  bool cfg_write_spam_debug_;
130 
131  Logger * logger_;
132  Laser360Interface * if_laser_;
133  std::shared_ptr<RoboShapeColli> robo_shape_; /**< my roboshape */
134  std::shared_ptr<ColliObstacleMap> obstacle_map_; /**< fast obstacle map */
135 
136  std::vector<LaserPoint> new_readings_;
137  std::vector<LaserPoint> old_readings_; /**< readings history */
138 
139  point_t laser_pos_; /**< the laser's position in the grid */
140 
141  /** Costs for the cells in grid */
142  colli_cell_cost_t cell_costs_;
143 
144  /* interface buffer history */
145  int if_buffer_size_;
146  std::vector<bool> if_buffer_filled_;
147 
148  /** History concerned constants */
149  float max_history_length_, min_history_length_;
150  int initial_history_size_;
151 
152  /** Laser concerned settings */
153  float min_laser_length_;
154  float obstacle_distance_;
155 
156  int
157  cfg_emergency_stop_beams_used_; /**< number of beams that are used to calculate the min distance to obstacle */
158 
159  bool cfg_obstacle_inc_; /**< increasing obstacles or not */
160  bool cfg_force_elipse_obstacle_; /**< the used shape for obstacles */
161 
162  bool cfg_delete_invisible_old_obstacles_; /**< delete old invalid obstables or not */
163  int cfg_delete_invisible_old_obstacles_angle_min_; /**< the min angle for old obstacles */
164  int cfg_delete_invisible_old_obstacles_angle_max_; /**< the max angle for old obstacles */
165  float angle_min_; /**< the angle min in rad */
166  float angle_range_; /**< the angle range from min - max */
167 
168  /** Offsets to robot center */
169  cart_coord_2d_t offset_laser_; /**< in meters */
170  point_t offset_base_; /**< in grid cells */
171 };
172 
173 } // namespace fawkes
174 
175 #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:55
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
Definition: og_laser.cpp:461
Cartesian coordinates (2D).
Definition: types.h:64
Fawkes library namespace.
point_t get_laser_position()
Get the laser's position in the grid.
Definition: og_laser.cpp:451
A class for handling time.
Definition: time.h:92
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:471
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:383
Costs of occupancy-grid cells.
Definition: types.h:49
void reset_old()
Reset all old readings and forget about the world state!
Definition: og_laser.cpp:172
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:46
Occupancy Grid class for general use.
Definition: occupancygrid.h:35
~LaserOccupancyGrid()
Descturctor.
Definition: og_laser.cpp:164
Transform that contains a timestamp and frame IDs.
Definition: types.h:91
Point with cartesian coordinates as signed integers.
Definition: types.h:41
Coordinate transforms between any two frames in a system.
Definition: transformer.h:64
Interface for configuration handling.
Definition: config.h:64
Interface for logging.
Definition: logger.h:41