diff --git a/.gitignore b/.gitignore index 618178ac0..7a29e4d6a 100644 --- a/.gitignore +++ b/.gitignore @@ -38,3 +38,6 @@ grid_map_pcl/data/* !grid_map_pcl/data/input_cloud.pcd + +# Python +*.pyc diff --git a/grid_map/package.xml b/grid_map/package.xml index 37e44529b..35f88dcbd 100644 --- a/grid_map/package.xml +++ b/grid_map/package.xml @@ -19,6 +19,7 @@ grid_map_rviz_plugin grid_map_loader grid_map_demos + grid_map_python diff --git a/grid_map_python/CMakeLists.txt b/grid_map_python/CMakeLists.txt new file mode 100644 index 000000000..b1b38fe73 --- /dev/null +++ b/grid_map_python/CMakeLists.txt @@ -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} +) diff --git a/grid_map_python/package.xml b/grid_map_python/package.xml new file mode 100644 index 000000000..f0be0c588 --- /dev/null +++ b/grid_map_python/package.xml @@ -0,0 +1,17 @@ + + + grid_map_python + 0.0.1 + Python bindings to the grid_map package. + Maximilian Wulf + Yoshua Nava + BSD + http://github.com/anybotics/grid_map + http://github.com/anybotics/grid_map/issues + Antoine Lima + catkin + eigen + pybind11_catkin + grid_map_core + grid_map_msgs + diff --git a/grid_map_python/py_core.cpp b/grid_map_python/py_core.cpp new file mode 100644 index 000000000..bbf1267d1 --- /dev/null +++ b/grid_map_python/py_core.cpp @@ -0,0 +1,125 @@ +#include + +// Python binding +#include +#include +#include + +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_(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_>(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>()) + .def(py::init<>()) + + // Binding public functions of GridMap.hpp + .def("setGeometry", py::overload_cast(&GridMap::setGeometry), py::arg("length"), py::arg("resolution"), py::arg("position") = Position::Zero()) + .def("add", py::overload_cast(&GridMap::add), py::arg("layer"), py::arg("value")=NAN) + .def("add", py::overload_cast(&GridMap::add), py::arg("layer"), py::arg("data")) + .def("exists", &GridMap::exists, py::arg("layer")) + .def("getc", py::overload_cast(&GridMap::get, py::const_), py::arg("layer")) // Constness is not supported in python, return a copy + .def("get", py::overload_cast(&GridMap::get), py::arg("layer"), pyref) + .def("__getitem__", py::overload_cast(&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(&GridMap::atPosition), py::arg("layer"), py::arg("position"), pyref) + .def("atPosition", py::overload_cast(&GridMap::atPosition, py::const_), py::arg("layer"), py::arg("position"), py::arg("interpolationMethod")=InterpolationMethods::INTER_NEAREST) + .def("atc", py::overload_cast(&GridMap::at, py::const_), py::arg("layer"), py::arg("index")) // Constness is not supported in python, return a copy + .def("at", py::overload_cast(&GridMap::at), py::arg("layer"), py::arg("index"), pyref) + .def("getIndex", &GridMap::getIndex, py::arg("position"), py::arg("index")) + .def("getPosition", py::overload_cast(&GridMap::getPosition, py::const_), py::arg("index"), py::arg("position")) + .def("isInside", &GridMap::isInside, py::arg("position")) + .def("isValidAt", py::overload_cast(&GridMap::isValid, py::const_), py::arg("index")) + .def("isLayerValidAt", py::overload_cast(&GridMap::isValid, py::const_), py::arg("index"), py::arg("layer")) + .def("isLayersValidAt", py::overload_cast&>(&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(&GridMap::getSubmap, py::const_), py::arg("position"), py::arg("length"), py::arg("success")) + .def("getSubmap", py::overload_cast(&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(&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& layers = gm.getLayers(); + std::vector 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()); + gm.setTimestamp(t[1].cast