Connecting ROS to the Wolfram Language

Or Controlling a Parrot ArDrone 2.0 from Mathematica

Foreword

During this summer, between the end of my high school education and the beginning of my undergraduate studies, I had the opportunity to participate in the Wolfram Mentorship program where I worked on a project called "Connecting ROS to the Wolfram Language".

ROS, which is an abbreviation for "Robot Operating System", is a software available for Linux which acts as a 'conductor' between different electronic components of one or more robots (such as the engine, the cameras...) such that the components and the controller (a computer for example) can communicate together. A good introduction to ROS can be found here and in depth tutorials are available there.

The aim of this project was to implement a connection to ROS from the Wolfram Language, i.e. to be able to collect and interpret data from the robots controlled by ROS in Mathematica as well as to control ROS and the robots from Mathematica and to test the results using a Parrot ArDrone 2.0.

This post, which marks the end of the project, describes the different steps I went through to complete this project. It assumes prior knowledge of ROS (and the ardrone autonomy package) as well as basic Linux and Mathematica. It is divided into two parts:

1. 'collecting and interpreting data' in Mathematica
2. 'controlling ROS and supported Robots' from Mathematica,

These are independent and throughout the first part code developed is illustrated using turtlesim (a simulator of a turtle - used in ROS tutorials) and throughout the second part, the examples refer to the Parrot ArDrone 2.0.

PART 1: COLLECTING & INTERPRETING DATA

Extracting and Understanding Data from ROS

After installing ROS and getting familiar with the environment, the first step is to figure out how to extract data from it, or to be more precise, how to log all the messages published over one or more topic in a file which can be retrieved by the user.

Although, there is a tool called a rosbag which records the selected ROS topics and store them in a .bag file, it turns out that I only managed to open these files in ROS so a different procedure was used. It consists of echoing the messages published on selected topics (using the command rostopic echo) to a file of the chosen format (since messages are just simple text files the format used will be .txt). This can simply be done by executing the following command in the Linux terminal before sending messages over the topic(s) (once ROS is installed and started):

rostopic echo [topic] > [file].txt 

For example, if we were going to extract the velocity commands sent to the turtle to a file called rosdata.txt, the command would be:

rostopic echo /turtle/cmd_vel > rosdata.txt 

And opening the text file (once the ROS session is ended) would return a sequence of messages looking like the one below (the values of x,y,z might change):

linear
x: 1.0
y: 0.0
z: 0.0
angular
x: 0.0
y: 0.0
z: 1.0
_ _ _ 

Now, In order to 'use' the data, it is necessary to understand the structure of the text file. It can be represented by the 'fomula' below :

Header_1              (some indication about variables displayed beneath)
    Variable_1: value1     (a variable name followed by its value)
    ....             
    Varable_n: value n     (a variable name followed by its value)    
....
Header_n              (some indication about variables displayed beneath)
    Variable_1: value1     (a variable name followed by its value)
    ....               
    Varable_n: value n     (a variable name followed by its value)     
_ _ _                 (end of message)

So, going back to the turtlesim example:

• linear x represents the linear velocity of the turtle (in m/s),
• angular z represents the angular velocity of the turtle in rad/s (its bearing being the sum of the angular displacements).
• The other variables are always 0 as the turtle moves in a 2D plane.

Note: the meaning of a variable in a message might not always be obvious at first sight (due to ambiguous variable name such as x,y,z for instance) however, by observing the values of the variables against the action of the robots (the trajectory it takes in our case), this meaning can be quite easily retrieved.

Importing the Data into Mathematica

Now that is possible to retrieve and understand messages published over a ROS topic the next step is to import those messages in Mathematica. In order to do this, a function, taking the path of the text file containing the messages as an argument and returning the messages in a list created; as displayed below:

MessageImport[filepath_]:=
Module[
{stream=OpenRead[filepath],character,rules= {}},
character=Read[stream,String];
Reap[
While[character=!=EndOfFile,
If[character=== "---",
Sow[Flatten[rules]];rules= {},
AppendTo[rules,
StringCases[character,
{var:(WordCharacter..)~~":"~~Whitespace~~value:NumberString:>(var->ToExpression[value]),
var:(WordCharacter..)~~":"~~Whitespace:>("Head"->var)}]]];
character=Read[stream,String]];
Close[stream];][[2,1]]];

And, it is then possible to represent this in a dataset like so:

Dataset[
Association[#]&/@
Partition[
Flatten[
MessageImport[filepath_]],4]]

Hence, returning to the turtlesim example, if rosdata.txt is the file where the velocity command messages are published, the code below:

Dataset[
 Association[#] & /@
  Partition[
   Flatten[
    MessageImport["rosdata.txt"]], 4]]

would return something of the form:

Interpreting the Data

Having imported the Data into Mathematica it can now be interpreted using the Wolfram Language. For example, in the case of a topic where the messages published contain linear and angular velocities like the one described above, the path of the robot can be retrieved using the following code (using the function AnglePath):

Graphics[Line[AnglePath[Values[MessageImport[filepath_][[All,{2,8}]]]]]]

Therefore,considering the turtlesim example, if the path of the turtle was:

The following code:

Graphics[Line[AnglePath[Values[MessageImport["rosdata.txt"][[All,{2,8}]]]]]]

would return:

which is the same as the actual path taken by the turtle.

PART 2: CONTROLING ROS AND SUPPORTED ROBOTS

Sending Commands to ROS from Mathematica

Yet another thing to consider is how to send ROS commands from Mathematica in order to be able to directly operate ROS and record ROS data from a Mathematica notebook.

One possible way to do so would be to use the Run command, however, it is necessary to source ROS commands before executing them and I did not manage to do so with the Run command.

So, another method to execute ROS commands from Mathematica is to create an empty shell script (using the chmod command in the Linux terminal) and then one can write the desired commands in the script and execute it from Mathematica, thanks to the function below (which takes the desired commands as an argument) :

RosCommand[Command_]:=
WaitNext[
ParallelSubmit/@
Unevaluated[{
{WriteString[
OpenWrite["script"],
"bash -c \"source /opt/ros/indigo/setup.bash && source ~/catkin_ws/devel/setup.bash && ",Command,"\""],Close["script"],
SetDirectory["/"],
SetEnvironment["LD_LIBRARY_PATH"->""],
RunProcess["script","StandardOutput"],
SetEnvironment["LD_LIBRARY_PATH"->FileNameJoin[{$InstallationDirectory,"SystemFiles","Libraries",$SystemID}]],
SetDirectory["your working directory"]},
Pause[5]}]];

Note: a second kernel is used in that code in order to kill the process running in the first one as some commands executed by the ROS command function would evaluate indefinitely.

Hence, to start the ROS master, the command would be :

RosCommand[rosstart]

Publishing messages from Mathematica

Finally, another thing to implement in order to be able to perform most of ROS's basic features from Mathematica is to be able to publish messages on topics. In some cases this can be done by using the ROS command $rostopic publish through the RosCommand function described in the previous section.

This method works fine however there is quite a considerable latency between the moment command is evaluated and the moment the command is actually published (about 1-2 seconds). Hence, this way only works for topics where messages have to be published at a rate lower than 0.5 Hz, for example the take off and land commands for the drone, as shown below:

Takeoff:
RosCommand["rostopic pub ardrone/takeoff std_msgs/Empty"]

Landing:
RosCommand["rostopic pub ardrone/land std_msgs/Empty"]

For topics where the rate for which the messages have to be published at a rate greater than 0.5 Hz, a publisher ROS node (a 'Mathematica' node), intaking the arguments from Mathematica should be created. This can be written in C++ and the arguments from Mathematica can be transferred by simple file I/O code; i.e. this process would have the following structure:

• Mathematica Notebook:
  • Some functions would assign a value to the desired message variables (controller part)
  • Some code would save the variable values to a text file at a chosen rate
• C++ code:
  • Some code declaring the Publisher Node
  • Some code importing the variable values from the text file
  • Some code publishing these values at the chosen rate

Note: the instructions to build the node are available here.

Hence, to send velocity commands to the ArDrone from Mathematica, the Mathematica Notebook could be :

CONTROL PART:

Turn right:
{p = {1, 0, 0}; Pause[1]; p = {0, 0, 0}}

Move forwards:
{p = {0, 0.2, 0}, Pause[1], p = {0, 0, 0}}

Move Up:
{p = {0, 0, 0.1}, Pause[1], p = {0, 0, 0}}

Move Down:
{p = {0, 0, -0.1}, Pause[1], p = {0, 0, 0}}


VARIABLE OUTPUT PART: 

Dynamic[
Refresh[
StringJoin["b  ",
Flatten[{
If[Round[p[[1]],0.1]>=0,
PadRight[Characters[ToString[Round[p[[1]],0.1]]],3,"0"],
PadRight[Characters[ToString[Round[p[[1]],0.1]]],4,"0"]],
"  ",
If[Round[p[[2]],0.1]>=0,
PadRight[Characters[ToString[Round[p[[2]],0.1]]],3,"0"],
PadRight[Characters[ToString[Round[p[[2]],0.1]]],4,"0"]],"  ",
If[Round[p[[3]],0.1]>=0,
PadRight[Characters[ToString[Round[p[[3]],0.1]]],3,"0"],
PadRight[Characters[ToString[Round[p[[3]],0.1]]],4,"0"]]
}],"  e"]>>"file.txt"
,UpdateInterval->1]]

where p={angluarz, linearx, linear_z} and:

• angular_z represents the angular velocity of the drone in the xy plane (in rad/s) (its bearing being the sum of the angular displacements),
• linear_x represents the linear velocity of the drone in the xy plane (in m/s),
• linear_z represents the velocity of the drone along the z axis (in m/s).

Moreover, the C++ code could look like:

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"

#include <boost/lexical_cast.hpp>
#include <iostream>
#include <string>
#include <fstream>
#include <stdlib.h>

using namespace std;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "mathematica");
    ros::NodeHandle n;

    ros::Publisher chatter_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);

    ros::Rate loop_rate(1.0);

    int count = 0;

    while (ros::ok())
    {
       //start
       ifstream infile( "file.txt" );
       double nb; 
       double nb2;
       double nb3;
       string b;
       string e;
       (infile >> b >> nb >> nb2 >> nb3 >> e);
       //end

       geometry_msgs::Twist vel_msg;

      vel_msg.linear.x = nb2;
       vel_msg.linear.y =0.0;
        vel_msg.linear.z = nb3;

        vel_msg.angular.x =0.0;
        vel_msg.angular.y =0.0;
        vel_msg.angular.z = nb;

           ROS_INFO("[Random Walk] linear.x = %.2f, angular.z=%.2f\n", vel_msg.linear.x, vel_msg.angular.z);

           chatter_pub.publish(vel_msg);

           ros::spinOnce();

           loop_rate.sleep();
           ++count;
    }

    return 0;
}

Conclusion

Throughout the Mentorship program, I developed functions enabling me to fully interact with ROS and the robot(s) it is controlling from a Mathematica notebook. Owning a Parrot ArDrone myself, which is a device supported by ROS, I tested this in real conditions and the result can be seen below:

This is only a GIF, the full video is available here.

All, in all, I have thoroughly enjoyed working on this project and would love to hear your feedback. I see it as a starting point towards using the many abilities of the Wolfram Language in order to improve the capabilities of ROS supported robots: I can already imagine myself using Mathematica to create an autopilot for my drone !

Finally, I would also like to thanks Todd Rowland, Alison Kimball and Wolfram Research for the help and support provided throughout the mentorship program.

Loris Gliner