Tuesday, December 4, 2018

layout: post title: Use Jinja to create gazebo sdf files categories: gazebo tags: [vscode, gazebo, jinja, sdf]

Jinja 101

Jinja2 is a template engine


pip install Jinja2


{{ }} Print and Evaluate an expression {% %} Statement {# #} Comments

Set variable

{% set title='hello world' %} {{ title }}


  • for loop
{% set names = ['a', 'b', 'c']%} {% for name in names %} {{ loop.index }}: {{name}} {% endfor %} # Output result 1: a 2: b 3: c
Variable Desc
loop.index index loop start with 1
loop.index0 index loop
  • if statment
{% set name='' %} {% if name %} {{ name }} {% else %} no name enter {% endif %}

White space control

use + and - are control whitespace

  • Run the loop example with hyphen at the end of line
{% set names = ['a', 'b', 'c'] -%} {% for name in names -%} {{ loop.index }}: {{name}} {% endfor -%} # same output without line spaces 1: a 2: b 3: c


{%- macro box(x, y, z) -%} <geometry> <box> <size>{{x}} {{y}} {{z}}</size> </box> </geometry> {%- endmacro -%} # Call the macro {{ box(1,1,1) }}

CLI for Jinja2

A CLI interface to Jinja2

  • install
sudo pip install jinja2-cli
  • usage
Usage: jinja2 [options] <input template> <input data>


  • Template
{% if foo is defined -%} foo difined and has value: define {{ foo }} {% endif -%}
  • Test
jinja2 -D foo=foo_value hello.txt.jinja # result foo difined and has value: define foo_value


  • Install vscode ext for better syntax color


Tuesday, November 27, 2018

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)
  • 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 = ('', 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": [] } ] }

Wednesday, November 21, 2018

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>


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

Sunday, November 18, 2018

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)


Thursday, November 15, 2018

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 frowning

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 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

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


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


  • run without gui
HEADLESS=1 make posix_sitl_default gazebo_<model>


  • 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


Friday, November 9, 2018

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


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: 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; }

Monday, November 5, 2018

SITL Gazebo camera stream

SITL Gazebo camera stream


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 smiley

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:

read image from gazebo

Friday, November 2, 2018

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

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

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