Skip to content
Peter Rudolph edited this page Jan 26, 2015 · 6 revisions

MRPT Python bindings

Note

This is still a work in progress! Many classes are missing, but there are already enough to set up RBPF SLAM. Additionally there are already ROS extensions included, which makes the usage of the bindings in combination with ROS really easy and powerful. New classes will be added step by step.

Installation

from source

For now you have to build the bindings with the mrpt sources. Currently they are integrated into python-bindings branch. The requierements are found quite, to see if your system is ready to build the bindings, have a look at the summary. Missing dependencies should be listed there too.

requirements

The python bindings are built using boost::python. So boost is a neccessary dependency. And of course pythonlibs itself. sudo apt-get install libboost-python1.55-dev libpython2.7-dev

from apt

It is planned to release the bindings as a several ubuntu (apt/deb) package.

Usage

For the usage of the classes itself have a look at the MRPT C++ class documentation. For the already binded classes most of the common functions are included. Some of them have been re-wrapped to have more pythonic function callings and return types, for example: methods which had &out_ variables, now usually return this, or a tuple of those, if there are more than one.

Import bindings

Supported import statements:

import pymrpt
from pymrpt import obs
from pymrpt.obs import *
from pymrpt.obs import CObservation2DRangeScan

Smart Pointers

The mrpt smart pointers are wrapped into << class_name >>Ptr-classes. They are created similar to MRPT C++ types with the static .Create() member function of their containing object class. To get acces from the pointer class to the object use .ctx() member function.

from pymrpt.obs import CObservation2DRangeScan

# create observation
obs = CObservation2DRangeScan()
# create smart pointer
obs_ptr = CObservation2DRangeScan.Create()
# set observation as new content of smart pointer
obs_ptr.ctx(obs)
# get observation object from smart pointer
obs = obs_ptr.ctx()

ROS Extensions

Several CObservation-derived classes and COccupancyGridMap2D class have .to_ROS_DataType_msg() and .from_ROS_DataType_msg() which allow easy conversion from MRPT to ROS datatypes.

# ROS imports
import rospy
from sensor_msgs.msg import LaserScan
# MRPT imports
from pymrpt.obs import CObservation2DRangeScan
from pymrpt.poses import CPose3D

# the sensor pose needed for conversion of CObservation-derived classes
# Note: Usually this is received via a call to ROS tf and a following conversion.
sensor_pose = CPose3D()

def scan_cb(scan):
	global sensor_pose
    # create mrpt observation
    obs = CObservation2DRangeScan()
    # convert ROS LaserScan to MRPT CObservation2DRangeScan
    obs.from_ROS_LaserScan_msg(scan, sensor_pose)
    # do something ...
    print 'converted ROS LaserScan to MRPT CObservation2DRangeScan'

rospy.init_node('test_conversion')
rospy.Subscriber('/scan', LaserScan, scan_cb, queue_size=10)
rospy.spin() 

Full examples

rbpf_slam.py

As the original MRPT C++ example this python script/program accepts a config file and a rawlog file to get an offline rbpf-slam running. Parameters are described by the program itself, use ./rbpf_slam.py -h to show them.

Clone this wiki locally