Klamp't Tutorial: Connect ROS To Klamp't

In this tutorial, we will learn how to connect the Robot Operating System (ROS) to Klamp't. It covers logging, C++ bindings, and subscribing to point clouds in Python.

Note that this tutorial assumes you are familiar with ROS. In addition, you will need to build Klamp't after installing ROS.

Difficulty: intermediate

Time: 10-30 minutes

Building the ROS Package

First, we need to build the ROS package so that it can communicate with Klamp't. Before proceeding, make sure to run

source /opt/ros/[ROS_DIST]/setup.bash

where [ROS_DIST] is the name of your ROS distribution (i.e. indigo, jade, etc.). Now navigate to the folder Klampt/Python/control/klampt_catkin, which contains a Catkin workspace. Run

catkin_make
catkin_make install

Your workspace is now connected.

Connecting a ROS Controller

Follow this tutorial to learn about connecting a custom controller to Klamp't.

Logging from Klamp't to ROS

To broadcast logging from Klamp't to ROS, run

roscore

This will start the roslaunch server and print the logging directory. Now navigate to the Klampt home directory and run

./SimTest [ROBOT_FILE]

where [ROBOT_FILE] is your desired robot file. In SimTest, click LoggingROS Broadcast. Klamp't will now log to the directory provided by roscore.

In C++, we can publish as well as subscribe to ROS topics containing various information (including poses, trajectories, and point clouds).

As an example, let's create a simulation and publish/subscribe to a joint state topic. First, make two files: "main.cpp" and "CMakeLists.txt". In main.cpp, copy the following lines

#include <Interface/SimulationGUI.h>
#include <IO/ROS.h>

int main(int argc,const char** argv) {

    // Create world
    RobotWorld world;
    SimGUIBackend backend(&world);
    WorldSimulation& sim = backend.sim;

    // Load world file
    if (!backend.LoadAndInitSim(argc,argv)) {
        cerr << "Error loading simulation from command line" << endl;
        return 1;
    }

    // Initialize ROS
    // This must be called before all other ROS calls
    if (!ROSInitialized()) {
        if (!ROSInit()) {
            cerr << "Error initializing ROS" << endl;
        }
        else {
            cout << "ROS initialized!" << endl;
        }
    }

    // Create subscriber and topic
    Robot robot_sub = *world.robots[0];     // dummy robot listener
    const char* topic = "myROSTopic";       // desired topic

    // Subscribe to joint state
    if (!ROSSubscribeJointState(robot_sub,topic)) {
        cerr << "Error subscribing joint state" << endl;
    }

    // Start simulation
    double dt = 0.1;
    while (sim.time < 3) {
        // Advance and update simulation
        sim.Advance(dt);
        sim.UpdateModel();
        cout << sim.time << ":\t";

        // Publish robot's joint state
        if (!ROSPublishJointState(*world.robots[0],topic)) {
          cerr << "Error publishing joint state" << endl;
        }

        // Check for updates
        if (ROSSubscribeUpdate()) {
            // robot_sub has now been updated from the topic
            cout << "Updated!" << endl;
        }
        else {
            // robot_sub already has the latest information
            cout << "No updates" << endl;
        }
    }

    // Print number of subscribed and published topics
    cout << "Subscribed to "<< ROSNumSubscribedTopics() << " topics" << endl;
    cout << "Published "<< ROSNumPublishedTopics() << " topics" << endl;

    // Shutdown ROS
    // Must call after all other ROS calls to cleanly shutdown ROS
    if (!ROSShutdown()) {
        cerr << "Error shutting down ROS" << endl;
    }

    return 0;
}

In CMakeLists.txt, copy the following lines

CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
SET(KLAMPT_ROOT [klampt_path_here])
SET(CMAKE_MODULE_PATH "${KLAMPT_ROOT}/CMakeModules")
FIND_PACKAGE(Klampt REQUIRED)
ADD_DEFINITIONS(${KLAMPT_DEFINITIONS})
INCLUDE_DIRECTORIES(${KLAMPT_INCLUDE_DIRS})
ADD_EXECUTABLE(MySim main.cpp)
TARGET_LINK_LIBRARIES(MySim ${KLAMPT_LIBRARIES})

where [klampt_path_here] is the directory of your Klamp't installation. Now run

make MySim

This should create an executable named "MySim." Before executing this program, make sure that you have roscore running in the background. Then, run

./MySim [ROBOT_FILE]

where [ROBOT_FILE] is your desired robot file. The program will attempt to publish the robot's joint state to the specified topic. In addition, it will subscribe to this topic as well. Notice the methods ROSNumSubscribedTopics() and ROSNumPublishedTopics(), which report the number of subscribed and published topics, respectively. Also, the method ROSSubscribeUpdate() can be used to update all subscribed topics.

Note that ROSInit() should be called before any other ROS call. Likewise, ROSShutdown() should be called after all other ROS calls.

Currently, the Python interface only provides a method to subscribe to point clouds.

Subscribing to Point Clouds

Using Geometry3D, you can subscribe to a ROS topic containing a PointCloud. This is accomplished via Geometry3D's attachToStream method, which takes as arguments the protocol (currently only "ros" protocol is supported) and the name of the ROS topic to subscribe to. For an example, create a new file called "pointCloudFromROS.py" and copy the following lines:

from klampt import PointCloud, Geometry3D, Appearance

#Create point cloud subscriber
topic = "myROSTopic"  #ROS topic containing actual point cloud
pc = PointCloud()     #point cloud listener
g = Geometry3D(pc)    #3d geometry from point cloud

#Subscribe to topic
if g.attachToStream("ros",topic):       #subscribe to myROSTopic
    print "Subscribed!"
else:
    print "Could not subscribe to", topic
Appearance().refresh(True)              #update appearance

#Unsubscribe from topic
#g.detachFromStream("ros",topic)        #unsubscribe from myROSTopic

Now run the script via

python pointCloudFromROS.py

Congratulations, you have subscribed to your first point cloud!

Note that you need to call Appearance().refresh(True) to get the appearance to update. Furthermore, detachFromStream must be called before deleting the geometry.