Hello DroneCode

The Dronecode SDK is a MAVLink Library for the PX4 flight stack, with APIs for C++ The library provides a simple API for managing one or more vehicles, providing programmatic access to vehicle information and telemetry, and control over missions, movement and other operations. SDK Main page

Install SDK

• Release
• Source

Note: First I Install from Release (deb) and try to compile the example takeoff_amd_land - The code look for action_result.h file that wasn't exists

Install SDK from source

git clone https://github.com/Dronecode/DronecodeSDK.git
cd DronecodeSDK
#master branch contain code like the deb
git checkout develop
git submodule update --init --recursive
make default
#Install
sudo make default install 
sudo ldconfig  # update linker cache

Takeoff example

• SDK C++

Project struct

├── bin
├── build
├── CMakeLists.txt
├── docs
├── README.md
└── src
    ├── CMakeLists.txt
    └── takeoff_and_land.cpp

• root Cmake file

cmake_minimum_required(VERSION 2.8.12)

project(xdrone)
set(CMAKE_CXX_STANDARD 11)

include_directories(/usr/local/include/dronecode_sdk)
link_directories(/usr/local/lib)
set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin)

add_subdirectory(src)

• src cmake file

add_executable(takeoff_and_land takeoff_and_land.cpp)
target_link_libraries(takeoff_and_land
    dronecode_sdk
    dronecode_sdk_telemetry
    dronecode_sdk_action
)

Paste code from takeoff_and_land

• Run SITL make posix_sitl_default gazebo_iris from px4 Firmware folder
• Run the code

ROS Service client

• Terminal 1

roscore

• Terminal 2

rosrun turtlesim turtlesim_node

• Terminal 3

rosservice list

/clear
/kill
/reset
/rosout/get_loggers
/spawn
/turtle1/set_pen
/turtle1/teleport_absolute
/turtle1/teleport_relative
/turtlesim/get_loggers

spawn service

Spawn another turtle with spawn service

• rosservice info <service name>

rosservice info /spawn

Node: /turtlesim
URI: rosrpc://user-N56VM:43083
Type: turtlesim/Spawn
Args: x y theta name

• rossrv info <service type>

float32 x
float32 y
float32 theta
string name
---
string name

call / execute

#left down corner: 0,0
rosservice call /spawn "x: 3.0
y: 3.0
theta: 0.0
name: 't1'"

service client

• rosservice info result show the the service implement in /turtlesim package

• most simple service client (without error handler)

#!/usr/bin/env python
import rospy
from turtlesim.srv import Spawn

def main():
    service = rospy.ServiceProxy('/spawn', Spawn)
    service(x=3.0, y=3.0, theta=0.0, name="t1")

if __name__ == "__main__":
    main()

Service client send Request and server answer with response

The service declare by service message that has tow parts request and response

request
---
response

• For example Spawn.srv file under turtlesim.srv folder

float32 x
float32 y
float32 theta
string name
---
string 

When compile the packaged catkin build and install class file that contains the class for example:

• Spawn
• SpawnRequest
• SpawnResponse

#!/usr/bin/env python
import rospy
from turtlesim.srv import Spawn, SpawnRequest

def main():
    service = rospy.ServiceProxy('/spawn', Spawn)
    req = SpawnRequest(x=3.0, y=3.0, theta=0.0, name="t1")
    res = service(req)
    print res.name

if __name__ == "__main__":
    main

ROSPy node templates

Spin and Rate

spin() and rate() are resposible to handle communication events, e.g. arriving messages. If you are subscribing messages, services or actions you must call them to process the events.

Using rospy.rate

• rospy rate

#! /usr/bin/env python

import rospy
from datetime import datetime

NODE_NAME = "reading_laser"

def my_shutdown():
    rospy.loginfo("Shutdown node")

def main():
    rospy.init_node(NODE_NAME)
    rospy.loginfo("Init node: {}".format(NODE_NAME))
    rospy.on_shutdown(my_shutdown)
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        rate.sleep()
        rospy.loginfo(datetime.now().strftime("%b %d %Y %H:%M:%S"))

if __name__ == "__main__":
    main()  

• Loop run every one second (rate 1 Hz)
• Run Node (don't forget to chmod)

[INFO] [1540546445.840602]: Init node: reading_laser
[INFO] [1540546446.842748]: Oct 26 2018 12:34:06
[INFO] [1540546447.842572]: Oct 26 2018 12:34:07
[INFO] [1540546448.842655]: Oct 26 2018 12:34:08

Using rospy.spin

• main thread wait for shutdown (ctrl-c)
• Other event are invoked

#! /usr/bin/env python

import rospy
from datetime import datetime

NODE_NAME = "reading_laser"

def my_shutdown():
    rospy.loginfo("Shutdown node")

def main():
    rospy.init_node(NODE_NAME)
    rospy.loginfo("Init node: {}".format(NODE_NAME))
    rospy.on_shutdown(my_shutdown)
    
    while not rospy.is_shutdown():
        rospy.spin()

if __name__ == "__main__":
    main()  

GTest Hello

GTest

Google test is a framework for writing C++ unit tests

Download and install

• apt (download source code)
• run cmake
• copy to /usr/lib

sudo apt install libgtest-dev
cd /usr/src/gtest
sudo cmake CMakeLists.txt
sudo make
 
# copy or symlink libgtest.a and libgtest_main.a to your /usr/lib folder
sudo cp *.a /usr/lib

Demo Project structure

├── CMakeLists.txt
├── bin
├── build
├── src
│   ├── CMakeLists.txt
│   ├── hello.cpp
│   └── whattotest.cpp
└── tests
    ├── CMakeLists.txt
    ├── tests.cpp
    └── tests_main.cpp

• code under tests (whattotest.cpp)

#include <math.h>

double squareRoot(const double a){
    double b = sqrt(a);
    if (b != b){
        return -1.0;
    }else {
        return sqrt(a);
    }
}

• test main file (tests_main.cpp)

#include <gtest/gtest.h>

int main(int argc, char **argv) {
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

• Tests (tests.cpp)

#include "../src/whattotest.cpp"
#include <gtest/gtest.h>
 
TEST(SquareRootTest, PositiveNos) { 
    ASSERT_EQ(6, squareRoot(36.0));
}
 
TEST(SquareRootTest, NegativeNos) {
    ASSERT_EQ(-1.0, squareRoot(-15.0));
}

• Main CMakeLists.txt

cmake_minimum_required(VERSION 3.0)
project(scope)

#set binary output
set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin)
message(${EXECUTABLE_OUTPUT_PATH})
#set flags
set(CMAKE_CXX_STANDARD 14)

#add source directory
add_subdirectory(src)
add_subdirectory(tests)

• tests folder CMakeLists.txt

# Locate GTest
find_package(GTest REQUIRED)
include_directories(${GTEST_INCLUDE_DIRS})
 
# Link runTests with what we want to test and the GTest and pthread library
add_executable(runTests tests_main.cpp
                        tests.cpp)
target_link_libraries(runTests ${GTEST_LIBRARIES} pthread)

Reference

• Getting started with Google Test (GTest) on Ubuntu
• Googletest Primer
• Advanced googletest Topics
• A quick introduction to the Google C++ Testing Framework

BASH Tips

Gnome Terminal Theme

https://github.com/Mayccoll/Gogh

Use arguments from previous command

!^      first argument
!$      last argument
!*      all arguments
!:2     second argument

!:2-3   second to third arguments
!:2-$   second to last arguments
!:2*    second to last arguments
!:2-    second to next to last arguments

!:0     the command
!!      repeat the previous line

Using first argument

echo a b c d
a b c d
echo !:1
echo a
a

Using last argument

echo a b c d
a b c d
echo !$
echo d
d

VSCode cmake and debug

VSCode build with cmake

• ctrl+shift+p -> Tasks: Configure Task

vscode open tasks.json

• Project fs structure

├── bin
├── build
├── doc
└── src
    ├── CMakeLists.txt
    └── hello.cpp
└── CMakeLists.txt

• Add three task
  • cmake: run cmake from build sub folder
  • make: run make depend on cmake
  • build: bind the above

{
    "version": "2.0.0",
    "tasks": [
        {
            "label": "cmake",
            "type": "shell",
            "options": {
                "cwd": "${workspaceRoot}/build"
            },
            "command": "cmake",
            "args": [
                ".."
            ]
        },
        {
            "label": "make",
            "type": "shell",
            "options": {
                "cwd": "${workspaceRoot}/build"
            },
            "command": "make",
            "dependsOn": "cmake"
        },
        {
            "label": "build",
            "type": "shell",
            "dependsOn": "make",
            "command": "echo",
            "group": {
                "kind": "build",
                "isDefault": true
            }
        }
    ]
}

• Note: task build command key need to be fill with some value echo

cmake files

• root cmake
  • set bin folder as binary output
  • set c++ version: set(CMAKE_CXX_STANDARD 11)

cmake_minimum_required(VERSION 3.0)
project(tutorials)

#set binary output
set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin)
message(${EXECUTABLE_OUTPUT_PATH})
#set flags
set(CMAKE_CXX_STANDARD 11)
#add source directory
add_subdirectory(src)

• src

add_executable(hello hello.cpp)

Debug Task

• Control cmake settings from command line
• Add args to task
• Add option variable and if statement to cmake

Main CMakeLists.txt

• Add boolean variable control

option (MODE_DEBUG "Set debug mode" OFF)
if (MODE_DEBUG)
    message("DEBUG MODE")
    set (CMAKE_BUILD_TYPE Debug)
endif()

• Add new task to VSC with args -DMODE_DEBUG=TRUE

{
    "label": "cmake_with_debug",
    "type": "shell",
    "options": {
        "cwd": "${workspaceRoot}/build"
    },
    "command": "cmake",
    "args": [
        "..",
        "-DMODE_DEBUG=TRUE"
        ]
}

• Add VSC keybinding

{
    "key": "ctrl+shift+d",
    "command": "workbench.action.tasks.runTask",
    "args": "cmake_with_debug"
    }

Debug

launch.json

• Changed to debug mode
• From combo box -> Add configuration
  • open launch.json
• Set program to debug

"program": "${workspaceFolder}/bin/hello",

Cross-compile strace

Compile strace statically build use on RPi

- Download strace source code strace 4.8
- Download/Pull RPi toolchain: https://github.com/raspberrypi/tools
- Export CC variable, point to compiler RPi toolchain gcc

export CC=/tools/arm-bcm2708/arm-rpi-4.9.3-linux-gnueabihf/bin/arm-linux-gnueabihf-gcc

run

export LDFLAGS=-static && ./configure --host=arm-linux

make

Check

file strace

strace: ELF 32-bit LSB executable, ARM, EABI5 version 1 (SYSV), statically linked, for GNU/Linux 2.6.32, not stripped