From 0464220cb3c904dc9772ca8aab165617453e33c6 Mon Sep 17 00:00:00 2001 From: Antoine Lima Date: Sun, 12 Dec 2021 13:20:14 +0100 Subject: [PATCH 1/7] Added minimal grid_map_core/GridMap python bindings Using pybind11 in a separate package --- .gitignore | 3 ++ grid_map/package.xml | 1 + grid_map_python/CMakeLists.txt | 26 ++++++++++++++ grid_map_python/package.xml | 17 +++++++++ grid_map_python/py_core.cpp | 44 ++++++++++++++++++++++++ grid_map_python/py_module.cpp | 10 ++++++ grid_map_python/setup.py | 12 +++++++ grid_map_python/src/grid_map/__init__.py | 1 + 8 files changed, 114 insertions(+) create mode 100644 grid_map_python/CMakeLists.txt create mode 100644 grid_map_python/package.xml create mode 100644 grid_map_python/py_core.cpp create mode 100644 grid_map_python/py_module.cpp create mode 100644 grid_map_python/setup.py create mode 100644 grid_map_python/src/grid_map/__init__.py 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..7c50e5a6c --- /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 17) +add_compile_options(-O2) + +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..ea76c771c --- /dev/null +++ b/grid_map_python/py_core.cpp @@ -0,0 +1,44 @@ +#include + +// Python binding +#include +#include +#include + +namespace py = pybind11; +using grid_map::GridMap; + +void init_core(py::module m) { + py::module& base_m(m); + + py::class_>(base_m, "GridMap") + .def(py::init<>()) + .def(py::init>()) + .def("setGeometry", py::overload_cast(&GridMap::setGeometry), py::arg("length"), py::arg("resolution"), py::arg("position")) + .def("add", py::overload_cast(&GridMap::add), py::arg("layer"), py::arg("value")) + .def("add", py::overload_cast(&GridMap::add), py::arg("layer"), py::arg("data")) + .def("exists", &GridMap::exists, py::arg("layer")) + .def("get", py::overload_cast(&GridMap::get, py::const_), py::arg("layer")) + .def("get", py::overload_cast(&GridMap::get), py::arg("layer")) + .def("__getitem__", py::overload_cast(&GridMap::operator[], py::const_), py::arg("layer")) + .def("__getitem__", py::overload_cast(&GridMap::operator[]), py::arg("layer")) + .def("erase", &GridMap::erase, py::arg("layer")); + .def("getLayers", &GridMap::getLayers) + .def("hasSameLayers", &GridMap::hasSameLayers, py::arg("other")) + .def("atPosition", py::overload_cast(&GridMap::atPosition), py::arg("layer"), py::arg("position")) + .def("atPosition", py::overload_cast(&GridMap::atPosition, py::const_), py::arg("layer"), py::arg("position"), py::arg("interpolationMethod")) + .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("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) +} diff --git a/grid_map_python/py_module.cpp b/grid_map_python/py_module.cpp new file mode 100644 index 000000000..46b810a0f --- /dev/null +++ b/grid_map_python/py_module.cpp @@ -0,0 +1,10 @@ +#include +#include +#include + +// Forward declaration +void init_core(pybind11::module m); + +PYBIND11_MODULE(grid_map_python, m) { + init_core(m); +} diff --git a/grid_map_python/setup.py b/grid_map_python/setup.py new file mode 100644 index 000000000..a2c9a9afb --- /dev/null +++ b/grid_map_python/setup.py @@ -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) diff --git a/grid_map_python/src/grid_map/__init__.py b/grid_map_python/src/grid_map/__init__.py new file mode 100644 index 000000000..4e9a35db8 --- /dev/null +++ b/grid_map_python/src/grid_map/__init__.py @@ -0,0 +1 @@ +from grid_map_python import * From d457070bfd578134a6304feeedc1399eafcbeea4 Mon Sep 17 00:00:00 2001 From: Antoine Lima Date: Sun, 12 Dec 2021 13:22:22 +0100 Subject: [PATCH 2/7] [grid_map_python] Added ROS conversion functions and a dedicated test --- grid_map_python/src/grid_map/__init__.py | 1 + grid_map_python/src/grid_map/convert.py | 42 ++++++++++++++++++++++++ grid_map_python/tests/test_ros.py | 25 ++++++++++++++ 3 files changed, 68 insertions(+) create mode 100644 grid_map_python/src/grid_map/convert.py create mode 100644 grid_map_python/tests/test_ros.py diff --git a/grid_map_python/src/grid_map/__init__.py b/grid_map_python/src/grid_map/__init__.py index 4e9a35db8..3e23c072d 100644 --- a/grid_map_python/src/grid_map/__init__.py +++ b/grid_map_python/src/grid_map/__init__.py @@ -1 +1,2 @@ from grid_map_python import * +from .convert import from_msg, to_msg diff --git a/grid_map_python/src/grid_map/convert.py b/grid_map_python/src/grid_map/convert.py new file mode 100644 index 000000000..280c8728b --- /dev/null +++ b/grid_map_python/src/grid_map/convert.py @@ -0,0 +1,42 @@ +import numpy as np +from grid_map_python import GridMap + +import rospy +from std_msgs.msg import Float32MultiArray, MultiArrayDimension +from grid_map_msgs.msg import GridMap as GridMapMsg + +def from_msg(msg): + gm = GridMap(msg.layers) + gm.setFrameId(msg.info.header.frame_id) + gm.setTimestamp(int(msg.info.header.stamp.to_sec()*1E9)) + 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(gm.getLayers()): + 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))) ) + + return gm + +def to_msg(self): + msg = GridMapMsg() + msg.info.header.stamp = rospy.Time(self.getTimestamp()/1E9) + 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() + msg.data.append(data_array) + + return msg diff --git a/grid_map_python/tests/test_ros.py b/grid_map_python/tests/test_ros.py new file mode 100644 index 000000000..0eb843254 --- /dev/null +++ b/grid_map_python/tests/test_ros.py @@ -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 From 8dca6249eb6adffcec58a7f796ac4bd340a009d4 Mon Sep 17 00:00:00 2001 From: Antoine Lima Date: Fri, 17 Dec 2021 15:30:08 +0100 Subject: [PATCH 3/7] [grid_map_python] Added bindings for all of GridMap --- grid_map_python/py_core.cpp | 140 ++++++++++++++++++++++++++++-------- 1 file changed, 109 insertions(+), 31 deletions(-) diff --git a/grid_map_python/py_core.cpp b/grid_map_python/py_core.cpp index ea76c771c..e106d3320 100644 --- a/grid_map_python/py_core.cpp +++ b/grid_map_python/py_core.cpp @@ -6,39 +6,117 @@ #include namespace py = pybind11; -using grid_map::GridMap; +using grid_map::GridMap, grid_map::Position, grid_map::Index, grid_map::Length, grid_map::Time, grid_map::InterpolationMethods; +constexpr static auto pyref = py::return_value_policy::reference_internal; + void init_core(py::module m) { - py::module& base_m(m); + py::module& core(m); - py::class_>(base_m, "GridMap") - .def(py::init<>()) + 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("setGeometry", py::overload_cast(&GridMap::setGeometry), py::arg("length"), py::arg("resolution"), py::arg("position")) - .def("add", py::overload_cast(&GridMap::add), py::arg("layer"), py::arg("value")) - .def("add", py::overload_cast(&GridMap::add), py::arg("layer"), py::arg("data")) - .def("exists", &GridMap::exists, py::arg("layer")) - .def("get", py::overload_cast(&GridMap::get, py::const_), py::arg("layer")) - .def("get", py::overload_cast(&GridMap::get), py::arg("layer")) - .def("__getitem__", py::overload_cast(&GridMap::operator[], py::const_), py::arg("layer")) - .def("__getitem__", py::overload_cast(&GridMap::operator[]), py::arg("layer")) - .def("erase", &GridMap::erase, py::arg("layer")); - .def("getLayers", &GridMap::getLayers) - .def("hasSameLayers", &GridMap::hasSameLayers, py::arg("other")) - .def("atPosition", py::overload_cast(&GridMap::atPosition), py::arg("layer"), py::arg("position")) - .def("atPosition", py::overload_cast(&GridMap::atPosition, py::const_), py::arg("layer"), py::arg("position"), py::arg("interpolationMethod")) - .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("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(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() + ); + }, + [](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