Welcome to the Matrix, NAO (NAO tutorial part 2)

NAO Linux C++ development cheat sheet / tutorial, Part 2

Following Part 1, we have a workspace in ~/nao/workspace, at least one toolchain set up for one of the two possible processors of NAO (qitoolchain list), and the C++ SDK folder extracted in ~/nao/devtools.

We will now make our own NAO C++ project, configure it, build it, and run it. We'll maybe have some fast review of some C++ at the same time.

Before doing that, let's have a look at what's in a qibuild project.
Change directory to your workspace: cd ~/nao/workspace

Anatomy of the default qiBuild project

qibuild create project

With that command, qibuild creates a project directory containing:

CMakeLists.txt
main.cpp
qiproject.xml
test.cpp

Let's have a look at each file.

cd project

qiBuild project file: qiproject.xml

This file contains the name of the project, and can be used to list dependencies on other qiBuild projects. It's of little interest.

CMake file: CMakeLists.txt

This file is a script of the build process. As the project is both meta-built (i.e. configured) and then built with the command qibuild, you must wonder why a CMake file is used here rather than, say, another XML file like qiproject.xml. The reason is that CMake and qibuild cooperate, and the command qibuild configure in fact calls cmake. qiBuild is in part a CMake code generator and a (fairly complex) CMake module, which is why the build file remains a CMake script (CMakeLists.txt), but has to start with the following statements:

cmake_minimum_required(VERSION 2.8)
project(project)
find_package(qibuild)

The first statement indicates that CMake version 2.8 or higher is required. The qiBuild library is written for this version of Cmake. The second (project(...)) statement informs CMake of the project name.

Finally, the find_package part tells CMake to load the qibuild module. 

As this is a CMake file, we should normally be able to directly process it with the meta-build tool CMake, like so: cmake CMakeLists.txt. This should roughly perform like qibuild configure. But it doesn't work, we get the following message: Could not find module Findqibuild.cmake or a configuration file for package qibuild. That's because we also need to inform CMake of the path to the qibuild module in our ~/nao/devtools folder:  cmake -D qibuild_DIR=~/nao/devtools/qibuild/cmake/qibuild/ CMakeLists.txt. This now should work, and the screen output should be similar to that of  qibuild configure. The disk output is different, though, in that using the qibuild configure command result in all build files in one neat folder named after your toolchain, while using directly cmake without further options results in a mess of build files directly in the project folder. We could need to further refine our command to get something clean like with qiBuild. It's not necessary though, as that's what qibuild configure does for you: calling cmake with the right options, and take care of the housekeeping.

The main.cpp and test.cpp file

Those two files seem pretty simple, as the default main.cpp is a "hello, world", and the provided test.cpp is the "hello world" of C++ unit testing. However, it's good to have a quick look at how these two files are handled by our build tools.

Let's look at the rest of the CMakeLists.txt file:

# Create a executable named "project" with the source file: main.cpp
qi_create_bin(project "main.cpp")

# Add a simple test:
enable_testing()
qi_create_test(test_project "test.cpp")

The qi_create_bin(project main.cpp) command instructs CMake that the file main.cpp should be used to create an executable named "project".  

enable_testing() is necessary to let you use any testing-related command later.

The qi_create_test(test_project "test.cpp") is such a command. It tells that the test.cpp file should be used to make a test binary "test_project."

A test is successful if it returns 0. You run all tests with:

qibuild test

The test results are printed and also stored in the build-tests/results folder.

You can call qiBuild commands from anywhere in the workspace by specifying the project name. For instance, calling qibuild configure helloworld from the ~/nao/workspace/project directory works as expected: it configures the helloworld project in ~/nao/workspace/helloworld. Calling  qibuild configure without specifying a project name will configure the project that corresponds to the working directory.

Now, let's make a real project that runs against NAO and does something useful. 

A real project that actually does something

We will make a C++ module that uses the inertial unit to plot the displacement on the center of gravity of the robot while he stands.

qibuild create inertial_monitor

Let's make a first version that runs on the computer and prints the displacement every second.

The doc indicates us that the body inclination angles are available in the naoqi sensors database (ALMemory) under the keys "Device/SubDeviceList/InertialSensor/Angle[X|Y]/Sensor/Value". Raw gyro and accelerometer data are also available, but for now, let's rely on the angle computed by naoqi.

There are several ways to access data in ALMemory.
ALMemoryProxy::getData(key) is the safest, but slowest. Returns an ALValue.
ALMemoryProxy::getDataPtr(key) returns a pointer to the data, so it can be accessed later too. Return a void*, to be cast to the appropriate 32 bit type. As it's a pointer, it only makes sense for local modules.
ALMemoryProxy::getDataOnChange(key) will block until the value changes, then returns it as ALValue.

That means that somewhere in our main.cpp, we need to get a proxy to ALMemory:

1. //... some code to get a broker  
2. boost::shared_ptr<ALMemoryProxy> memoryProxy;  
3. try {  
4. memoryProxy = boost::shared_ptr<ALMemoryProxy>(new ALMemoryProxy(broker));  
5. } catch (const ALError& e) {  
6.   std::cerr << "Could not create proxy: " << e.what() << std::endl;  
7.   return 3;  
8. }  

Let's say that we define this to make the code more readable:

1. const std::string intertialSensorXKey("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value"),  
2.                   intertialSensorYKey("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value");  

Then, our data gathering code could look like this:

1. float *intertialSensorX = static_cast<float*>(memoryProxy->getDataPtr(intertialSensorXKey));  
2. float *intertialSensorY = static_cast<float*>(memoryProxy->getDataPtr(intertialSensorYKey));  
3. while (true) {  
4.   std::cout << "X: " << *intertialSensorX << ", Y: " << *intertialSensorY << std::endl;  
5.   boost::this_thread::sleep(boost::posix_time::seconds(1));  
6. }  

Or it could also look like that:

1. while (true) {  
2.   std::cout << "X: " << memoryProxy->getData(intertialSensorXKey) << ", Y: " << memoryProxy->getData(intertialSensorXKey) << std::endl;  
3.   boost::this_thread::sleep(boost::posix_time::seconds(1));  
4. } 

That should be enough to get what we want. With that, let's first to make that a program that connects from the computer to the robot, and then make it a module that runs on NAO itself.

Note that in the following code, a lot of code is added compared to above:

• Code to handle the command-line arguments, that I've chosen to put in a separate function called parseOpt, for clarity. The IP of the robot and the port where to contact the robot's broker (naoqi) must be given wight he options --pip and --pport, respectively, as the robot runs on a different computer. If those options are not given, then the default is --pip nao.local --pport 9559. It might work, in particular if the robot's name was never changed from "nao".
• Code to make a local broker and connect it to NAO's remote one, all grouped in the function makeLocalBroker, to make it clearer for you.

main.cpp

1. #include <iostream> // output, etc  
2. #include <boost/program_options.hpp> // a clean way to process command-line arguments  
3. #include <boost/shared_ptr.hpp> // Good practice to use C++ facilities in C++.  
4. #include <boost/thread/thread.hpp> // To use Boost's sleep. There are others, but Boost is a good portable library.  
5. #include <alcommon/albroker.h> // To handle Naoqi brokers (the local one and the one on NAO)  
6. #include <alcommon/albrokermanager.h> // same  
7. #include <alerror/alerror.h> // To catch and process Aldebaran's exceptions  
8. #include <alproxies/almemoryproxy.h> // To access ALMemory.  
9.   
10. void parseOpt(std::string* naoBrokerIP, int* naoBrokerPort, int argc, char* argv[]) {  
11.   namespace po = boost::program_options; // shorter to write po than boost::program_options  
12.   po::options_description desc("Allowed options");  
13.   desc.add_options()  
14.       ("pip", po::value<std::string>(naoBrokerIP)->default_value("nao.local"), "IP of the parent broker. Default: nao.local")  
15.       ("pport", po::value<int>(naoBrokerPort)->default_value(9559), "Port of the parent broker. Default: 9559");  
16.   po::variables_map vm; // Map containing all the options with their values  
17.   // program option library throws all kind of errors, we just catch them all, print usage and exit  
18.   try {  
19.     po::store(po::parse_command_line(argc, argv, desc), vm);  
20.     po::notify(vm);  
21.   } catch(po::error &e) {  
22.     std::cerr << e.what() << std::endl;  
23.     std::cout << desc << std::endl;  
24.     exit(1);  
25.   }  
26. }  
27.   
28. boost::shared_ptr<AL::ALBroker> makeLocalBroker(const std::string parentBrokerIP, int parentBrokerPort) {  
29.   // Name, IP and port of our local broker that talks to NAO's broker:  
30.   const std::string brokerName = "localbroker";  
31.   int brokerPort = 54000;   // FIXME: would be a good idea to look for a free port first  
32.   const std::string brokerIp   = "0.0.0.0";  // listen to anything  
33.   try {  
34.     boost::shared_ptr<AL::ALBroker> broker = AL::ALBroker::createBroker(  
35.         brokerName,  
36.         brokerIp,  
37.         brokerPort,  
38.         parentBrokerIP,  
39.         parentBrokerPort,  
40.         0    // you can pass various options for the broker creation, but default is fine  
41.       );  
42.     // ALBrokerManager is a singleton class (only one instance).  
43.     AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());  
44.     AL::ALBrokerManager::getInstance()->addBroker(broker);  
45.     return broker;  
46.   } catch(const AL::ALError& /* e */) {  
47.     std::cerr << "Faild to connect broker to: " << parentBrokerIP << ":" << parentBrokerPort  
48.               << std::endl;  
49.     AL::ALBrokerManager::getInstance()->killAllBroker();  
50.     AL::ALBrokerManager::kill();  
51.     exit(2);  
52.   }  
53. }  
54.   
55. int main(int argc, char* argv[]) {  
56.   boost::shared_ptr<AL::ALBroker> broker;  
57.   boost::shared_ptr<AL::ALMemoryProxy> memoryProxy;  
58.   
59.   std::string parentBrokerIP;  
60.   int parentBrokerPort;  
61.   setlocale(LC_NUMERIC, "C"); // Need this to for SOAP serialization of floats to work  
62.   // IP and port of the broker currently running on NAO:  
63.   parseOpt(&parentBrokerIP, &parentBrokerPort, argc, argv);  
64.   // Our own broker, connected to NAO's:  
65.   broker = makeLocalBroker(parentBrokerIP, parentBrokerPort);  
66.   
67.   try {  
68.     memoryProxy = boost::shared_ptr<AL::ALMemoryProxy>(new AL::ALMemoryProxy(broker));  
69.   } catch (const AL::ALError& e) {  
70.     std::cerr << "Could not create proxy: " << e.what() << std::endl;  
71.     return 3;  
72.   }  
73.   
74.   const std::string intertialSensorXKey("Device/SubDeviceList/InertialSensor/AngleX/Sensor/Value"),  
75.       intertialSensorYKey("Device/SubDeviceList/InertialSensor/AngleY/Sensor/Value");  
76.   
77.   while (true) {  
78.     std::cout << "X: " << memoryProxy->getData(intertialSensorXKey) << ", Y: "  
79.               << memoryProxy->getData(intertialSensorYKey) << std::endl;  
80.     boost::this_thread::sleep(boost::posix_time::seconds(1));  
81.   }  
82.   
83.  return 0;  
84. }  

Note that if we tried to use memoryProxy->getDataPtr in the that code instead of memoryProxy->getData, it would compile, but would throw an exception at runtime (as expected) with the pretty explicit error message "ALMemory::getDataPtr Cannot be called remotely".

The CMakeLists file is very simple.

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(inertial_monitor)
find_package(qibuild)

qi_create_bin(inertial_monitor main.cpp)
qi_use_lib(inertial_monitor ALCOMMON BOOST BOOST_PROGRAM_OPTIONS)

Configure and compile the project for your computer:

(To make the program that runs on the computer, we need a toolchain for compilation for the computer's architecture, not for NAO. This toolchain is located directly in the SDK folder ~/nao/devtools/naoqi-sdk-1.14.2-linux??. As we saw in Part 1, to make this toolchain available to qibuild, you can add it like that (here, giving it the name "local"): qitoolchain create local ~/nao/devtools/naoqi-sdk-1.14.2-linux??/toolchain.xml)

qibuild configure -c local inertial_monitor
qibuild make -c local inertial_monitor

Call the program from your computer, for instance: 

cd ~/nao/workspace/inertial_monitor
build-local/sdk/bin/inertial_monitor --pip 192.168.1.11 --pport 9559

It should display a list (X, Y) of values. Here, I run it with NAO standing, and make NAO lie on its belly:

build-local/sdk/bin/inertial_monitor --pip 192.168.2.3 --pport 9559

[INFO ] Starting ALNetwork
[INFO ] localbroker is listening on 192.168.2.2:54000
X: -0.0218986, Y: 0.99
X: -0.0157626, Y: 1.01435
X: -0.016146, Y: 1.01224
X: -0.00885946, Y: 1.0086
X: -0.0111607, Y: 1.00745
X: 0.0051381, Y: 1.00898
X: -0.0148037, Y: 1.02145
X: -0.0238159, Y: 1.01665
X: -0.0155709, Y: 1.03161
X: 0.122871, Y: 0.231638
X: -0.000231091, Y: 0.125218
X: -0.0209397, Y: 0.110645
X: 0.022587, Y: 0.142667
X: -0.0596727, Y: -0.0828282
X: -0.0107773, Y: 0.0784314
X: -0.112403, Y: -0.0853206
X: -0.0483597, Y: 0.0257009
X: -0.00962669, Y: 0.075939

You can stop it with Ctrl-C.

Note the change in Y value in the middle of that list. In general, we note also that the values are not very stable. Here, I'm sitting on a bed with NAO next to me, so the inertial sensor values vary more, but even on a flat and stable ground, they might vary a bit when NAO is immobile.

In the next part, we will transform that program into a module that runs directly on NAO.