Feature : Adding Python bindings

.gitignore

@@ -38,3 +38,6 @@
 
 grid_map_pcl/data/*
 !grid_map_pcl/data/input_cloud.pcd
+
+# Python
+*.pyc

grid_map/package.xml

@@ -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>

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}
+)

grid_map_python/package.xml

@@ -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>

grid_map_python/py_core.cpp

@@ -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;
+      }
+    ));
+}

grid_map_python/py_module.cpp

@@ -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);
+}

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)

grid_map_python/src/grid_map/__init__.py

@@ -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)

grid_map_python/src/grid_map/convert.py

@@ -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

grid_map_python/tests/test_core.py

@@ -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)

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