Skip to content

Commit d1fab4d

Browse files
bhaskarabhaskara
committed
Initial commit.
Just copied over from https://code.ros.org/svn/ros-pkg/stacks/graph_mapping/trunk/occupancy_grid_utils r40053.
0 parents  commit d1fab4d

26 files changed

+3546
-0
lines changed

‎CMakeLists.txt

+46
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
cmake_minimum_required(VERSION 2.4.6)
2+
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
3+
4+
# Set the build type. Options are:
5+
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
6+
# Debug : w/ debug symbols, w/o optimization
7+
# Release : w/o debug symbols, w/ optimization
8+
# RelWithDebInfo : w/ debug symbols, w/ optimization
9+
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
10+
set(ROS_BUILD_TYPE RelWithDebInfo)
11+
12+
rosbuild_init()
13+
rosbuild_genmsg()
14+
15+
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
16+
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
17+
18+
rosbuild_add_boost_directories()
19+
find_package(PythonLibs REQUIRED)
20+
include_directories(${PYTHON_INCLUDE_PATH})
21+
link_libraries(${PYTHON_LIBRARIES})
22+
23+
rosbuild_add_library(grid_utils src/ray_tracer.cpp
24+
src/grid_overlay.cpp
25+
src/shortest_path.cpp
26+
src/coordinate_conversions.cpp
27+
src/combine_grids.cpp
28+
src/file.cpp)
29+
target_link_libraries(grid_utils SDL SDL_image)
30+
31+
rosbuild_add_library(occupancy_grid_utils src/boost_python_exports.cpp)
32+
33+
set_target_properties(occupancy_grid_utils
34+
PROPERTIES
35+
PREFIX "")
36+
37+
target_link_libraries(occupancy_grid_utils grid_utils)
38+
rosbuild_link_boost(occupancy_grid_utils python)
39+
40+
rosbuild_add_gtest(test_grid_utils test/test_grid_utils.cpp)
41+
target_link_libraries(test_grid_utils grid_utils)
42+
43+
rosbuild_add_executable(grid_construction_node
44+
src/examples/grid_construction_node.cpp)
45+
target_link_libraries(grid_construction_node
46+
grid_utils)

‎Makefile

+1
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
include $(shell rospack find mk)/cmake.mk

‎include/occupancy_grid_utils/combine_grids.h

+66
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
/*
2+
* Copyright (c) 2008, Willow Garage, Inc.
3+
* All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* * Redistributions of source code must retain the above copyright
9+
* notice, this list of conditions and the following disclaimer.
10+
* * Redistributions in binary form must reproduce the above copyright
11+
* notice, this list of conditions and the following disclaimer in the
12+
* documentation and/or other materials provided with the distribution.
13+
* * Neither the name of the Willow Garage, Inc. nor the names of its
14+
* contributors may be used to endorse or promote products derived from
15+
* this software without specific prior written permission.
16+
*
17+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21+
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
* POSSIBILITY OF SUCH DAMAGE.
28+
*
29+
*/
30+
31+
32+
/**
33+
* \file
34+
*
35+
* Combining overlapping grids
36+
*
37+
* \author Bhaskara Marthi
38+
*/
39+
40+
#ifndef OCCUPANCY_GRID_UTILS_COMBINE_GRIDS_H
41+
#define OCCUPANCY_GRID_UTILS_COMBINE_GRIDS_H
42+
43+
#include <nav_msgs/OccupancyGrid.h>
44+
45+
namespace occupancy_grid_utils
46+
{
47+
48+
/// \brief Combines a set of grids
49+
///
50+
/// The resulting grid's origin will be a translated version of grid 1's origin, with resolution \a resolution
51+
///
52+
/// The policy for combination is that for each cell, we look at each grid cell that
53+
/// intersects it and consider their values. Map these to integers, where everything above
54+
/// OCCUPIED (100) goes to -1. Then take the max. If there are no intersecting cells, value is -1.
55+
///
56+
/// Assumes all grids lie on the xy plane, and will fail in weird ways if that's not true
57+
nav_msgs::OccupancyGrid::Ptr combineGrids(const std::vector<nav_msgs::OccupancyGrid::ConstPtr>& grids, double resolution);
58+
59+
/// Version of combineGrids that uses the resolution of the first grid.
60+
nav_msgs::OccupancyGrid::Ptr combineGrids(const std::vector<nav_msgs::OccupancyGrid::ConstPtr>& grids);
61+
62+
63+
64+
} // namespace occupancy_grid_utils
65+
66+
#endif // include guard

‎include/occupancy_grid_utils/coordinate_conversions.h

+209
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,209 @@
1+
/*
2+
* Copyright (c) 2008, Willow Garage, Inc.
3+
* All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions are met:
7+
*
8+
* * Redistributions of source code must retain the above copyright
9+
* notice, this list of conditions and the following disclaimer.
10+
* * Redistributions in binary form must reproduce the above copyright
11+
* notice, this list of conditions and the following disclaimer in the
12+
* documentation and/or other materials provided with the distribution.
13+
* * Neither the name of the Willow Garage, Inc. nor the names of its
14+
* contributors may be used to endorse or promote products derived from
15+
* this software without specific prior written permission.
16+
*
17+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18+
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19+
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20+
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21+
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22+
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23+
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24+
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25+
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26+
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27+
* POSSIBILITY OF SUCH DAMAGE.
28+
*
29+
*/
30+
31+
32+
/**
33+
* \file
34+
*
35+
* General utilities for coordinate conversions
36+
*
37+
* \author Bhaskara Marthi
38+
*/
39+
40+
41+
#ifndef OCCUPANCY_GRID_UTILS_COORDINATE_CONVERSIONS_H
42+
#define OCCUPANCY_GRID_UTILS_COORDINATE_CONVERSIONS_H
43+
44+
#include <occupancy_grid_utils/exceptions.h>
45+
#include <tf/transform_datatypes.h>
46+
#include <nav_msgs/OccupancyGrid.h>
47+
#include <geometry_msgs/Polygon.h>
48+
#include <ros/assert.h>
49+
#include <cstdlib>
50+
#include <ostream>
51+
52+
namespace occupancy_grid_utils
53+
{
54+
55+
/************************************************************
56+
* Public interface
57+
************************************************************/
58+
59+
typedef uint32_t index_t;
60+
typedef int16_t coord_t;
61+
62+
struct Cell
63+
{
64+
Cell(const coord_t x=0, const coord_t y=0) : x(x), y(y) {}
65+
coord_t x;
66+
coord_t y;
67+
68+
bool operator== (const Cell& c) const;
69+
bool operator< (const Cell& c) const;
70+
};
71+
72+
73+
// These values have conventional meanings for occupancy grids
74+
const int8_t UNOCCUPIED=0;
75+
const int8_t OCCUPIED=100;
76+
const int8_t UNKNOWN=255;
77+
78+
79+
/// \brief Returns the index of a cell.
80+
///
81+
/// \throws CellOutOfBoundsException if cell isn't within grid bounds
82+
index_t cellIndex (const nav_msgs::MapMetaData& info, const Cell& c);
83+
84+
/// \brief Returns cell corresponding to index
85+
Cell indexCell (const nav_msgs::MapMetaData& info, index_t ind);
86+
87+
/// \brief Returns cell corresponding to a point.
88+
///
89+
/// The z coordinate of the point is ignored.
90+
Cell pointCell (const nav_msgs::MapMetaData& info, const geometry_msgs::Point& p);
91+
92+
/// \brief Returns index of a point.
93+
/// \throws CellOutOfBoundsException if point isn't within grid bounds
94+
///
95+
/// Ignores z coordinate of point
96+
index_t pointIndex (const nav_msgs::MapMetaData& info, const geometry_msgs::Point& p);
97+
98+
/// \brief Return center of a cell
99+
geometry_msgs::Point cellCenter (const nav_msgs::MapMetaData& info, const Cell& c);
100+
101+
/// \brief Return polygon corresponding to a cell
102+
geometry_msgs::Polygon cellPolygon (const nav_msgs::MapMetaData& info, const Cell& c);
103+
104+
/// \brief Check if a point is on the grid
105+
bool withinBounds (const nav_msgs::MapMetaData& info, const geometry_msgs::Point& p);
106+
107+
/// \brief Check if a cell is on the grid
108+
bool withinBounds (const nav_msgs::MapMetaData& info, const Cell& c);
109+
110+
/// \brief Return polygon corresponding to grid bounds
111+
geometry_msgs::Polygon gridPolygon (const nav_msgs::MapMetaData& info);
112+
113+
/// \brief Verify that data vector has the right size, throw
114+
/// DataSizeException otherwise
115+
void verifyDataSize (const nav_msgs::OccupancyGrid& g);
116+
117+
/************************************************************
118+
* Implementations of inline functions
119+
************************************************************/
120+
121+
inline
122+
index_t cellIndex (const nav_msgs::MapMetaData& info, const Cell& c)
123+
{
124+
if (!withinBounds(info, c))
125+
throw CellOutOfBoundsException(c.x, c.y);
126+
return c.x + c.y*info.width;
127+
}
128+
129+
inline
130+
Cell indexCell (const nav_msgs::MapMetaData& info, const index_t ind)
131+
{
132+
const div_t result = div((int) ind, (int) info.width);
133+
return Cell(result.rem, result.quot);
134+
}
135+
136+
137+
inline
138+
tf::Transform mapToWorld (const nav_msgs::MapMetaData& info)
139+
{
140+
tf::Transform world_to_map;
141+
tf::poseMsgToTF (info.origin, world_to_map);
142+
return world_to_map;
143+
}
144+
145+
inline
146+
tf::Transform worldToMap (const nav_msgs::MapMetaData& info)
147+
{
148+
return mapToWorld(info).inverse();
149+
}
150+
151+
inline
152+
Cell pointCell (const nav_msgs::MapMetaData& info, const geometry_msgs::Point& p)
153+
{
154+
tf::Point pt;
155+
tf::pointMsgToTF(p, pt);
156+
tf::Point p2 = worldToMap(info)*pt;
157+
return Cell(floor(p2.x()/info.resolution), floor(p2.y()/info.resolution));
158+
}
159+
160+
inline
161+
index_t pointIndex (const nav_msgs::MapMetaData& info, const geometry_msgs::Point& p)
162+
{
163+
return cellIndex(info, pointCell(info, p));
164+
}
165+
166+
inline
167+
geometry_msgs::Point cellCenter (const nav_msgs::MapMetaData& info, const Cell& c)
168+
{
169+
tf::Point pt((c.x+0.5)*info.resolution, (c.y+0.5)*info.resolution, 0.0);
170+
geometry_msgs::Point p;
171+
tf::pointTFToMsg(mapToWorld(info)*pt, p);
172+
return p;
173+
}
174+
175+
176+
inline
177+
bool Cell::operator== (const Cell& c) const
178+
{
179+
return ((this->x == c.x) && (this->y == c.y));
180+
}
181+
182+
inline
183+
bool Cell::operator< (const Cell& c) const
184+
{
185+
return ((this->x < c.x) || ((this->x == c.x) && (this->y < c.y)));
186+
}
187+
188+
inline
189+
std::ostream& operator<< (std::ostream& str, const Cell& c)
190+
{
191+
str << "(" << c.x << ", " << c.y << ")";
192+
return str;
193+
}
194+
195+
inline
196+
bool withinBounds (const nav_msgs::MapMetaData& info, const geometry_msgs::Point& p)
197+
{
198+
return withinBounds(info, pointCell(info, p));
199+
}
200+
201+
inline
202+
bool withinBounds (const nav_msgs::MapMetaData& info, const Cell& c)
203+
{
204+
return (c.x >= 0) && (c.y >= 0) && (c.x < (coord_t) info.width) && (c.y < (coord_t) info.height);
205+
}
206+
207+
} // namespace occupancy_grid_utils
208+
209+
#endif // include guard

0 commit comments

Comments
 (0)