CCTronics: when Navio+ meets ROS and QT5

Introduction:

In robotics and industrial application, data sharing and code reuse are important keys of success.

Many efforts have been made by academic researchers to improve Robotic Operating System (ROS) functionalities to extend this middleware beyond its data sharing capability. The result can be appreciated by all the people who have the patience to compile a version and use one or more of the thousands available modules… Within this hell of available hardware/software platforms, I usually find myself comfortable with classic x86/arm platforms and modern unix-like operating system due to robustness and personal experience. Then the practical, but not binding, solution to use Raspberry as low cost platform to develop mobile robotics applications. Here you can appreciate the last version of the Raspberry Pi 2 B which includes a fantastic quad-core 🙂

 

Raspberry Pi 2
Raspberry Pi 2

Another important component of a mobile robotic application is the sensor suite. Fortunately, Emlid, an emergent new company, is now providing a fantastic Raspberry shield named Navio+. It is a ready-to-go solution for mobile robotics platform since it includes GPS, a complete inertial sensor set (accelerometers and gyroscopes), a barometer and a magnetometer to support aerial navigation and perform AHRS. In addition, to remove the need of any additional extra board, Navio+ includes also a PWM generation for servo motors, PPM acquisition, A/D converter, I2C, serial line etc… To be honest, after a unique experience with Arduino and more advanced (low-level) boards, such as FlyMaple, having a full Debian based RT operating system and all the sensor you may need in a single credit card size solution is priceless 🙂

Navio+
Navio+

 

Since Emlid released the basic implementation of Navio+ drivers and a fantastic kernel image with RT preemption, I started to implement some C++ functions and some bash scripting to release a ROS oriented image which can provide a basic and important tool to improve development of control and estimation algorithm over the Navio+/ROS combo. A personal flavor was included by adding QT5 library which I find extremely fascinating an effective.

SD CCTronics Image:

The resulting SD image (8Gb), can be downloaded here.

C++ CCTronics Firmware:

The CCTronics firmware includes a binary version of the ccnavio source code with the GPS, PWM, the IMU and the Barometer ready to be used via ROS infrastructure. Because of the flexibility of ROS, the solutions are endless, however, to set a simple interface, I implemented an abstraction layer which includes an automatic XML-based module configuration. For example, the mGPS module includes the following ccsettings.xml:

<xml>
     <module id="mGPS">
          <periodic>true</periodic>
          <frequency>5.0</frequency>
          <master>http://navio:11311</master>
          <hostname>navio</hostname>
     </module>
</xml>

This XML configuration modifies the mGPS module instance of the mGPS class by setting a frequency of 5Hz, a ROS master node with hostname navio and a module node with hostname navio. Changing master and hostname can be useful to set a pre-define modules topology.

Every module is defined by means of header/source file combo. For example, the mGPS.h header file defines the basic GPSPacket to be delivered and the Navio+ drivers object to interface the hardware

#ifndef MGPS_H
#define MGPS_H

/*------------------------------------------------------------------------------------------------------*/
/**
* \file: mgps.h
* \brief GPS Module
*
* \author: Vincenzo Calabrò <vincenzo@calabrothers.com>
*
* \ingroup CCNAVIODEMO
*/

#include <CCLibROS.h>
#include <cc_msgs/GPSRaw.h>

#include <Navio/Ublox.h>

/*------------------------------------------------------------------------------------------------------*/
/** \addtogroup CCNAVIODEMO
* @{
*/

using namespace CC::ROS;

namespace CC {
namespace MODULES {

class mGPS : public QROSModule
{
private:
     typedef cc_msgs::GPSRaw GPSPacket;
     static const qreal m_rDefaultFreq = 5.0;
protected:
     Ublox m_Device;
     qbool m_DeviceOk;
     GPSPacket m_Packet;
     Publisher m_Publiher;

public:
     mGPS();
     mGPS(QString sGPSID);

     qbool isReady() const;

protected:
     void runningCode();
};
}
}

/** @}*/

#endif // MGPS_H

Finally, the C code will perform subscription or publishing of relevant data via ROS. For example the mGPS module will publish into the topic Navio/Gps:

#include "mgps.h"
#include <CCLibMath.h>

using namespace CC::MATH;
using namespace CC::ROS;
using namespace CC::MODULES;

mGPS::mGPS() : QROSModule("mGPS"), m_DeviceOk(false)
{
     m_Publiher = advertise<GPSPacket>("Navio/Gps");
     m_DeviceOk = m_Device.testConnection();
     qDebug() << "mGPS::mGPS() Module Initialized(" << m_DeviceOk << ")";

     if (hasConf() == false) {
          setFrequency(m_rDefaultFreq);
     }
}

qbool mGPS::isReady() const
{
     return m_DeviceOk;
}

void mGPS::runningCode()
{
     if (m_DeviceOk == false) {
          qDebug() << "mGPS::runningCode() ERROR: Module is not properly initialized";
          this->stop();
     }

     std::vector<double> pos_data;

     if (m_Device.decodeSingleMessage(Ublox::NAV_POSLLH, pos_data) == 1)
     {
          m_Packet.latitude_deg = pos_data[2]/10000000.0;
          m_Packet.longitude_deg = pos_data[1]/10000000.0;
          m_Packet.altitude_m = pos_data[3]/1000.0;
          m_Packet.gps_time_s.fromSec(pos_data[0]/1000.0);

          m_Packet.header.stamp = getTime();
          m_Packet.header.seq++;
     }

     if (m_Device.decodeSingleMessage(Ublox::NAV_STATUS, pos_data) == 1)
     {
          qbool bGpsFix = (((int)pos_data[1] & 0x01) > 0);
          qbool bPublish = false;

          switch((int)pos_data[0]){
               case 0x00: /* no fix */
               case 0x01: /* dead reckoning only */
               case 0x05: /* Time only fix */;
               default: /* Reserved value. Current state unknown */
                              break;

               case 0x02: /* 2D-fix */
               case 0x03: /* 3D-fix */
               case 0x04: /* GPS + dead reckoning combined */
                              bPublish = true;
                              break;
          }

          if ((bGpsFix == true) && (bPublish == true)) {
               m_Publiher.publish(m_Packet);
          }
     }
}

Monitor and Access the CCTronics:

The following setup is referring to Ubuntu 14.04 LTS release. If you have a different distribution… well, it’s your problem 😛

Once that you downloaded the image, you should setup the network as follows:

1) Add a wifi network to connect to the Ad-Hoc network CCTRONICS:

Wifi Settings To Access CCTronics - Ad-Hoc Mode
Wifi Settings To Access CCTronics – Ad-Hoc Mode
Wifi Settings to Access CCTronics - IP Address
Wifi Settings to Access CCTronics – IP Address

2) Add a ethernet connection to share your internet connection:

Ethernet Connection to Share internet connection to the Raspberry
Ethernet Connection to Share internet connection to the Raspberry

3) Set your /etc/hosts

192.168.1.1 navio_wifi navio
10.42.0.69  navio_eth

4) Access and Manage the CCTronics!

If you want to update the CCTronics source code (git) or you want to update the Raspbian distribution, add new library or make a mess you can easily connect via ethernet. Just connect the Raspberry to your computer and log into the CCTronics via ssh:

 


ssh -lpi navio_eth 

the password is the default “raspberry

If you want to access the raspberry via wifi:


ssh -lpi navio_wifi 

Wifi can be useful to trace ROS data using CCTronics as master node and a remotely connected instance of rqt as client…

4) Building the CCTronics Firmware

Enter into:


> cd ~/Development/build

Update the custom ROS packets definition and the Makefiles (qmake):


> ./updateROSMessages.sh
> ./updateMakefile.sh

Build the system…


> make

4) RUN CCTronics over ROS

Activate the ROS System:


> roscore &

Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://navio:42180/
ros_comm version 1.11.10

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.10

NODES

auto-starting new master
process[master]: started with pid [3834]
ROS_MASTER_URI=http://navio:11311/

setting /run_id to ddc6874e-f5b3-11e4-8bdc-525400123456
process[rosout-1]: started with pid [3847]
started core service [/rosout]
Setting the GPS module to connect to the ROS system:

> cd ~/Development/build/modules/mGPS
> cp ~/Development/ccnavio/modules/mGPS/ccsettings.xml .
> ./mGPS

XML::Parser::Parser Opening file "ccsettings.xml" ...
XML::Parser::Parser Parsing XML content...
XML::Parser::Parser Getting XML root
XML::Parser::Parser Setting Valid XML
ROS::QROSModule::QROSModule XML Config Enable ( true )
ROS::QROSModule::QROSModule XML File Content:
"|xml"
"|-module"
"|--periodic"
"|--frequency"
"|--master"
"|--hostname"
ROS::QROSModule::QROSModule Module "mGPS" has XML Configuration!
ROS::QROSModule::QROSModule Creating new module named "mGPS"
ROS::QROSModule::QROSModule Overriding master URL to "http://navio:11311"
ROS::QROSModule::QROSModule Overriding hostname to "navio" 

....

Enjoy the GPS!! 🙂

Conclusion:

I hope you enjoyed this journey into Raspberry Images, C++ code and bash programming 😉 Feel free to contact me for any clarification and suggestion.

Long life to CCTronics: the unique combo of ROS, QT5 and Navio+ with a flavor of module oriented design 😉

 

V.

 

Leave a Reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.