Skip to content

Commit b535881

Browse files
tomlankhorsttomlankhorst
authored
Merge pull request #4 from leggedrobotics/merge-anybotics
Merge anybotics
2 parents 449f1f4 + a573e11 commit b535881

File tree

79 files changed

+1673
-1060
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

79 files changed

+1673
-1060
lines changed

‎README.md‎

Lines changed: 31 additions & 24 deletions
Large diffs are not rendered by default.

‎grid_map/CHANGELOG.rst‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,9 @@
22
Changelog for package grid_map
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
1.6.4 (2020-12-04)
6+
------------------
7+
58
1.6.2 (2019-10-14)
69
------------------
710

‎grid_map/package.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>grid_map</name>
4-
<version>1.6.2</version>
4+
<version>1.6.4</version>
55
<description>Meta-package for the universal grid map library.</description>
66
<maintainer email="mwulf@anybotics.com">Maximilian Wulf</maintainer>
77
<maintainer email="ynava@anybotics.com">Yoshua Nava</maintainer>

‎grid_map_core/CHANGELOG.rst‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,9 @@
22
Changelog for package grid_map_core
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
1.6.4 (2020-12-04)
6+
------------------
7+
58
1.6.2 (2019-10-14)
69
------------------
710
* Implements a grid map transformation from one map frame to another map frame given the transform between the frames.

‎grid_map_core/CMakeLists.txt‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.5.1)
22
project(grid_map_core)
33

44
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
57

68
## Find catkin macros and libraries
79
find_package(catkin REQUIRED)
@@ -146,4 +148,14 @@ if(CATKIN_ENABLE_TESTING)
146148
${PROJECT_NAME}-test
147149
)
148150
endif()
149-
endif()
151+
endif()
152+
153+
#################
154+
## Clang_tools ##
155+
#################
156+
find_package(cmake_clang_tools QUIET)
157+
if(cmake_clang_tools_FOUND)
158+
add_default_clang_tooling(
159+
DISABLE_CLANG_FORMAT
160+
)
161+
endif(cmake_clang_tools_FOUND)

‎grid_map_core/package.xml‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<package format="2">
33
<name>grid_map_core</name>
4-
<version>1.6.2</version>
4+
<version>1.6.4</version>
55
<description>Universal grid map library to manage two-dimensional grid maps with multiple data layers.</description>
66
<maintainer email="mwulf@anybotics.com">Maximilian Wulf</maintainer>
77
<maintainer email="ynava@anybotics.com">Yoshua Nava</maintainer>
@@ -10,6 +10,7 @@
1010
<url type="bugtracker">http://github.com/anybotics/grid_map/issues</url>
1111
<author email="pfankhauser@anybotics.com">Péter Fankhauser</author>
1212
<buildtool_depend>catkin</buildtool_depend>
13+
<!-- <build_depend>cmake_clang_tools</build_depend> -->
1314
<depend>eigen</depend>
1415
<!-- <test_depend>cmake_code_coverage</test_depend> -->
1516
<test_depend>gtest</test_depend>

‎grid_map_core/src/GridMap.cpp‎

Lines changed: 8 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -720,12 +720,13 @@ Position GridMap::getClosestPositionInMap(const Position& position) const {
720720
const double maxY = bottomLeftCorner.y();
721721
const double minY = bottomRightCorner.y();
722722

723-
// Clip to box constraints.
724-
positionInMap.x() = std::fmin(positionInMap.x(), maxX);
725-
positionInMap.y() = std::fmin(positionInMap.y(), maxY);
723+
// Clip to box constraints and correct for indexing precision.
724+
// Points on the border can lead to invalid indices because the cells represent half open intervals, i.e. [...).
725+
positionInMap.x() = std::fmin(positionInMap.x(), maxX - std::numeric_limits<double>::epsilon());
726+
positionInMap.y() = std::fmin(positionInMap.y(), maxY - std::numeric_limits<double>::epsilon());
726727

727-
positionInMap.x() = std::fmax(positionInMap.x(), minX);
728-
positionInMap.y() = std::fmax(positionInMap.y(), minY);
728+
positionInMap.x() = std::fmax(positionInMap.x(), minX + std::numeric_limits<double>::epsilon());
729+
positionInMap.y() = std::fmax(positionInMap.y(), minY + std::numeric_limits<double>::epsilon());
729730

730731
return positionInMap;
731732
}
@@ -751,23 +752,13 @@ void GridMap::clearAll() {
751752
}
752753

753754
void GridMap::clearRows(unsigned int index, unsigned int nRows) {
754-
std::vector<std::string> layersToClear;
755-
if (basicLayers_.size() > 0)
756-
layersToClear = basicLayers_;
757-
else
758-
layersToClear = layers_;
759-
for (auto& layer : layersToClear) {
755+
for (auto& layer : layers_) {
760756
data_.at(layer).block(index, 0, nRows, getSize()(1)).setConstant(NAN);
761757
}
762758
}
763759

764760
void GridMap::clearCols(unsigned int index, unsigned int nCols) {
765-
std::vector<std::string> layersToClear;
766-
if (basicLayers_.size() > 0)
767-
layersToClear = basicLayers_;
768-
else
769-
layersToClear = layers_;
770-
for (auto& layer : layersToClear) {
761+
for (auto& layer : layers_) {
771762
data_.at(layer).block(0, index, getSize()(0), nCols).setConstant(NAN);
772763
}
773764
}

‎grid_map_core/test/GridMapTest.cpp‎

Lines changed: 204 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -148,6 +148,210 @@ TEST(GridMap, ClipToMap)
148148
EXPECT_TRUE(map.isInside(clippedPositionOutMap));
149149
}
150150

151+
152+
153+
TEST(GridMap, ClipToMap2)
154+
{
155+
GridMap map({"types"});
156+
map.setGeometry(Length(1.0, 1.0), 0.05, Position(0.0, 0.0));
157+
158+
// Test 8 points outside of map.
159+
/*
160+
* A B C
161+
* +---+
162+
* | | X
163+
* D| |E ^
164+
* | | |
165+
* +---+ Y<--+
166+
* F G H
167+
*
168+
* Note: Position to index alignment is an half open interval.
169+
* An example position of 0.5 is assigned to the upper index.
170+
* The interval in the current example is:
171+
* Position: [...)[0.485 ... 0.5)[0.5 ... 0.505)[...)
172+
* Index: 8 9 10 11
173+
*/
174+
175+
Index insideIndex;
176+
Position outsidePosition;
177+
178+
// Point A
179+
outsidePosition = Position(1.0, 1.0);
180+
auto closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
181+
bool isInside = map.getIndex(closestInsidePosition, insideIndex);
182+
183+
auto expectedPosition = Position(0.5, 0.5);
184+
auto expectedIndex = Index(0, 0);
185+
186+
// Check position.
187+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
188+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
189+
190+
// Check index.
191+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
192+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
193+
194+
// Check if index is inside.
195+
EXPECT_TRUE(isInside) << "position is: " << std::endl
196+
<< closestInsidePosition << std::endl
197+
<< " index is: " << std::endl
198+
<< insideIndex << std::endl;
199+
200+
// Point B
201+
outsidePosition = Position(1.0, 0.0);
202+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
203+
isInside = map.getIndex(closestInsidePosition, insideIndex);
204+
205+
expectedPosition = Position(0.5, 0.0);
206+
expectedIndex = Index(0, 10);
207+
208+
// Check position.
209+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
210+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
211+
212+
// Check index.
213+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
214+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
215+
216+
// Check if index is inside.
217+
EXPECT_TRUE(isInside) << "position is: " << std::endl
218+
<< closestInsidePosition << std::endl
219+
<< " index is: " << std::endl
220+
<< insideIndex << std::endl;
221+
222+
// Point C
223+
outsidePosition = Position(1.0, -1.0);
224+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
225+
isInside = map.getIndex(closestInsidePosition, insideIndex);
226+
227+
expectedPosition = Position(0.5, -0.5);
228+
expectedIndex = Index(0, 19);
229+
230+
// Check position.
231+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
232+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
233+
234+
// Check index.
235+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
236+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
237+
238+
// Check if index is inside.
239+
EXPECT_TRUE(isInside) << "position is: " << std::endl
240+
<< closestInsidePosition << std::endl
241+
<< " index is: " << std::endl
242+
<< insideIndex << std::endl;
243+
244+
// Point D
245+
outsidePosition = Position(0.0, 1.0);
246+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
247+
isInside = map.getIndex(closestInsidePosition, insideIndex);
248+
249+
expectedPosition = Position(0.0, 0.5);
250+
expectedIndex = Index(10, 0);
251+
252+
// Check position.
253+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
254+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
255+
256+
// Check index.
257+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
258+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
259+
260+
// Check if index is inside.
261+
EXPECT_TRUE(isInside) << "position is: " << std::endl
262+
<< closestInsidePosition << std::endl
263+
<< " index is: " << std::endl
264+
<< insideIndex << std::endl;
265+
266+
// Point E
267+
outsidePosition = Position(0.0, -1.0);
268+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
269+
isInside = map.getIndex(closestInsidePosition, insideIndex);
270+
271+
expectedPosition = Position(0.0, -0.5);
272+
expectedIndex = Index(10, 19);
273+
274+
// Check position.
275+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
276+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
277+
278+
// Check index.
279+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
280+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
281+
282+
// Check if index is inside.
283+
EXPECT_TRUE(isInside) << "position is: " << std::endl
284+
<< closestInsidePosition << std::endl
285+
<< " index is: " << std::endl
286+
<< insideIndex << std::endl;
287+
288+
// Point F
289+
outsidePosition = Position(-1.0, 1.0);
290+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
291+
isInside = map.getIndex(closestInsidePosition, insideIndex);
292+
293+
expectedPosition = Position(-0.5, 0.5);
294+
expectedIndex = Index(19, 0);
295+
296+
// Check position.
297+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
298+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
299+
300+
// Check index.
301+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
302+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
303+
304+
// Check if index is inside.
305+
EXPECT_TRUE(isInside) << "position is: " << std::endl
306+
<< closestInsidePosition << std::endl
307+
<< " index is: " << std::endl
308+
<< insideIndex << std::endl;
309+
310+
// Point G
311+
outsidePosition = Position(-1.0, 0.0);
312+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
313+
isInside = map.getIndex(closestInsidePosition, insideIndex);
314+
315+
expectedPosition = Position(-0.5, 0.0);
316+
expectedIndex = Index(19, 10);
317+
318+
// Check position.
319+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
320+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
321+
322+
// Check index.
323+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
324+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
325+
326+
// Check if index is inside.
327+
EXPECT_TRUE(isInside) << "position is: " << std::endl
328+
<< closestInsidePosition << std::endl
329+
<< " index is: " << std::endl
330+
<< insideIndex << std::endl;
331+
332+
// Point H
333+
outsidePosition = Position(-1.0, -1.0);
334+
closestInsidePosition = map.getClosestPositionInMap(outsidePosition);
335+
isInside = map.getIndex(closestInsidePosition, insideIndex);
336+
337+
expectedPosition = Position(-0.5, -0.5);
338+
expectedIndex = Index(19, 19);
339+
340+
// Check position.
341+
EXPECT_DOUBLE_EQ(expectedPosition.x(), closestInsidePosition.x());
342+
EXPECT_DOUBLE_EQ(expectedPosition.y(), closestInsidePosition.y());
343+
344+
// Check index.
345+
EXPECT_EQ(expectedIndex.x(), insideIndex.x()) << "closestInsidePosition" << closestInsidePosition;
346+
EXPECT_EQ(expectedIndex.y(), insideIndex.y()) << "closestInsidePosition" << closestInsidePosition;
347+
348+
// Check if index is inside.
349+
EXPECT_TRUE(isInside) << "position is: " << std::endl
350+
<< closestInsidePosition << std::endl
351+
<< " index is: " << std::endl
352+
<< insideIndex << std::endl;
353+
}
354+
151355
TEST(AddDataFrom, ExtendMapAligned)
152356
{
153357
GridMap map1, map2;

‎grid_map_costmap_2d/CHANGELOG.rst‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,9 @@
22
Changelog for package grid_map_costmap_2d
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
1.6.4 (2020-12-04)
6+
------------------
7+
58
1.6.2 (2019-10-14)
69
------------------
710

‎grid_map_costmap_2d/CMakeLists.txt‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,8 @@ cmake_minimum_required(VERSION 3.5.1)
22
project(grid_map_costmap_2d)
33

44
set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
57

68
## Find catkin macros and libraries
79
find_package(catkin REQUIRED
@@ -91,4 +93,14 @@ if(CATKIN_ENABLE_TESTING)
9193
${PROJECT_NAME}-test
9294
)
9395
endif()
94-
endif()
96+
endif()
97+
98+
#################
99+
## Clang_tools ##
100+
#################
101+
find_package(cmake_clang_tools QUIET)
102+
if(cmake_clang_tools_FOUND)
103+
add_default_clang_tooling(
104+
DISABLE_CLANG_FORMAT
105+
)
106+
endif(cmake_clang_tools_FOUND)

0 commit comments

Comments
 (0)