Python docker vscode and remote debugging

• Docker file (Dockerfile)
  • install project requirements
  • base on python 3.6

FROM python:3.6-alpine

COPY requirements.txt /tmp/requirements.txt
WORKDIR /tmp
RUN pip install -r requirements.txt

WORKDIR /project
CMD ["sh"]

• requirements.txt
  • ptvsd: version 4.1.4 (4.2.X version has issue with run debugging multiple times)

ptvsd==4.1.4

• Docker compose file base on image image: image name build: Docker file path volumes: shared project note that host folder point to current parent folder the remote path add project folder name automaticall

version: '2'

services:
  dev-image:
    image: python_36
    build: .
    ports:
      - 3000:3000
    privileged: true
    container_name: pydev
    volumes:
      - ../:/project

• vscode launch.json file

{
            "name": "Attach (Remote Debug)",
            "type": "python",
            "request": "attach",
            "port": 3000,
            "host": "localhost",
            "pathMappings": [
                {
                    "localRoot": "${workspaceFolder}",
                    "remoteRoot": "/project/"
                }
            ]
        }

• python simple example

import ptvsd
import time
import os

print("Waiting to attach")

address = ('0.0.0.0', 3000)
ptvsd.enable_attach(address)
ptvsd.wait_for_attach()

time.sleep(2)
x = 1
print(x)

run docker (tasks.json)

• start docker
  • --service-port:
  • dev-image: compose file service name
  • --name: container name

docker-compose -f docker-compose.yml run --service-ports --name devenv dev-image

• run remote application

#docker exec -it devenv python "<relative path from working directory>/simple.py"
docker exec -it devenv python "python_remote/src/simple.py"

• stop docker

docker-compose -f docker-compose.yml down

{
    "version": "2.0.0",
    "tasks": [
        {
            "label": "start_docker",
            "type": "shell",
            "options": {
                "cwd": "${workspaceFolder}/python_remote"
            },
            "command": "docker-compose -f docker-compose.yml run --service-ports --name devenv dev-image sh",
            "problemMatcher": []
        },
        {
            "label": "stop_docker",
            "type": "shell",
            "command": "docker-compose -f docker-compose.yml down",
            "options": {
                "cwd": "${workspaceFolder}/python_remote"
            },
            "problemMatcher": []
        },
        {
            "label": "run remote",
            "type": "shell",
            "command":["docker exec devenv", "python", "${relativeFile}"],
            "problemMatcher": []
        }
    ]
}

Start Gazebo and PX4 SITL separately

• Terminal 1

make posix_sitl_default gazebo no_sim=1

• Terminal 2

# PX4 gazebo plugins
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:/home/user/px4/Firmware/build/posix_sitl_default/build_gazebo
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:/home/user/px4/Firmware/Tools/sitl_gazebo/models

# add iris model into warehouse world
#run
gazebo --verbose /home/user/px4/Firmware/Tools/sitl_gazebo/worlds/warehouse.world

Add iris into warehouse.world

<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <!-- A global light source -->
    <include>
      <uri>model://sun</uri>
    </include>
    <!-- A ground plane -->
    <include>
      <uri>model://ground_plane</uri>
    </include>
    <include>
      <uri>model://asphalt_plane</uri>
    </include>
    <!--add iris model for example -->
    <include>
      <uri>model://iris</uri>
      <pose>1.01 0.98 0.83 0 0 1.14</pose>
    </include>

Note

Try to add model with gz command, the model add into the world model list but not visible

gz model --spawn-file=iris.sdf --model-name=iaaa -p 1.01 0.98 0.83 0 0 1.14

Gazebo ImageStamped message to OpenCV

Convert Gazebo camera ImageStamped message to OpenCV Mat

Client code subscribe to gazebo image topic message type ImageStamped and view the image as opencv mat

• Terminal 1 (run gazebo)

make posix_sitl_default gazebo_typhoon_h480

• Termianl 2 (view topics and topic message)

gz topic -l
# 
/gazebo/default/typhoon_h480/camera/link/irlock
/gazebo/default/typhoon_h480/cgo3_camera_link/camera/cmd
/gazebo/default/typhoon_h480/cgo3_camera_link/camera/image
/gazebo/default/typhoon_h480/cgo3_camera_link/camera_imu/imu
/gazebo/default/typhoon_h480/cgo3_camera_link/wrench
/gazebo/default/typhoon_h480/cgo3_horizontal_arm_link/wr

#
gz topic -i /gazebo/default/typhoon_h480/cgo3_camera_link/camera/image
Type: gazebo.msgs.ImageStamped

• Terminal 2 (run the app)

cd build
cmake ..
make
../bin/viewer

• callback function

void cb(ConstImageStampedPtr &msg)
{
    int width;
    int height;
    char *data;

    width = (int) msg->image().width();
    height = (int) msg->image().height();
    //+1 for null terminate
    data = new char[msg->image().data().length() + 1];

    memcpy(data, msg->image().data().c_str(), msg->image().data().length());
    //gazebo output rgb data
    //PixelFormat.R8G8B8
    cv::Mat image(height, width, CV_8UC3, data);

    cv::imshow("camera", image);
    cv::waitKey(1);
    delete data;  // DO NOT FORGET TO DELETE THIS, 
                  // ELSE GAZEBO WILL TAKE ALL YOUR MEMORY
}

cmake_minimum_required(VERSION 2.8)

project(gz_viewer)
set(CMAKE_CXX_STANDARD 11)
set(EXECUTABLE_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/bin)

find_package(gazebo REQUIRED)

include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

find_package( OpenCV REQUIRED )
 
add_executable(viewer viewer.cpp)
target_link_libraries(viewer ${OpenCV_LIBS} ${GAZEBO_LIBRARIES} pthread)

Reference

• How to convert Gazebo's ConstImageStamped to cv::Mat?
• gz client standalone listener
• my source code

MAVLink wireshark lua plugins

Write wireshark extension for mavlink

Write wireshark plugin to parse and display mavlink protocol. Plugin can be write with lua script language or C/C++

install and config wireshark to run as non root user

sudo apt install wireshark

# Config wireshark to run as non root user
sudo groupadd wireshark
sudo usermod -a -G wireshark $USER
sudo chgrp wireshark /usr/bin/dumpcap
sudo chmod o-rx /usr/bin/dumpcap
sudo setcap 'CAP_NET_RAW+eip CAP_NET_ADMIN+eip' /usr/bin/dumpcap
sudo getcap /usr/bin/dumpcap

• Logout and login again
• Note: the command: sudo dpkg-reconfigure wireshark-common not working for me

Hello lua - Wireshark dissector

Dissector are meant to analyze some part of a packet's data

• Create file hello.lua
• paste

print "hello lua\n\n\n"

• run

tshark -v -Xlua_script:<path>/hello.lua
#output
ne/Tools/wireshark/hello.lua
hello lua



TShark (Wireshark)

Mavlink 2.0 protocol structure

MAVLink 2 Packet Format

Lua dissector

-- 
mavlink_protocol = Proto("Mavlink", "Mavlink protocol")

mavlink_protocol.fileds = {}

-- call for every packet
-- buffer: packet data to dissect
-- pinfo: columns of the packt 
-- tree: packet tree items
function mavlink_protocol.dissector(buffer, pinfo, tree)
    length = buffer:len()
    if length == 0 then return end

    -- change protocol column value from tcp to Mavlink
    pinfo.cols.protocol = mavlink_protocol.name

    -- Add subtree item 
    local subtree = tree:add(mavlink_protocol, buffer(), "Mavlink protocol data")
end
-- Assign protocol to port
local udp_port = DissectorTable.get("udp.port")
udp_port:add(15540, mavlink_protocol)

source poc

poc source code

Test in wireshark

wireshark -i lo -f "udp port 14540" -Xlua_script:<path>/mavlink.lua

Using mavlink generator

MAVLink generator tool

• XML: select target XML from mavlink/message_definitions/1.0
• Out: output directory
• Language: wlua
• protocolo: 2.0

Note: minimum.xml parse only heartbeat message

Note

Generated code has bug parse msgid (maybe data from pixhawk are little endian) for know changed the code

• rshift to lshift
• shidt index 2,3 and not 1,2

local msgidt1 = buffer(offset,1):uint()
offset = offset + 1
local msgidt2 = buffer(offset,1):uint()
offset = offset + 1
local msgidt3 = buffer(offset,1):uint()
msgidt1 = bit.rshift(msgidt1, 8)
msgidt2 = bit.rshift(msgidt2, 16)
msgid = msgidt1+msgidt2+msgidt3
header:add(f.msgid, msgid)

local msgidt1 = buffer(offset,1):uint()
offset = offset + 1
local msgidt2 = buffer(offset,1):uint()
offset = offset + 1
local msgidt3 = buffer(offset,1):uint()
msgidt2 = bit.lshift(msgidt2, 8)
msgidt3 = bit.lshift(msgidt3, 16)
msgid = msgidt1+msgidt2+msgidt3
header:add(f.msgid, msgid)

Tip: run SITL without gui

• gazebo
• jmavsim

Gazebo

• run without gui

HEADLESS=1 make posix_sitl_default gazebo_<model>

jMAVsim

• Disabled jmavsim GUI

setViewType(VIEW_TYPE);
setZoomMode(ZOOM_MODE);
setVisible(true); // -> setVisible(false)
splitPane.resetToPreferredSizes();
toggleReportPanel(false);
resetView();

• Compile jmavsim from <>/Firmware/Tools/jMAVSim run ant to compile java

• Run SITL

make posix_sitl_default jmavsim

Resource

• Creating a Wireshark dissector in Lua - part 1

Gazebo topic subscribe

Gazebo communicates on TCP/IP sockets Messages are sent on named channels called topics via publishers. On the other side of a topic are subscribers, which receive callbacks when messages arrive.

gz topic -l list the topic on running system

Subscriber

Demo code subscribe to /gazebo/default/world_stats

• topic list

gz topic -l

....
/gazebo/default/world_control

/gazebo/default/world_stats

/gazebo/motor_failure_num
/gazebo/server/control
/gazebo/world/modify

• topic info

gz topic -i /gazebo/default/world_stats
Type: gazebo.msgs.WorldStatistics

Publishers:
 192.168.2.200:39165

Subscribers:

• topic message view

# gz topic -e (echo)
gz topic -v /gazebo/default/world_stats

Write code

• Reference

• My GIT

• cmake file (not include project name and minimum_required)

find_package(gazebo REQUIRED)

include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

add_executable(sub_demo sub_demo.cpp)
target_link_libraries(sub_demo ${GAZEBO_LIBRARIES} pthread)

• sub_demo source

#include <gazebo-7/gazebo/transport/TransportIface.hh>
#include <gazebo-7/gazebo/msgs/msgs.hh>
#include <gazebo-7/gazebo/gazebo_client.hh>

#include <iostream>
#include <csignal>

const std::string TOPIC = "/gazebo/default/world_stats";

void my_shutdown(int signal){
    std::cout<<"shutdown gazebo client" << std::endl;
    gazebo::client::shutdown();
    exit(0);
}

void cb(ConstWorldStatisticsPtr &_msg){
    std::cout << _msg->DebugString() << std::endl;
}

int main(int argc, char **argv){
    signal (SIGINT, my_shutdown);
    gazebo::client::setup(argc, argv);

    //create node for communication
    gazebo::transport::NodePtr node(new gazebo::transport::Node());
    node->Init();

    //subscribe to topic
    gazebo::transport::SubscriberPtr sub = node->Subscribe(TOPIC, cb);

    while(true){
        gazebo::common::Time::MSleep(100);
    }
    
    return 0;
}

SITL Gazebo camera stream

• px4 gazebo

Prerequisites

sudo apt-get install $(apt-cache --names-only search ^gstreamer1.0-* | awk '{ print $1 }' | grep -v gstreamer1.0-hybris) -y

Enable GStreamer Plugin

• Changed line at file <Framework>/Tools/sitl_gazebo/CMakeLists.txt

  option(BUILD_GSTREAMER_PLUGIN "enable gstreamer plugin" "ON")
  

• Run

  posix_sitl_default gazebo_typhoon_h480
  

View stream

• Run gstream

gst-launch-1.0  -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink fps-update-interval=1000 sync=false

View From Ground control

• Config GSC

• Stream view (bottom left

Opt flow

make posix_sitl_default gazebo_iris_opt_flow

gz topic -l

/gazebo/default/iris_opt_flow/px4flow/link/opticalFlow
/gazebo/default/iris_opt_flow/px4flow/link/px4flow/cmd
/gazebo/default/iris_opt_flow/px4flow/link/px4flow/image
/gazebo/default/iris_opt_flow/px4flow/link/px4flow/imu
/gazebo/default/iris_opt_flow/px4flow/link/wrench

gz topic -i /gazebo/default/iris_opt_flow/px4flow/link/px4flow/image
Type: gazebo.msgs.ImageStamped

Publishers:
 192.168.2.200:45849

read image from gazebo

• How to convert Gazebo's ConstImageStamped to cv::Mat?

• Topics subscription

• nodejs

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