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
37namespace fawkes {
38
39class Laser360Interface;
40class RoboShapeColli;
41class ColliObstacleMap;
42
43class Logger;
44class Configuration;
45
47{
48public:
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
74private:
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
Interface for configuration handling.
Definition: config.h:68
Laser360Interface Fawkes BlackBoard Interface.
This OccGrid is derived by the Occupancy Grid originally from Andreas Strack, but modified for speed ...
Definition: og_laser.h:47
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
void set_base_offset(float x, float y)
Set the offset of base_link from laser.
Definition: og_laser.cpp:461
~LaserOccupancyGrid()
Descturctor.
Definition: og_laser.cpp:164
colli_cell_cost_t get_cell_costs() const
Get cell costs.
Definition: og_laser.cpp:471
void reset_old()
Reset all old readings and forget about the world state!
Definition: og_laser.cpp:172
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
point_t get_laser_position()
Get the laser's position in the grid.
Definition: og_laser.cpp:451
Interface for logging.
Definition: logger.h:42
Occupancy Grid class for general use.
Definition: occupancygrid.h:36
A class for handling time.
Definition: time.h:93
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Coordinate transforms between any two frames in a system.
Definition: transformer.h:65
Fawkes library namespace.
Cartesian coordinates (2D).
Definition: types.h:65
Costs of occupancy-grid cells.
Definition: types.h:50
Point with cartesian coordinates as signed integers.
Definition: types.h:42