-
Notifications
You must be signed in to change notification settings - Fork 0
/
StepFilter.cpp
191 lines (153 loc) · 5.59 KB
/
StepFilter.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
/*
* StepFilter.cpp
*
* Created on: Mar 11, 2015
* Author: Martin Wermelinger
* Institute: ETH Zurich, Autonomous Systems Lab
*/
#include "filters/StepFilter.hpp"
// #include <pluginlib/class_list_macros.h>
#include <algorithm>
// Grid Map
#include <grid_map_ros/grid_map_ros.hpp>
#include "grid_map_cv/utilities.hpp"
using namespace grid_map;
namespace filters {
template<typename T>
StepFilter<T>::StepFilter()
: criticalValue_(0.3),
firstWindowRadius_(0.08),
secondWindowRadius_(0.08),
nCellCritical_(5),
type_("traversability_step")
{
}
template<typename T>
StepFilter<T>::~StepFilter()
{
}
template<typename T>
bool StepFilter<T>::configure()
{
if (!FilterBase<T>::getParam(std::string("critical_value"), criticalValue_)) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "Step filter did not find param critical_value.");
return false;
}
if (criticalValue_ < 0.0) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "Critical step height must be greater than zero.");
return false;
}
RCLCPP_DEBUG(rclcpp::get_logger("StepFilter"), "Critical step height = %f.", criticalValue_);
if (!FilterBase<T>::getParam(std::string("first_window_radius"),
firstWindowRadius_)) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "Step filter did not find param 'first_window_radius'.");
return false;
}
if (firstWindowRadius_ < 0.0) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "'first_window_radius' must be greater than zero.");
return false;
}
RCLCPP_DEBUG(rclcpp::get_logger("StepFilter"), "First window radius of step filter = %f.", firstWindowRadius_);
if (!FilterBase<T>::getParam(std::string("second_window_radius"),
secondWindowRadius_)) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "Step filter did not find param 'second_window_radius'.");
return false;
}
if (secondWindowRadius_ < 0.0) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "'second_window_radius' must be greater than zero.");
return false;
}
RCLCPP_DEBUG(rclcpp::get_logger("StepFilter"), "Second window radius of step filter = %f.", secondWindowRadius_);
if (!FilterBase<T>::getParam(std::string("critical_cell_number"),
nCellCritical_)) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "Step filter did not find param 'critical_cell_number'");
return false;
}
if (nCellCritical_ <= 0) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "'critical_cell_number' must be greater than zero.");
return false;
}
RCLCPP_DEBUG(rclcpp::get_logger("StepFilter"), "Number of critical cells of step filter = %d.", nCellCritical_);
if (!FilterBase<T>::getParam(std::string("map_type"), type_)) {
RCLCPP_ERROR(rclcpp::get_logger("StepFilter"), "Step filter did not find param map_type.");
return false;
}
RCLCPP_DEBUG(rclcpp::get_logger("StepFilter"), "Step map type = %s.", type_.c_str());
return true;
}
template<typename T>
bool StepFilter<T>::update(const T& mapIn, T& mapOut)
{
// Add new layers to the elevation map.
mapOut = mapIn;
mapOut.add(type_);
mapOut.add("step_height");
double height, step;
// First iteration through the elevation map.
for (GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
if (!mapOut.isValid(*iterator, "elevation"))
continue;
height = mapOut.at("elevation", *iterator);
double heightMax, heightMin;
// Requested position (center) of circle in map.
Eigen::Vector2d center;
mapOut.getPosition(*iterator, center);
// Get the highest step in the circular window.
bool init = false;
for (CircleIterator submapIterator(mapOut, center, firstWindowRadius_);
!submapIterator.isPastEnd(); ++submapIterator) {
if (!mapOut.isValid(*submapIterator, "elevation"))
continue;
height = mapOut.at("elevation", *submapIterator);
// Init heightMax and heightMin
if (!init) {
heightMax = height;
heightMin = height;
init = true;
continue;
}
if (height > heightMax)
heightMax = height;
if (height < heightMin)
heightMin = height;
}
if (init)
mapOut.at("step_height", *iterator) = heightMax - heightMin;
}
// Second iteration through the elevation map.
for (GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
int nCells = 0;
double stepMax = 0.0;
bool isValid = false;
// Requested position (center) of circle in map.
Eigen::Vector2d center;
mapOut.getPosition(*iterator, center);
// Compute the step height.
for (CircleIterator submapIterator(mapOut, center, secondWindowRadius_);
!submapIterator.isPastEnd(); ++submapIterator) {
if (!mapOut.isValid(*submapIterator, "step_height"))
continue;
isValid = true;
if (mapOut.at("step_height", *submapIterator) > stepMax) {
stepMax = mapOut.at("step_height", *submapIterator);
}
if (mapOut.at("step_height", *submapIterator) > criticalValue_)
nCells++;
}
if (isValid) {
step = std::min(stepMax,
(double) nCells / (double) nCellCritical_ * stepMax);
if (step < criticalValue_) {
mapOut.at(type_, *iterator) = 1.0 - step / criticalValue_;
} else {
mapOut.at(type_, *iterator) = 0.0;
}
}
}
// Remove unnecessary layer.
mapOut.erase("step_height");
return true;
}
} /* namespace */
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(filters::StepFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)