Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature : Adding Python bindings #335

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
Expand Up @@ -38,3 +38,6 @@

grid_map_pcl/data/*
!grid_map_pcl/data/input_cloud.pcd

# Python
*.pyc
1 change: 1 addition & 0 deletions grid_map/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<exec_depend>grid_map_rviz_plugin</exec_depend>
<exec_depend>grid_map_loader</exec_depend>
<exec_depend>grid_map_demos</exec_depend>
<exec_depend>grid_map_python</exec_depend>
<export>
<metapackage />
</export>
Expand Down
26 changes: 26 additions & 0 deletions grid_map_python/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
cmake_minimum_required(VERSION 3.5)
project(grid_map_python)

set(CMAKE_CXX_STANDARD 14)
add_compile_options(-O2 -Wall -Wextra -Wpedantic)

find_package(catkin REQUIRED COMPONENTS pybind11_catkin grid_map_core grid_map_msgs)
find_package(Eigen3 REQUIRED)
include_directories(
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)

catkin_python_setup()

pybind11_add_module(grid_map_python SHARED py_module.cpp py_core.cpp)
target_link_libraries(grid_map_python PRIVATE ${catkin_LIBRARIES} ${EIGEN3_LIBRARIES})
add_dependencies(grid_map_python ${grid_map_python_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
set_target_properties(grid_map_python
PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${PYTHON_INSTALL_DIR}
)

install(TARGETS grid_map_python
LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}
ARCHIVE DESTINATION ${PYTHON_INSTALL_DIR}
)
17 changes: 17 additions & 0 deletions grid_map_python/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
<?xml version="1.0"?>
<package format="2">
<name>grid_map_python</name>
<version>0.0.1</version>
<description>Python bindings to the grid_map package.</description>
<maintainer email="mwulf@anybotics.com">Maximilian Wulf</maintainer>
<maintainer email="ynava@anybotics.com">Yoshua Nava</maintainer>
<license>BSD</license>
<url type="website">http://github.com/anybotics/grid_map</url>
<url type="bugtracker">http://github.com/anybotics/grid_map/issues</url>
<author email="antoine.lima@hds.utc.fr">Antoine Lima</author>
<buildtool_depend>catkin</buildtool_depend>
<depend>eigen</depend>
<depend>pybind11_catkin</depend>
<depend>grid_map_core</depend>
<depend>grid_map_msgs</depend>
</package>
125 changes: 125 additions & 0 deletions grid_map_python/py_core.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,125 @@
#include <grid_map_core/GridMap.hpp>

// Python binding
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

namespace py = pybind11;
using grid_map::GridMap;
using grid_map::Position;
using grid_map::Index;
using grid_map::Length;
using grid_map::Time;
using grid_map::InterpolationMethods;
constexpr static auto pyref = py::return_value_policy::reference_internal;

void init_core(py::module m) {
py::module& core(m);

py::enum_<InterpolationMethods>(core, "InterpolationMethods")
.value("INTER_NEAREST", InterpolationMethods::INTER_NEAREST)
.value("INTER_LINEAR", InterpolationMethods::INTER_LINEAR)
.value("INTER_CUBIC_CONVOLUTION", InterpolationMethods::INTER_CUBIC_CONVOLUTION)
.value("INTER_CUBIC", InterpolationMethods::INTER_CUBIC)
.export_values();

py::class_<GridMap, std::shared_ptr<GridMap>>(core, "GridMapBinding")
// Constructors (copy handled below and destruction is done by pybind11 itself)
// (move constructor and operator= are not handled in python)
.def(py::init<const std::vector<std::string>>())
.def(py::init<>())

// Binding public functions of GridMap.hpp
.def("setGeometry", py::overload_cast<const Length&, const double, const Position&>(&GridMap::setGeometry), py::arg("length"), py::arg("resolution"), py::arg("position") = Position::Zero())
.def("add", py::overload_cast<const std::string&, const double>(&GridMap::add), py::arg("layer"), py::arg("value")=NAN)
.def("add", py::overload_cast<const std::string&, const GridMap::Matrix&>(&GridMap::add), py::arg("layer"), py::arg("data"))
.def("exists", &GridMap::exists, py::arg("layer"))
.def("getc", py::overload_cast<const std::string&>(&GridMap::get, py::const_), py::arg("layer")) // Constness is not supported in python, return a copy
.def("get", py::overload_cast<const std::string&>(&GridMap::get), py::arg("layer"), pyref)
.def("__getitem__", py::overload_cast<const std::string&>(&GridMap::operator[]), py::arg("layer"), pyref)
.def("erase", &GridMap::erase, py::arg("layer"))
.def("getLayers", &GridMap::getLayers)
.def("setBasicLayers", &GridMap::setBasicLayers, py::arg("basicLayers"))
.def("getBasicLayers", &GridMap::getBasicLayers)
.def("hasBasicLayers", &GridMap::hasBasicLayers)
.def("hasSameLayers", &GridMap::hasSameLayers, py::arg("other"))
.def("atPosition", py::overload_cast<const std::string&, const Position&>(&GridMap::atPosition), py::arg("layer"), py::arg("position"), pyref)
.def("atPosition", py::overload_cast<const std::string&, const Position&, InterpolationMethods>(&GridMap::atPosition, py::const_), py::arg("layer"), py::arg("position"), py::arg("interpolationMethod")=InterpolationMethods::INTER_NEAREST)
.def("atc", py::overload_cast<const std::string&, const Index&>(&GridMap::at, py::const_), py::arg("layer"), py::arg("index")) // Constness is not supported in python, return a copy
.def("at", py::overload_cast<const std::string&, const Index&>(&GridMap::at), py::arg("layer"), py::arg("index"), pyref)
.def("getIndex", &GridMap::getIndex, py::arg("position"), py::arg("index"))
.def("getPosition", py::overload_cast<const Index&, Position&>(&GridMap::getPosition, py::const_), py::arg("index"), py::arg("position"))
.def("isInside", &GridMap::isInside, py::arg("position"))
.def("isValidAt", py::overload_cast<const Index&>(&GridMap::isValid, py::const_), py::arg("index"))
.def("isLayerValidAt", py::overload_cast<const Index&, const std::string&>(&GridMap::isValid, py::const_), py::arg("index"), py::arg("layer"))
.def("isLayersValidAt", py::overload_cast<const Index&, const std::vector<std::string>&>(&GridMap::isValid, py::const_), py::arg("index"), py::arg("layers"))
.def("getPosition3", &GridMap::getPosition3, py::arg("layer"), py::arg("index"), py::arg("position"))
.def("getVector", &GridMap::getVector, py::arg("layerPrefix"), py::arg("index"), py::arg("vector"))
.def("getSubmap", py::overload_cast<const Position&, const Length&, bool&>(&GridMap::getSubmap, py::const_), py::arg("position"), py::arg("length"), py::arg("success"))
.def("getSubmap", py::overload_cast<const Position&, const Length&, Index&, bool&>(&GridMap::getSubmap, py::const_), py::arg("position"), py::arg("length"), py::arg("indexInSubmap"), py::arg("success"))
.def("getTransformedMap", &GridMap::getTransformedMap, py::arg("transform"), py::arg("heightLayerName"), py::arg("newFrameId"), py::arg("sampleRatio") = 0.0)
.def("setPosition", &GridMap::setPosition, py::arg("position"))
.def("move", py::overload_cast<const Position&>(&GridMap::move), py::arg("position"))
.def("addDataFrom", &GridMap::addDataFrom, py::arg("other"), py::arg("extendMap"), py::arg("overwriteData"), py::arg("copyAllLayers"), py::arg("layers"))
.def("extendToInclude", &GridMap::extendToInclude, py::arg("other"))
.def("clear", &GridMap::clear, py::arg("layer"))
.def("clearBasic", &GridMap::clearBasic)
.def("clearAll", &GridMap::clearAll)
.def("setTimestamp", &GridMap::setTimestamp, py::arg("timestamp"))
.def("getTimestamp", &GridMap::getTimestamp)
.def("setFrameId", &GridMap::setFrameId, py::arg("frameId"))
.def("getFrameId", &GridMap::getFrameId)
.def("getLength", &GridMap::getLength)
.def("getPosition", py::overload_cast<>(&GridMap::getPosition, py::const_))
.def("getResolution", &GridMap::getResolution)
.def("getSize", &GridMap::getSize)
.def("setStartIndex", &GridMap::setStartIndex, py::arg("startIndex"))
.def("getStartIndex", &GridMap::getStartIndex)
.def("isDefaultStartIndex", &GridMap::isDefaultStartIndex)
.def("convertToDefaultStartIndex", &GridMap::convertToDefaultStartIndex)
.def("getClosestPositionInMap", &GridMap::getClosestPositionInMap, py::arg("position"))

// Copy support
.def("__copy__", [](const GridMap& self) { return GridMap(self); })
.def("__deepcopy__", [](const GridMap& self, py::dict) { return GridMap(self); })

// Pickle support
.def(py::pickle(
[](const GridMap& gm) {
// Equivalent to __getstate__, return a tuple fully encoding the object
const std::vector<std::string>& layers = gm.getLayers();
std::vector<GridMap::Matrix> grids;
std::transform(layers.cbegin(), layers.cend(), std::back_inserter(grids), [&gm](const std::string& layer){ return gm[layer]; });

return py::make_tuple(
gm.getFrameId(),
gm.getTimestamp(),
layers,
grids,
gm.getBasicLayers(),
gm.getLength(),
gm.getResolution(),
gm.getPosition(),
gm.getStartIndex()
);
},
[](const py::tuple& t) {
// Equivalent to __setstate__, return an instance from tuple
if(t.size()!=9) {
throw std::runtime_error("Invalid pickled state!");
}

GridMap gm;
gm.setFrameId(t[0].cast<std::string>());
gm.setTimestamp(t[1].cast<Time>());
gm.setGeometry(t[5].cast<Length>(), t[6].cast<double>(), t[7].cast<Position>());
gm.setStartIndex(t[8].cast<Index>());
const auto layers = t[2].cast<std::vector<std::string>>();
const auto grids = t[3].cast<std::vector<GridMap::Matrix>>();
for(size_t i=0; i<layers.size(); i++) gm.add(layers[i], grids[i]);
gm.setBasicLayers(t[4].cast<std::vector<std::string>>());
return gm;
}
));
}
10 changes: 10 additions & 0 deletions grid_map_python/py_module.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>

// Forward declaration
void init_core(pybind11::module m);

PYBIND11_MODULE(grid_map_python, m) {
init_core(m);
}
12 changes: 12 additions & 0 deletions grid_map_python/setup.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD

from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup

# fetch values from package.xml
setup_args = generate_distutils_setup(
packages=['grid_map'],
package_dir={'': 'src'}
)

setup(**setup_args)
16 changes: 16 additions & 0 deletions grid_map_python/src/grid_map/__init__.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,16 @@
from grid_map_python import GridMapBinding
from .convert import from_msg, to_msg

class GridMap(GridMapBinding):
def __setitem__(self, layer, val):
self[layer][:] = val

def __repr__(self):
return f'<{self.getLength()[0]:.2f}x{self.getLength()[1]:.2f}x{self.getResolution():.2f} grid on {self.getLayers()} at {self.getPosition()}>'

@classmethod
def from_msg(cls, msg):
return from_msg(msg, cls)

def to_msg(self):
return to_msg(self)
42 changes: 42 additions & 0 deletions grid_map_python/src/grid_map/convert.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
import numpy as np
from grid_map_python import GridMapBinding

import rospy
from std_msgs.msg import Float32MultiArray, MultiArrayDimension
from grid_map_msgs.msg import GridMap as GridMapMsg

def from_msg(msg, cls=GridMapBinding):
gm = cls()
gm.setFrameId(msg.info.header.frame_id)
gm.setTimestamp(int(msg.info.header.stamp.to_sec()*10E9))
gm.setStartIndex(np.array((msg.outer_start_index, msg.inner_start_index)))
gm.setGeometry(
np.array((msg.info.length_x, msg.info.length_y)),
msg.info.resolution,
np.array((msg.info.pose.position.x, msg.info.pose.position.y))
)

for i, layer in enumerate(msg.layers):
gm.add(layer, np.float32(np.reshape(msg.data[i].data, (msg.data[i].layout.dim[1].size, msg.data[i].layout.dim[0].size), order='F')) )

return gm

def to_msg(self):
msg = GridMapMsg()
msg.info.header.stamp = rospy.Time(self.getTimestamp()/10E9)
msg.info.header.frame_id = self.getFrameId()
msg.info.resolution = self.getResolution()
msg.info.length_x = self.getLength()[0]
msg.info.length_y = self.getLength()[1]
msg.info.pose.position.x, msg.info.pose.position.y = self.getPosition()
msg.info.pose.orientation.w = 1
msg.layers = list(self.getLayers())
for layer in self.getLayers():
matrix = self[layer]
data_array = Float32MultiArray()
data_array.layout.dim.append(MultiArrayDimension("column_index", matrix.shape[1], matrix.shape[0]*matrix.shape[1]))
data_array.layout.dim.append(MultiArrayDimension("row_index", matrix.shape[0], matrix.shape[0]))
data_array.data = matrix.flatten(order='F')
msg.data.append(data_array)

return msg
67 changes: 67 additions & 0 deletions grid_map_python/tests/test_core.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,67 @@
#!/usr/bin/env python3

import copy
import pickle
import numpy as np
from grid_map import GridMap
np.set_printoptions(precision=2, linewidth=999, suppress=True, sign=' ', formatter={'float': '{:0.2f}'.format})

if __name__ == '__main__':
# Default constructor
gm = GridMap()
gm.setGeometry([10, 11], 0.01, [-27.6, 32.9])
gm_size = gm.getSize()
assert gm_size[0]==(10/0.01) and gm_size[1]==(11/0.01)

# Layer management
gm.add('a')
gm.add('b', 10)
gm.add('c', np.arange(gm_size.prod()).reshape(gm_size))
assert gm.exists('a')
assert np.isnan(gm['a'][12,2])
assert gm.exists('b')
assert gm['b'][12,2]==10
assert gm.exists('c')
assert gm['c'][12,2]==12*gm_size[1]+2
assert not gm.exists('d')

# Layer retrieval
a = gm.get('a')
a[2,5] = -64
b = gm.getc('b')
b[101, 53] = -99

assert isinstance(a, np.ndarray)
assert np.all(a.shape==gm_size)
assert gm['a'][2,5]==-64
assert np.isnan(gm['a'][2,6])
assert isinstance(b, np.ndarray)
assert np.all(b.shape==gm_size)
assert gm['b'][101, 53]==10 # Should not have changed the value as it b is a copy
assert gm['b'][101, 53]==10

gm.erase('b')
layers = gm.getLayers()
assert len(layers)==2 and layers[0]=='a' and layers[1]=='c'

gm.setBasicLayers(['a'])
basic_layers = gm.getBasicLayers()
assert len(basic_layers)==1 and basic_layers[0]=='a'
assert gm.hasBasicLayers()

# Copy
gm2 = copy.copy(gm)
gm3 = copy.copy(gm)
assert gm2.hasSameLayers(gm)
assert gm3.hasSameLayers(gm)

# Pickle
gm_bytes = pickle.dumps(gm)
gm4 = pickle.loads(gm_bytes)
assert gm4.hasSameLayers(gm)

# Pickle
gms_ref = {'1': gm, 'a': gm4, 'fiezb': gm2}
gms_bytes = pickle.dumps(gms_ref)
gms = pickle.loads(gms_bytes)
print(len(gms_bytes), len(gm_bytes), gms)
25 changes: 25 additions & 0 deletions grid_map_python/tests/test_ros.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
#!/usr/bin/env python3

import rospy
import numpy as np
from grid_map import GridMap, from_msg, to_msg
from grid_map_msgs.msg import GridMap as GridMapMsg

class GridMapPythonTest_ROS:
def __init__(self) :
self.pub = rospy.Publisher('out', GridMapMsg, queue_size=0)
self.sub = rospy.Subscriber('in', GridMapMsg, self.callback)

def callback(self, msg):
gm = from_msg(msg)
msgOut = to_msg(gm)
msgOut.info.header.seq = msg.info.header.seq
self.pub.publish(msgOut)

if __name__ == '__main__':
try:
rospy.init_node('GridMapPythonTest_ROS')
node = GridMapPythonTest_ROS()
rospy.spin()
except rospy.ROSInterruptException as e:
pass