NAV Navbar

Einführung

Wie diese Dokumentation aufgebaut ist

Diese Dokumentation soll alle Aspekte des Quadrocopterlabors und der stattfindenden Projekte abdecken. Jeder Aspekt ist in einem Abschnitt auf oberster Gliederungsebene einsortiert. Die Struktur darunter kann variieren, sollte nach Möglichkeit aber nicht zu tief verschachtelt sein.

Die Dokumentation sollte so aufgebaut sein, dass sie sich einfach lesen lässt, das heißt die Abschnitte sollten thematisch aufeinander aufbauen und einzelne Abschnitte sollten Erklärungen beinhalten, sodass diese auch Einsteiger/Anfänger nicht überfordert.

Hinweise zur Formatierung

Hallo Welt!

std::cout << "Hallo Welt" << std::endl;
print('Hallo Welt')
System.out.println("Hallo Welt");
print('Hallo Welt')
echo 'Hallo Welt'

Codesnippets können direkt neben der Erklärung angezeigt werden. Es sollte jeweils eine Überschrift darüber stehen, falls der Code zum Text verschoben ist.

Tipps

Tipps können mithilfe von aside-Tags als farblich hinterlegtes Band dargestellt werden, wie in den folgenden Beispielen:

<aside class="success">
Nützliche Tipps, die einem die Bedienung erleichtern.
</aside>
<aside class="notice">
Konventionen, die beachtet werden sollten mit Begründung, warum diese Konventionen sinnvoll sind.
</aside>
<aside class="warning">
Fallstricke die ziemlich sicher zu Fehlern führen, wenn man sie nicht beachtet.
</aside>

spgit Struktur

Alle Nodes sollen als Repository unterhalb von Research Projects/Quadrocopter abgelegt und in einer der Untergruppen eingeordnet werden:

Qualitätsziele

Wir wollen aufgeräumten, verständlichen, stabilen und wartbaren Code um effizient arbeiten zu können. Um dies zu erreichen wollen wir einheitlichen Richtlinien folgen, Code möglichst vollständig und regelmäßig testen und gut und verständlich dokumentieren.

Jedes Repository sollte:

Labor

Lab

Labor:
O26/0204
☎️ 24281

Thomas W.:
O27/4103
☎️ 24164

PC-Pool:
O27/412
☎️ 24169

Das Quadcopter Lab befindet sich in O26/0204 auf Niveau 0. Das Labor hat etwa 100m² (7m x 14m). Die Telefonnummer ist 24281. Um Zugang zum Labor zu erhalten wird daher sowohl ein Schlüssel für Niveau 0 als auch ein Schlüssel für das Labor benötigt.

Das Labor besteht aus

Die Kameras erkennen die reflektiven Marker und der via Ethernet verbundene Windowsrechner kann aus mindestens 3 Markern pro Objekt die Pose im Raum berechnen sowie das Objekt identifizieren. Die Pose und das erkannte Objekt werden dann als vrpn-Datenstrom an den verbundenen Ubunturechner geschickt. Dort läuft ein ROS-Node (vrpn-client-node), der die Pose via tf in ROS verfügbar macht.

Optitrack

Kameras kalibrieren

Die Kalibration der Kameras erfolgt fast automatisch. Nachdem alle Kameras und Optitrack Motive gestartet sind kann das Kalibrationspanel über Layout -> Calibrate angezeigt werden

Wikiseite des Herstellers dazu.

Optitrack Motive

Koordinatensystemkonvention

Fahrzeugkoordinaten Flugzeugkoordinaten

Per Konvention sind Koordinatensysteme bei uns normalerweise rechtshändige Koordinatensysteme mit x-Achse nach vorne, y-Achse nach links und z-Achse nach oben (erstes Bild). Ausnahmen bilden die internen Koordinatensysteme der Saphira Drohnen, die eine nach unten gerichtete z-Achse und NED Koordinaten verwenden (zweites Bild) sowie Optitrack, das x-Achse nach oben, y-Achse nach oben und z-Achse nach rechts verwendet. Alle Distanzen werden in Metern gemessen (allgemein überall SI-Einheiten, also Sekunden, Meter, kg, …).

Die deshalb nötigen Transformationen sollten automatisch im vrpn_client_node bzw im saphira_driver_node durchgeführt werden.

Rechner und installierte Software

Hardware

ROS: Einführung

Das Robot Operating System ist ein umfangreiches Framework für Robotikanwendungen. Es wird in Distributionen unterteilt, die sich jeweils an spezifische Ubuntu-Distributionen richten, und eine ähnliche Funktion erfüllen (mehrere untereinander kompatible Tools bereitzustellen). So haben die ROS-Versionen auch Namen und es gibt LTS-Releases. Die aktuelle LTS-Distribution von ROS ist Melodic Morenia, welche auf Ubuntu 18.04 LTS (Bionic Beaver) abzielt. Diese Kombination wird zurzeit auch im Quadrocopter-Labor im Keller verwendet.

ROS umfasst viele verschiedene Tools, in die man sich etwas einarbeiten muss, doch die grundlegenden Konzepte sind einfach und können Schritt für Schritt erlernt werden. In dieser Einführung sollen die grundlegenden Konzepte erläutert werden und an einem kleinen Tutorial angewendet werden.

Da sich seit der Konzeption von ROS einiges geändert hat, wird seit einigen Jahren ROS 2 entwickelt. Es ist mit dem Ziel entwickelt worden, die Vorteile von ROS1 zu behalten und die Nachteile auszubessern. ROS2 ist grundsätzlich mit ROS1 inkompatibel, die Basics sind aber sehr ähnlich. Deshalb, und auch weil im Keller zur Zeit noch ROS 1 verwendet wird, beschäftigt sich diese Einführung nur mit ROS 1. Zu ROS 2 gibt es später im Wiki einen eigenen Abschnitt.

Grundlegende Konzepte

Zusammensetzung des Messagetypen geometry_msgs/Pose

geometry_msgs/Point position
    float64 x
    float64 y
    float64 z
geometry_msgs/Quaternion orientation
    float64 x
    float64 y
    float64 z
    float64 w

start_ar_bebop.launch im Paket quad_ar (gekürzt)

<launch>
  <arg name="ns" default="/start_bebop"/>
  <arg name="quad_name" default="quad1"/>
  <arg name="script_path" default="$(find quad_ar)/script/screen.lua"/>
  <!-- ... -->

  <group ns="$(arg ns)">
    <!-- quad_ar node -->
    <node name="quad_ar"
          pkg="quad_ar"
          type="quad_ar_node.py"
          required="true"
          output="screen"/>

    <!-- ar_drone -->
    <node name="ar_drone"
          pkg="quad_ar"
          type="ar_drone.py"
          output="screen"
          required="true" />

    <!-- optitrack -->
    <include file="$(find bebop_position_control)/launch/optitrack.launch">
      <arg name="ns" value="$(arg ns)/optitrack"/>
      <!-- ... -->
      <arg name="static_obstacle_config" default="$(find quad_ar)/config/labor.txt"/>
    </include>

    <!-- bebop -->
    <include file="$(find bebop_position_control)/launch/bebop.launch">
      <arg name="ns" value="$(arg ns)/$(arg quad_name)"/>
      <arg name="pose_topic" value="$(arg ns)/optitrack/vrpn_client_node/$(arg quad_name)/pose"/>
      <arg name="script_path" value="$(arg script_path)"/>
      <!-- ... --> 
    </include>
   </group>
</launch>

ROS Installation

check ob ROS installiert ist

rosversion -d

Nun zur Installation von ROS. Für die Arbeit am Projekt wirst du zusätzlich einzelne ROS-Packages benötigen (von ROS direkt, von anderen Projekten / Repos, oder aus unseren eigenen Repos). Da diese sich je nach Projekt unterscheiden, installieren wir hier erst mal das Grundgerüst, was einfach über apt mithilfe eines PPAs geht.

Dazu der Anleitung von ROS folgen, Abschnitt 1.7 kann erst mal weggelassen werden. Wichtig: im Abschnitt 1.4 das Komplettpaket "Desktop-Full Install: (Recommended)" (ros-melodic-desktop-full) wählen. Es beinhaltet ROS sowie einige optionale Zusatztools, die man früher oder später sowieso braucht. Außerdem sind ein paar einfache Beispiele enthalten, derer wir uns im nächsten Abschnitt bedienen werden.

Evtl. noch kurz im Terminal rosversion -d ausführen. Wenn alles geklappt hat, sollte melodic rauskommen.

TurtleSim ausprobieren

turtlesim

Um die Füße ein bisschen einzutauchen, schauen wir uns einmal das turtlesim Beispiel an, welches in ROS enthalten ist. Dieses Tutorial ist zwar für die ältere ROS-Distribution kinetic, ist aber ausführlicher und besser als die Kurzfassung hier. Die ältere Version sollte hinsichtlich des Tutorials keinen Unterschied machen.

roscore starten

roscore starten

roscore

Zunächst den ROS master im Terminal über roscore starten.

turtlesim_node starten

turtlesim_node starten

rosrun turtlesim turtlesim_node

rospack list zeigt uns alle installierten ROS packages mitsamt Pfad an. Es sollte ein Paket namens turtlesim angezeigt werden. Die ROS Commandline Tools machen grundsätzlich einen guten Job bei der <Tab>-Autovervollständigung: z.B. werden uns bei rosrun turtlesim <Tab> alle nodes im turtlesim Paket angezeigt. Wir starten also den turtlesim_node, der uns ein Fenster mit einer ROS-Schildkröte anzeigt.

Position auslesen

auf Pose-Topic subscriben

rostopic echo /turtle1/pose

rostopic list zeigt uns alle verfügbaren topics an. Die namespaces sind hierarchisch, z.B. sehen wir das topic pose im namespace turtle1. Um auf ein topic zu subscriben, benutzen wir rostopic echo <topicname>. Wir lassen uns also die aktuelle Pose (Position + Rotation) der turtle1 anzeigen, aber da tut sich momentan natürlich noch nichts.

Turtle bewegen

Turtle im Kreis fahren lassen

rostopic pub /turtle1/cmd_vel geometry_msgs/Twist -r 10 "linear:
  x: 1.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 1.0"

Beim Aufruf von rostopic list vorhin wurde uns auch turtle1/cmd_vel aufgelistet. Den Typen dieses Topics finden wir über rostopic type <topicname> heraus. Wir sehen, dass turtle1/cmd_vel/ Nachrichten vom Typ geometry_msgs/Twist erwartet. Wenn wir sehen wollen, wie sich der typ zusammensetzt, benutzen wir rosmsg show <messagetype>, oder wir nutzen wieder ROS' gute Autocompletion: Bei rostopic pub /turtle1/cmd_vel <Tab> wird der Typ ergänzt, und bei nochmaligem <Tab> werden seine Bestandteile ergänzt und mit Defaultwerten (in diesem Fall 0) gefüllt. Passen wir nun die lineare x-Komponente an und drücken Enter, sehen wir, dass die Schildkröte sich nach vorne bewegt. Wenn man auf dem Topic turtle1/Pose horcht, sieht man nun auch, wie die neuen Positionen sofort gepublisht werden.

Allerdings hört die Schildkröte auch schnell wieder auf sich zu bewegen. Das liegt daran, dass man explizit angeben muss, wenn eine Nachricht auf einem Topic dauerhaft gepublisht werden soll. Das geht mit dem flag -r <hz>, mit dem man die publishing rate in Hertz angibt. Wenn wir unserem cmd_vel Befehl von vorhin also z.B. -r 10 hinzufügen, bewegt sich die Schildkröte weiter. (Guckt man auf das Terminal, in dem der turtlesim_node gestartet wurde, bemerkt man die ROS-Warnungen "Oh no! I hit the wall!", sobald die Turtle am Rand angekommen ist.)

Wenn man die angular z-Komponente in der cmd_vel Nachricht anpasst, kann man die Schildkröte übrigens auch rotieren lassen.

Services

Farbe / Stärke der Linie ändern

rosservice call /turtle1/set_pen "{r: 255, g: 0, b: 0, width: 3, 'off': 0}"

Services können wir uns über rosservice list auflisten lassen, und über rosservice call <servicename> aufrufen (rosservice list --nodes zeigt uns, von welchem node die services angeboten werden). Ein gängiger Service ist /reset, der unser System wieder zurücksetzt (Die Semantik hiervon ist natürlich nicht immer eindeutig). Darüber hinaus gibt es aber auch package-spezifische Services. Im turtlesim-package gibt es da z.B. <turtlename>/set_pen, welchen wir gleich ausprobieren.

Vorher starten wir aber noch den vorgefertigten Node draw_square im turtlesim-package, der uns ein Quadrat zeichnet. So können wir die Auswirkungen unserer service calls besser sehen.

Wieder bedienen wir uns der Autocompletion (rosservice call /turtle1/set_pen <Tab>) und sehen, dass wir Farbe, Linienstärke, und on/off als Argumente mitgeben können. Beim Aufruf von rosservice call /turtle1/set_pen "{r: 255, g: 0, b: 0, width: 3, 'off': 0}" sieht man, wie sich die Farbe und Stärke der Linie ändert.

ROS: catkin workspaces einrichten

Die Anleitung ist an https://spgit.informatik.uni-ulm.de/research-projects/quadrocopter/quadrotor_workspace/wikis/home und https://wiki.ros.org/catkin/Tutorials/workspace_overlaying angelehnt.

Workspace mit Fremdcode:

Da die hector Pakete Probleme machen, wenn sie mehrfach konfiguriert werden (Bisherige Problemlösung: devel und build Ordner im Workspaceverzeichnis löschen und alles neu bauen), wird der Workspace aufgeteilt in einen Teil mit Fremdcode (sollte sich selten ändern und neu konfiguriert werden) und einen Teil mit eigenem Code. (aus Anleitung quadrocopter-workspace)

Workspace erstellen

# Installation fehlender ros Pakete
sudo apt install ros-melodic-hector-gazebo-plugins
sudo apt install ros-melodic-geographic-msgs
sudo apt install ros-melodic-joy
sudo apt install ros-melodic-rosbridge-server 

# Workspace einrichten
mkdir ~/ros_ws
cd ~/ros_ws
mkdir src
cd src
catkin_init_workspace
git clone ... # Repositiories mit Fremdcode
cd ~/ros_ws
catkin_make
source devel/setup.bash

Repositiories mit Fremdcode:

git clone git@spgit.informatik.uni-ulm.de:quadrocopter/vrpn_lib.git --recurse-submodules &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/vrpn_client_ros.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/hector_localization.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/hector_quadrotor.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/parrot_arsdk.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/bebop_autonomy.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/teleop_tools.git

Nun alles bauen

cd ~/ros_ws
catkin_make #mehrmals wiederholen, bis alles baut!

Um fehlenden Abhängigkeiten beim späteren Starten des Bebop Treibers zu vermeiden, muss noch folgender Befehl ausgeführt werden.

cd ~/ros_ws/devel/lib/parrot_arsdk && cp -av lib* ..

Workspace mit eigenen Code und chaining des Workspaces

Workspace mit eigenen Code

mkdir ~/catkin_ws
cd ~/catkin_ws
mkdir src
cd src
catkin_init_workspace
git clone .. Repositories mit eigenen Code (unten aufgelistet)
cd ~/catkin_ws
## Chaining catkin workspace
source ~/ros_ws/devel/setup.(bash|zsh) ## bash bzw. zsh ob bash oder zsh benutzt werden muss, hängt von der Konsole ab
catkin_make --pkg se_trajectory_msgs
catkin_make
## Für bash Konsolen
echo "source ~/ros_ws/devel/setup.bash" >> ~/.bashrc
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc


(Mindestens) folgende Pakete auschecken:

git clone git@spgit.informatik.uni-ulm.de:quadrocopter/bebop_position_control.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/quad_state.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/se-trajektorien.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/trajectory_server.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/gesture-node.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/quad_obstacle.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/blockly.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/sim_photo.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/quad_common_utils.git &&
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/quad_script.git && 
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/se_trajectory_msgs.git

Beim Output von catkin_make sieht man, ob das chaining funktioniert hat.

Workspace overlay

-- This workspace overlays: /home/thomas/Project/ros_ws/devel;/opt/ros/melodic ...

ROS: Nodes schreiben

Package erstellen

Node erstellen

Logging

Publisher

Subscriber

Actions

Parameter

Timer

tf

tf2_ros doku

tf2 tutorials

Koordinatensystemkonvention

broadcaster schreiben

listener schreiben

Nodes testen

Neue Messages, Services und Actions definieren

ROS: rviz

Marker

Interactive Marker

ROS: rqt

Plugins schreiben

ROS: Best practices

ROS: Continous Integration

# Beispiel für mehrere Abhängigkeiten (Repositorys)
 - for repo in \
       "abhaengigkeit1" \
       "abhaengigkeit2" \
       ...

# Beispiel für keine Abhängigkeiten (Repositorys)
 - for repo in \
       "" \
       ...

Die Abhängigkeiten müssen per Hand, manuell in die .gitlab-ci.yml in der Form nebenan in der gegebenen Zeile der .gitlab-ci.yml eingetragen werden. Falls keine Abhängigkeiten auf andere Repositorys bestehen kann statt "abhaengigkeit1" der leere String verwendet werden. Eine beispielhafte .gitlab-ci.yml des bebop_position_controller s mit Catkin ist angehängt. Diese führt alle notwendigen Schritte aus, um das Repository samt Abhängigkeiten zu bauen und gegebenenfalls Tests durchzuführen.

# gitlab-ci.yml: bebop_position_controller

build:
 stage: build
 script:
   - source /opt/ros/melodic/setup.bash
   - source ~/catkin_ws/devel/setup.bash

   - rm -f catkin_ws/src/* || true

   - mkdir -p catkin_ws/src

   - shopt -s extglob
   - if [ -f "package.xml" ]; then 
        mkdir -p "catkin_ws/src/$(basename $(pwd))" ;
        ln -s $(pwd)/!(catkin_ws) catkin_ws/src/$(basename $(pwd))/. ;
      else
        ln -s $(pwd)/!(catkin_ws) catkin_ws/src/. ;
      fi

   - cd catkin_ws/src

   - for repo in \
         "se_trajectory_msgs" \
         "quad_common_utils" \
     ; do repo2=$(echo $repo | sed -r 's/^\s*(.+)\s*/\1/g') && if [ ! -z "$repo2" ]; then echo "Cloning repository $repo2" && git clone "git@spgit.informatik.uni-ulm.de:quadrocopter/${repo2}.git" ; fi ; done

   - cd ../..

   - cd catkin_ws
   - catkin_make clean
   - catkin_make
   - source devel/setup.bash
 artifacts:
    paths:
        - catkin_ws

bebop guard node:
 stage: test
 script:
   - source catkin_ws/devel/setup.bash
   - rostest bebop_position_control bebop_guard_node.test

bebop switch node:
 stage: test
 script:
   - source catkin_ws/devel/setup.bash
   - rostest bebop_position_control bebop_switch_node.test

ROS2: Features

ROS2 ist die neue Version von ROS, die parallel zu ROS entwickelt wird mit halbjährlichen Releases. Die aktuelle Version (Stand August 2019) ist dashing. Diese ist die erste ROS2 LTS Version, die bis Mai 2021 supported wird.

Lifecycle

Nodes in ROS2 können einen vorgegebenen Lifecycle implementieren. Dies ermöglicht es, das Konfigurieren und Starten der Nodes voneinander zu trennen. Zunächst werden alle Nodes konfiguriert und erst wenn alle Nodes bereit sind wird die Anwendung gestartet.

Components

Nodes können als Komponente in eine Bibliothek gebaut werden. Ein Node kann eine solche Komponente fest beim Start oder dynamisch zur Laufzeit laden. Zusätzlich können mehrere Komponenten in einen Prozess geladen werden um den Overhead beim Nachrichtentransport zu reduzieren.

Dezentrale Discovery

Es existiert kein rosmaster als zentraler Nameserver mehr. Dieser stellt in ROS1 einen Single Point of Failure dar. Stattdessen passiert die Discovery von Nodes/Services etc dezentral. Ein Effekt ist, dass keine globalen Parameter mehr existieren können, sondern jeder Parameter von einem Node verwaltet wird, der auf eigene Parameter sehr einfach zugreifen kann, Parameter anderer Nodes aber über einen RPC abrufen muss.

neues launch System

rcl

ROS2: Installation

Paketquellen und Keys einrichten

sudo apt update && sudo apt install curl gnupg2 lsb-release
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
sudo sh -c 'echo "deb [arch=amd64,arm64] http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list'

Notwendige Pakete installieren

sudo apt update
sudo apt install ros-<distro>-desktop  # ros-dashing-desktop, ros-eloquent-desktop
sudo apt install python3-colcon-common-extensions

Workspace einrichten

mkdir -p ~/ros2_ws/src

Die Installation unter Ubuntu 18.04 ist hier beschrieben.

Folgende Zeilen müssen zur ~/.bashrc hinzugefügt werden

function <distro> {
    source /opt/ros/<distro>/setup.bash
    source ~/ros2_ws/install/setup.sh
}

Um ROS2 in einem Terminal zu nutzen

dashing  # bzw. eloquent

Um ROS2 zu nutzen muss das Environment gesourced werden. Dazu müssen die setup.sh der systemweiten ROS2 installation, sowie die des lokalen Workspaces gesourced werden.

ROS2: Buildsystem einrichten

Colcon

kompletten Workspace bauen

colcon build
colcon build --packages-select ...   # baut nur ausgewählte packages
colcon build --packages-up-to ...    # baut ausgewählte packages mit allen Abhängigkeiten
colcon build --symlink-install ...   # baut und installiert als symlinks

Das Buildsystem von ROS2 ist Colcon. Colcon sammelt alle Abhängigkeiten zwischen Paketen eines Workspaces und startet den Buildvorgang. Pakete werden als Unterverzeichnisse im Ordner src unterhalb des Workspaces abgelegt. Sie können zur besseren Gliederung auch tiefer verschachtelt werden. Colcon unterstützt mehrere Buildsysteme für Pakete: ament_cmake, cmake, ament_python und auch catkin. Sowohl ament als auch catkin bauen auf cmake auf und vereinfachen das Schreiben der CMakeLists.txt durch einige Funktionen.

Colcon baut die Packages isoliert voneinander. Das heißt, Pakete können einzeln gebaut werden und werden getrennt gebaut und installiert (Ein reiner catkin build in ROS1 hat einfach alle Buildtargets aller Packages gesammelt und gemeinsam gebaut). Daher müssen die Abhängigkeiten korrekt angegeben sein, damit Abhängigkeiten zu anderen Packages überhaupt gefunden werden!

Soll der komplette Workspace neu gebaut werden, können die Verzeichnisse build, install und log gefahrlos gelöscht werden.

Package erstellen

ein neues vorkonfiguriertes ROS2 Paket erstellen

cd ~/ros_ws/src
ros2 pkg create my_package --package_format 3 --build-type ament_cmake --dependencies rclcpp

Um ein ROS package zu erstellen reicht es, in einem Unterverzeichnis unterhalb von src im Workspace eine CMakeLists.txt mit der Buildkonfiguration, sowie eine package.xml mit Metadaten anzulegen. ROS2 kann dazu direkt ein Template generieren, das nur noch ausgefüllt werden muss.

package.xml

Abhängigkeiten in der package.xml eintragen

<depend>rclcpp</depend>

Hängt das eigene Package von einem anderen ROS2 Package ab, so muss diese Abhängigkeit sowohl in die package.xml als auch in die CMakeLists.txt eingetragen werden. In der package.xml ab Format 2 reicht dazu ein depend Tag.

CMakeLists.txt

Abhängigkeiten in der CMakeLists.txt eintragen

find_package(rclcpp REQUIRED)

Um ein anderes Package innerhalb von CMake nutzen zu können muss dieses zunächst mit find_package gefunden werden. Danach kann die Abhängigkeit einzelnen Targets hinzugefügt werden.

IDE (qtcreator) konfigurieren

Falls eine IDE genutzt werden soll kann das Projekt dort konfiguriert werden. Hier wird als Beispiel qtcreator verwendet.

[Möglichkeit 1] ROS Plugin für Qt-Creator verwenden

Hier befindet sich eine Installationsanleitung für das Plugin (bzw ein qtcreator bundle mit installiertem Plugin). Die anleitung wurde mit ROS Eloquent getestet und sollte wie beschrieben funktionieren. Einfach den Workspace als Projekt importieren und die ROS-Distribution auf die richtige Version stellen und den gewünschten Buildtyp einstellen.

Qrcreator ros plugin project

Qtcreator ros plugin config

[Möglichkeit 2] manuelle Einrichtung des Projekts

Da der CMake build Abhängigkeiten zu ament etc hat und colcon als Buildtool zum Auflösen der Abhängigkeiten genutzt wird ist es deutlich einfacher kein CMake-Projekt anzulegen, sondern ein bestehendes Projekt zu importieren.

Qt Creator: neues Projekt

quad_state.includes

/opt/ros/eloquent/include
src
../../install/quad_common_utils/include
include/quad_state
include
test

Nach Import aller Dateien werden im Packageverzeichnis 5 neue Dateien angelegt (my_package.cflags, my_package.config, my_package.cxxflags, my_package.files, my_package.includes). Sollen dem Projekt neue Dateien hinzugefügt werden, müssen sie in der .files Datei hinzugefügt werden. Die Includes Datei sollte die Includepfade der Abhängigkeiten enthalten, damit die statische Codeanalyse funktioniert.

Als nächstes kann der Build in der IDE konfiguriert werden:

Qt Creator: Build

Wichtig ist, dass beim Build sowie der Run Konfiguration zunächst das Environment gesourced wird. Im Beispiel wird dies über eine interaktive Shell (bash -i) erreicht und das Sourcen des Environments (eloquent;), bevor der eigentliche Build mit colcon gestartet wird. Analog dazu kann so auch ein Node oder eine launch Konfiguration mit ros2 run oder ros2 launch als run-Konfiguration gestartet werden.

ROS2: Nodes schreiben

Sobald ein Workspace eingerichtet und das Environment gesourced ist und ein Package erstellt wurde, können eigene ROS2 Nodes/Komponenten geschrieben werden.

Lifecycle Node erstellen

CMakeLists.txt

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED)
find_package(rclcpp_components REQUIRED)

include_directories(include)

# Build component
add_library(thing_component SHARED src/thing_component.cpp)
target_compile_definitions(thing_component PRIVATE "COMPOSITION_BUILDING_DLL")
ament_target_dependencies(thing_component rclcpp rclcpp_lifecycle rclcpp_components)
rclcpp_components_register_nodes(thing_component "sp::Thing")

# Build node
add_executable(thing_node src/thing_node.cpp)
target_link_libraries(thing_node thing_component)
ament_target_dependencies(thing_node rclcpp rclcpp_components)

# Install headers
install(
  DIRECTORY include/
  DESTINATION include
)

# Install components
install(TARGETS
  thing_component
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION include
)

# Install nodes
install(TARGETS
  thing_node
  DESTINATION lib/${PROJECT_NAME}
)

Ein minimaler 'Thing'-Node besteht aus mindestens folgenden Dateien:

CMakeLists.txt

Zunächst müssen die Abhängigkeiten gefunden werden. Ein Lifecycle Node, der als Komponente erstellt wird benötigt mindestens rclcpp, rclcpp_lifecycle, rclcpp_components.

Danach muss die Komponente gebaut und registriert werden. add_library erstellt die Komponente aus den angegebenen Quelldateien. Es müssen zusätzlich noch die Abhängigkeiten angegeben und die Komponente registriert werden (mit Namespace und Klassenname).

Nun kann der Node gebaut werden. Dieser hat nut rclcpp(_components) und die vorher gebaute Komponente als Abhängigkeit.

Die Install-Anweisungen installieren dann alle erstellten Artefakte in die entsprechenden Verzeichnisse.

thing_component.hpp

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"

namespace sp {

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

// create a lifecycle node
class Thing : public rclcpp_lifecycle::LifecycleNode {

public:
    explicit Thing(const rclcpp::NodeOptions & options);

    // callback functions for the different stages of the lifecycle
    CallbackReturn on_configure(const rclcpp_lifecycle::State &) override;
    CallbackReturn on_activate(const rclcpp_lifecycle::State &) override;
    CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override;
    CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override;
    CallbackReturn on_shutdown(const rclcpp_lifecycle::State &) override;

private:
};

}

thing_component.cpp

#include "thing/thing_component.h"

namespace sp {

Thing::Thing(const rclcpp::NodeOptions & options)
  : rclcpp_lifecycle::LifecycleNode("thing_component", options)
{
}

CallbackReturn Thing::on_configure(const rclcpp_lifecycle::State &) {
    return CallbackReturn::SUCCESS;
}

CallbackReturn Thing::on_activate(const rclcpp_lifecycle::State &) {
    return CallbackReturn::SUCCESS;
}

CallbackReturn Thing::on_deactivate(const rclcpp_lifecycle::State &) {
    return CallbackReturn::SUCCESS;
}

CallbackReturn Thing::on_cleanup(const rclcpp_lifecycle::State &) {
    return CallbackReturn::SUCCESS;
}

CallbackReturn Thing::on_shutdown(const rclcpp_lifecycle::State &) {
    return CallbackReturn::SUCCESS;
}

}

// register the component
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(sp::Thing)

thing_component.hpp/.cpp

LifecycleNodes implementieren intern eine State-Machine wie hier dargestellt: Node lifecycle

Um einen Lifecycle Node zu erstellen muss von der Klasse rclcpp_lifecycle::LifecycleNode abgeleitet werden. Für die einzelnen Zustandsübergänge werden entsprechende Callbackfunktionen überschrieben. Die Callbacks bekommen als Argument die vorherige Stage (kann meist ignoriert werden). Der Rückgabewert ist entweder CallbackReturn::SUCCESS oder CALLBACK_RETURN::ERROR.

Am Ende muss die Klasse noch als Komponente registriert werden. Dies geschieht mit RCLCPP_COMPONENTS_REGISTER_NODE.

thing_node.cpp

#include "thing/thing_component.hpp"

int main(int argc, char * argv[]) {
    // Force flush of the stdout buffer.
    setvbuf(stdout, NULL, _IONBF, BUFSIZ);

    rclcpp::init(argc, argv);

    rclcpp::executors::SingleThreadedExecutor exec;
    rclcpp::NodeOptions options;

    // load the publisher component manually into the node
    auto component = std::make_shared<sp::Thing>(options);
    exec.add_node(component->get_node_base_interface());
    exec.spin();

    rclcpp::shutdown();

    return 0;
}

thing_node.cpp

Der Node kann direkt übernommen werden, es muss nur die Klasse der geladenen Komponente angepasst werden. In Zukunft ist der Node evtl überhaupt nicht mehr nötig, wenn es funktioniert, eine Lifecycle-Komponente zur Laufzeit in einen Node Container zu laden (in einem launch file).

Was gehört in die einzelnen Stages?

Logging

ROS2 log messages ausgeben

RCLCPP_DEBUG(get_logger(), "Some debug info %d", 5);
RCLCPP_INFO_ONCE(get_logger(), "Some info that is outputted only once %d", 5);
RCLCPP_WARN_SKIPFIRST(get_logger(), "Some warning that is not outputted the first time %d", 5);
RCLCPP_ERROR_EXPRESSION(get_logger(), is_some_error(), "Some error: %s", "something went wrong");
RCLCPP_FATAL_FUNCTION(get_logger(), &is_some_fatal_error, "Some fatal error: %s", "something went horribly wrong");

ROS2 bietet eine integrierte Loggingfunktionalität, die auch genutzt werden sollte, da nur so das Loglevel und -zeil von ROS gesetzt werden kann und die Messages auch auf /rosout publiziert werden. ROS2 stellt einige Makros dafür zur Verfügung, siehe ROS2 Doku. get_logger ist eine Methode von rclcpp::Node bzw. rclcpp_lifecycle::LifecycleNode.

Es existieren zu jedem der 5 Loglevel Makros, um eine Nachricht nur einmalig, erst ab dem 2. Aufruf oder abhängig von einem Prädikat auszugeben. Der Ausgabetext kann wie printf formatiert werden.

(Lifecycle) Publisher

Publisher erstellen

// component header:
rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr publisher;
// on_configure:
publisher = create_publisher<std_msgs::msg::String>("foo", 10);
// send data:
auto message = std_msgs::msg::String();
publisher_->publish(message);

Lifecycle Publisher aktivieren/deaktivieren/status prüfen

publisher->on_activate();
publisher->on_deactivate();
publisher->is_activated();

Ein Publisher publiziert Nachrichten eines Typs auf einem benannten Topic. Topics sind der normale Weg über den ROS Nodes kommunizieren. Die Node-Klasse bietet eine Templatemethode create_publisher um einen Publisher -- oder im Falle eines LifecycleNodes -- LifecyclePublisher zu erstellen. Die Methode benötigt als ersten Parameter den Namen des Topics auf das geschrieben werden soll und als zweiten Parameter die QoS Einstellungen oder eine maximale Tiefe der History, also wie viele Messages zwischengespeichert werden.

LifecyclePublisher müssen aktiviert werden, damit sie Nachrichten versenden. Dies geschieht mittels ensprechender Methoden in der on_activate und on_deactivate Methode.

Subscription

Subscription erstellen

// component header:
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription;
// on_configure:
using namespace std::placeholders;
using namespace rclcpp;
subscription = create_subscription<std_msgs::msg::String>("foo", QoS(KeepLast(10)), std::bind(&Thing::sub_handler, this, _1));
// message handler signature
void Thing::sub_handler(std_msgs::msg::String::UniquePtr msg);

Mittels einer Subscription können Nachrichten von einem Topic gelesen werden. Die Node-Klasse bietet eine Templatemethode create_subscription um eine subscription zu erstellen.

Die Methode benötigt als ersten Parameter den Namen des Topics von dem gelesen werden soll und als zweiten Parameter die QoS Einstellungen oder eine maximale Tiefe der History, also wie viele Messages zwischengespeichert werden. Die QoS Einstellungen sind deutlich vielfältiger als in ROS1: Historytiefe, Zuverlässigkeit und ob Nachrichten anhängig bleiben wenn kein Subscriber verbunden ist, lassen sich getrennt konfigurieren. Standardmäßig wird ein ähnliches Profil wie in ROS1 gewählt, also best effort und volatile Nachrichten (nicht anhängig) mit einer begrenzten Historytiefe. Der 3. Parameter ist eine Callbackfunktion, die für jede Nachricht, die an das Topic gesendet wird, aufgerufen wird.

Services

Hilfsfunktion um an einem Future einer Serviceresponse zu blocken

template <typename T, template <class> class Fut, typename Node>
T get_future(Node& node, Fut<T>& fut) {
    if (rclcpp::spin_until_future_complete(node.get_node_base_interface(), fut) ==
            rclcpp::executor::FutureReturnCode::SUCCESS) {
        return fut.get();
    }
    RCLCPP_ERROR(node.get_logger(), "Error getting result from future");
    return nullptr;
}

Actions

Action server erstellen

// component header:
#include <rclcpp_action/server.hpp>

using Pose = sp_trajectory_msgs::action::Pose;
using Goal = std::shared_ptr<const Pose::Goal>;
using GoalHandle = std::shared_ptr<rclcpp_action::ServerGoalHandle<Pose>>;

rclcpp_action::Server<Pose>::SharedPtr pose_server;

// on_configure:
using namespace std::placeholders;
pose_server = rclcpp_action::create_server<Pose>(
        get_node_base_interface(),
        get_node_clock_interface(),
        get_node_logging_interface(),
        get_node_waitables_interface(),
        "action/pose",
        std::bind(&TrajectoryClient::on_pose_goal, this, _1, _2),
        std::bind(&TrajectoryClient::on_pose_cancel, this, _1),
        std::bind(&TrajectoryClient::on_pose_accepted, this, _1));

// action handler signature
rclcpp_action::GoalResponse on_pose_goal(const rclcpp_action::GoalUUID & uuid, const Goal& goal);
rclcpp_action::CancelResponse on_pose_cancel(const GoalHandle& goal_handle);
void on_pose_accepted(const GoalHandle& goal_handle);

Action client erstellen

// component header:
#include <rclcpp_action/client.hpp>

using Pose = sp_trajectory_msgs::action::Pose;
using Goal = std::shared_ptr<const Pose::Goal>;
using Feedback = std::shared_ptr<const Pose::Feedback>;
using Result = rclcpp_action::ClientGoalHandle<Pose>::WrappedResult;
using GoalHandle = std::shared_ptr<rclcpp_action::ClientGoalHandle<Pose>>;

rclcpp_action::Client<Pose>::SharedPtr pose_client;

// on_configure:
pose_client = rclcpp_action::create_client<Pose>(
        get_node_base_interface(),
        get_node_clock_interface(),
        get_node_logging_interface(),
        get_node_waitables_interface(),
        "action/pose");

// wait for action server to become ready
bool b = pose_client->wait_for_action_server(10000ms); // timeout 10s

// neues goal senden:
Pose::Goal g = ...

using namespace std::placeholders;
rclcpp_action::Client<Pose>::SendGoalOptions options;
options.feedback_callback = std::bind(&ScriptNode::on_feedback, this, _1, _2);
options.goal_response_callback = ...
options.result_callback = ...

GoalHandle handle = pose_client->async_send_goal(g, options);
auto result = pose_client->async_get_result(handle);
pose_client->async_cancel_goal(handle);

// feedback handler signature:
void on_feedback(const GoalHandle&, const Feedback&);
void on_result(const Result&);

Actions werden für länger laufende Aufgaben verwendet, die zwischenzeitlich abgebrochen werden können oder von denen man Nachricht über den Fortschritt erhalten möchte. Ein Beispiel könnte das Anfliegen eines Zielpunkts sein. Ein Serviceaufruf blockt bis eine Antwort eintrifft und ist daher ungeeignet für diesen Anwendungsfall.

Eine Action benötigt daher im Vergleich zu einer Subscription auch mehrere Callbacks:

Neben den Callbacks und dem Topicnamen benötigt die create_server Funktion auch noch einige Features des unterliegenden Nodes (z.B. um publisher und subscriptions zu erstellen) und damit die base interfaces, wie im Beispiel gezeigt.

Leitet die eigene Klasse nicht von Node ab, kann stattdessen auch eine Referenz auf den Node übergeben werden.

Um ein neues Goal zu senden wird die Methode async_send_goal aufgerufen, die ein Handle zurückgibt, über den das Goal z.B. abgebrochen werden kann. Beim senden eines Goals müssen keine Callbacks angegeben werden, die SendGoalOptions sind ein optionaler Parameter. Die Methode async_get_result gibt ein Future-Objekt zurück, mit dem auf den Abschluss gewartet werden und das Ergebnis abgerufen werden kann. Alternativ können analog zu ROS1 mittels der SendGoalOptions Callbacks registriert werden, um benachrichtigt zu werden, sobald das Goal akzeptiert/abgewiesen wurde (goal_response_callback), Fortschritt gemacht wurde (feedback_callback) oder abgeschlossen wurde (result_callback).

Parameter

Parameter deklarieren

declare_parameter("name", rclcpp::ParameterValue(), rcl_interfaces::msg::ParameterDescriptor()
    .set__name("name")
    .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_STRING)
    .set__description("the name of the quadcopter/frame, this client receives messages for")
    .set__additional_constraints("must be a valid tf2 frame name"));

declare_parameter("quad_speed", 3.0, rcl_interfaces::msg::ParameterDescriptor()
    .set__name("quad_speed")
    .set__type(rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE)
    .set__description("max speed of the quadcopter")
    .set__floating_point_range({rcl_interfaces::msg::FloatingPointRange()
        .set__from_value(0.0)
        .set__to_value(10.0)
        .set__step(0.1)}));

Eine ROS2 Komponente kann über Parameter konfiguriert werden. Parameter werden normalerweise im Konstruktor des Nodes deklariert und in der on_configure Methode gelesen. Es kann auch ein Callback erstellt werden, der bei Paramteränderungen aufgerufen wird um Parameter zur Laufzeit anpassbar zu machen.

Ab ROS2 dashing müssen Parameter standardmäßig deklariert werden. Auf diese Weise kann leichter analysiert werden, welche Parameter ein Node überhaupt benötigt. Zu jedem Parameter kann dabei ein Descriptor hinterlegt werden, der zusätzliche Informationen wie eine Beschreibung oder mögliche Wertebereiche (IntegerRange/FloatingPointRange) angibt.

declare_parameter benötigt als ersten Parameter den Namen des Parameters. In ROS2 existiert kein zentraler Parameterserver, deshalb sind Parameter lokal jeweils einem Node zugeordnet. Der 2. Parameter gibt einen Defaultwert an. Soll kein Defaultwert existieren kann mit rclcpp::ParameterValue() ein uninitialisierter Parameterwert übergeben werden. Der 3. Parameter ist der ParameterDescriptor, in dem Metadaten für den Parameter hinterlegt werden können.

Parameter lesen

double quad_speed = 0.0;
if (rclcpp::Parameter p; get_parameter("quad_speed", p) &&
                         p.get_type() != rclcpp::ParameterType::PARAMETER_NOT_SET) {
    quad_speed = p.get_value<double>();
    RCLCPP_INFO(get_logger(), "Parameter \"quad_speed\" := %lf", quad_speed);
} else {
    RCLCPP_ERROR(get_logger(), "Parameter \"quad_speed\" not set!");
    return CallbackReturn::ERROR;
}

Parameter können mit get_parameter in der on_configure Methode gelesen werden. Dabei sollte sowohl der Rückgabewert beachtet werden, ob ein Parameter überhaupt gefunden und gelesen werden konnte, als auch der Typ geprüft werden, der bei ungesetzten Parametern PARAMETER_NOT_SET ist.

Callback wenn Parameter gesetzt wird

using namespace std::placeholders;
set_on_parameters_set_callback(std::bind(&Thing::on_parameters_set, this, _1));

on_parameters_set(const std::vector<rclcpp::Parameter>& pv) {
    for (const auto& p : pv) {
        if (p.get_name() == "bar")
            // the function is called before the parameter is set, so the constraints
            // must be checked manually
            if (p.as_int() <= 100000)
                count_ = p.as_int();
    }

    return rcl_interfaces::msg::SetParametersResult().set__successful(true);
}

Möchte man auch nach dem initialen Konfigurieren des Nodes Parameter setzen oder eigene Constraints auf Parametern prüfen, kann eine Callbackfunktion registriert werden, die vor jeder Änderung der Parameter gerufen wird. Diese signalisiert dann über ihren Rückgabewert, ob das setzen des Parameters erlaubt ist oder nicht.

Parameter von anderen Nodes lesen

Timer

Timer erstellen

#include <chrono>
// header
rclcpp::TimerBase::SharedPtr timer;
// on_configure
using namespace std::chrono_literals;
timer = create_wall_timer(20ms, std::bind(&TrajectoryClient::on_timer, this));
// callback
void on_timer();

Für viele Aufgaben, wie das periodische Verschicken von Nachrichten ist ein Timer hilfreich. Dieser kann mit create_wall_timer angelegt werden. Die übergebene Callbackfunktion wird periodisch mit dem gegebenen Intervall aufgerufen.

tf2

Beispiel für einen TransformListener

// header
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

std::unique_ptr<tf2_ros::Buffer> tf_buffer;
std::unique_ptr<tf2_ros::TransformListener> tf_listener;

// on_configure
tf_buffer = std::make_unique<tf2_ros::Buffer>(get_clock());
tf_listener = std::make_unique<tf2_ros::TransformListener>(*tf_buffer);

// on_cleanup
tf_listener.reset();
tf_buffer.reset();

Beispiel für einen TransformBroadcaster

// header
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> static_tf_broadcaster;

// on_configure
tf_broadcaster = std::make_unique<tf2_ros::TransformBroadcaster>();
static_tf_broadcaster = std::make_unique<tf2_ros::StaticTransformBroadcaster>();

// on_cleanup
tf_broadcaster.reset();
static_tf_broadcaster.reset();

Ein häufiger Anwendungsfall sind Koordinatentrasformationen. Jeder Roboter definiert normalerweise ein oder mehrere Koordinatensysteme, die sich auch über die Zeit ändern können. Das tf2 Paket enthält Klassen um Punkte, Verktoren, Posen etc einfach von einem Koordinatensystem (zu einem Zeitpunkt) in ein anderes Koordinatensystem (zu einem Zeitpunkt) überführen. Koordinatensysteme werden dabei über einen String identifiziert. Die History, also wie weit in die Vergangenheit Transformationen möglich sind, ist begrenzt (< 10s).

Details zu Design und Implementierung kann man auch im Paper zu tf finden.

Details zu den Koordinatensystemkonventionen sind im Abschnitt Optitrack zu finden.

Intern nutzt tf2 das Topic /tf bzw /tf_static um Änderungen an dynamischen und statischen Koordinatensystemen zu kommunizieren. Sobald ein Node mittels eines TransformBroadcasters eine Transformation zwischen zwei Koordinatensystemen veröffentlicht können andere Nodes, die einen TransformListener erstellt haben Punkte zwischen diesen Koordinatensystemen transformieren.

Tf2 kann wie aus ROS1 gewohnt in ROS2 verwendet werden. Zu beachten ist lediglich, dass der Buffer und TransformListener bzw TransformBroadcaster an den Lifecycle des Nodes angepasst werden müssen.

Die tf2 Objekte werden dazu hinter einem std::unique_ptr verborgen, die im on_configure Callback initialisiert werden. Im on_cleanup und on_shutdown Callback können die Pointer dann resettet werden.

tf2 in package einbinden

package.xml dependencies für tf2

<package format="3">
  ...
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
  ...
</package>

cmake konfiguration für tf2

find_package(tf2 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
...
ament_target_dependencies(my_component
    ...
    tf2
    tf2_ros
    tf2_geometry_msgs)

Um tf2 in einem eigenen package zu nutzen müssen die Abhängigkeiten zu tf2 und tf2_ros beide angegeben werden. Der Kern von tf2 ist unabhängig von ROS, deshalb wird zusätzlich das Paket tf2_ros benötigt.

tf2 nutzen

TransformBroadcaster Beispiele

geometry_msgs::msg::TransformStamped transform;
...
tf_broadcaster->sendTransform(transform);

TransformListener Beispiele

geometry_msgs::msg::PoseStamped p;
...
// transformiere p in das Koordinatensystem "world"
auto p_world = tf_buffer->transform(p, "world");

// transformiere p in das Koordinatensystem "quad" vor einer Sekunde
auto p_quad = tf_buffer->transform(p, "quad", now() - 1s, "world");

Sobald ein TransformListener erstellt wurde können Daten beliebiger Typen transformiert werden. Voraussetzungen dafür ist eine implementierte doTransform Funktion für den konkreten Typen; der Header tf2_geometry_msgs.h implementiert die Funktion für wichtige Typen aus geometry_msgs.

Dabei können verschiedene Fehler zur Laufzeit auftreten:

Alle Fehler treten sehr leicht und häufig auf und müssen daher immer im Code behandelt werden. Die Methode canTransform prüft, ob eine Transformation wie gewünscht möglich ist. Andernfalls schreibt die transform Methode eine Fehlermeldung in das Log.

Die erste Fehlerart kann leicht durch einen Schreibfehler im Namen des Frames entstehen oder falls sich der Frame auf ein getracktes Objekt bezieht, das nicht erkannt oder vorhanden ist.

Um die anderen Fehlerarten zu vermeiden, sollte genau darauf geachtet werden, dass der Zeitstempel des transformierten Objekts sowie die Zielzeit innerhalb der History des Buffers liegen.

Ist kein konkreter Zeitpunkt gewünscht, sondern soll die jeweils aktuellste Transformation verwendet werden kann tf2::TimePoint() als Zeitpunkt angegeben werden (ros::Time wurde durch Klassen aus std::chrono ersetzt, deshalb funktioniert ros::Time(0) nicht mehr).

Einige Hinweise zu Debuggen von tf2 Fehlern finden sich hier.

Nodes Testen

Libraries schreiben

Library erstellen

include_directories(include)

# Build library
add_library(rosslt SHARED
    src/trackingnode.cpp)

ament_target_dependencies(rosslt rclcpp rosslt_msgs)
ament_export_include_directories(include)

ament_export_interfaces(export_rosslt HAS_LIBRARY_TARGET)

install(
  DIRECTORY include/
  DESTINATION include
)

# Install library
install(TARGETS
  rosslt
  EXPORT export_rosslt
  ARCHIVE DESTINATION lib
  LIBRARY DESTINATION lib
  RUNTIME DESTINATION bin
  INCLUDES DESTINATION include
)

ament_package()

Gegen Library linken

add_executable(slt_talker src/slt_talker.cpp)
ament_target_dependencies(slt_talker rclcpp std_msgs rosslt_msgs rosslt_lib)
target_link_libraries(slt_talker rosslt_lib::rosslt)

Häufig ist es notwendig eine externe Library einzubinden oder eine Funktionalität mehreren ROS Komponenten verfügbar zu machen. Die Bibliothek kann wie gewöhnlich mit add_library gebaut werden und dann mit install Befehlen für die Headerdateien und Buildartefakte installiert werden. Es reicht dabei nicht aus nur im zweiten install-Befehl INCLUDES DESTINATION include anzugeben, da dies nur die Abhängigkeiten für Pakete definiert, die gegen die erstellte Bibliothek linken!

Messages, Services und Actions definieren

package.xml

  <build_depend>builtin_interfaces</build_depend>
  <build_depend>rosidl_default_generators</build_depend>
  <exec_depend>builtin_interfaces</exec_depend>
  <exec_depend>rosidl_default_runtime</exec_depend>

  <member_of_group>rosidl_interface_packages</member_of_group>

Messagedateien aus Beschreibungsdateien generieren

find_package(ament_cmake REQUIRED)
find_package(builtin_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(sp_trajectory_msgs
  "msg/MyMessage.msg"
  "srv/MyService.srv"
  "action/MyAction.action"
  DEPENDENCIES builtin_interfaces
)

Eigene Message-, Service- und Actiontypen können wie in ROS1 über Beschreibungsdateien definiert werden. Aus diesen werden dann entsprechende C/C++ Header sowie Python Dateien generiert. Messages, Services und Actions können beliebig geschachtelt werden und andere Messagetypen referenzieren.

Für eigene Messages (und Services/Actions) sollte ein separates Package erstellt werden, da oft viele andere Packages davon abhängen und ansonsten schnell zyklische Abhängigkeiten entstehen.

Eine entsprechende CMake-Datei benötigt eine Abhängigkeit zu rosidl_default_generators um die Interfaces generieren zu können. Dies geschieht wiederum durch Aufruf von rosidl_generate_interfaces.

ROS2: Nodes starten

Das launch System aus ROS1 wurde mit ROS2 komplett überarbeitet. launch ersetzt dabei roslaunch.

launch

launch xml

ROS 2 unterstützt wie roslaunch auch XML Launch-dateien. Diese werden über das gleiche Tool bzw. über den gleichen ROS2 Command ausgeführt wie Python Launch-dateien. Es ist allerdings nötig oder empfohlen, die jeweiligen Launch-dateien auch mit ihrer Dateiendung zu versehen.

Ein paar nennenswerte notwendige Änderungen gegenüber ROS 1 XML Launch-dateien sind hier aufgez&aauml;hlt:

Lifecycle managen

ROS2: rqt

rqt ist das Standardwerkzeug um graphische Oberflächen für ROS2 zu entwickeln. Die Oberflächen werden in Form von Plugins entwickelt, die dynamisch in rqt gestartet und angeordnet werden können.

Plugins schreiben

Für ROS1 gibt es ein gutes Tutorial um Plugins zu entwickeln. Dieses Tutorial existiert noch nicht für ROS2. Stattdessen kann das InteractiveScript Plugin oder rqt_image_view als Vorbild genommen werden.

Typische (minimale) Verzeichnisstruktur eines rqt-Plugins

├── CMakeLists.txt
├── include
│   └── interactive_script
│       └── interactive_script_plugin.h
├── package.xml
├── plugin.xml
├── scripts
│   └── interactive_script
├── setup.py
└── src
    └── interactive_script_plugin
        ├── interactive_script_plugin.cpp
        └── interactive_script_plugin.ui

Das Listing zeigt eine typische Verzeichnisstruktur für ein rqt-Plugin. Im Unterschied zu einem normalen ROS-Paket ist eine zusätzliche plugin.xml und ein Python-Script (+ setup.py) vorhanden um das Plugin standalone zu starten. Im folgenden wird von der gezeigten Verzeichnisstruktur ausgegangen.

package.xml

package.xml für rqt-Plugin

<?xml version="1.0"?>
<package format="2">
  <name>interactive_script</name>
  <description>The interactive_script package</description>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>rqt_gui</depend>
  <depend>rqt_gui_cpp</depend>
  <depend>sp_trajectory_msgs</depend>

  <export>
    <build_type>ament_cmake</build_type>
    <!-- Other tools can request additional information be placed here -->
    <rqt_gui plugin="${prefix}/plugin.xml"/>
  </export>

</package>

Ein Plugin wird ganz normal im ROS-Workspace gebaut und registriert sich über die pluginlib bei rqt_gui. Dazu sind in der package.xml mindestens die rechts gezeigten Abhängigkeiten nötig. Zusätzlich muss im export-Tag angegeben werden, wo sich die plugin.xml Datei, die das Plugin zum registrieren bei rqt_gui beschreibt, befindet.

plugin.xml

plugin.xml für rqt-Plugin

<library path="interactive_script">
  <class name="interactive_script_plugin/InteractiveScriptGui" type="interactive_script_plugin::InteractiveScriptGui" base_class_type="rqt_gui_cpp::Plugin">
    <description>
      An example C++ GUI plugin to create a great user interface.
    </description>
    <qtgui>
      <group>
        <label>Topics</label>
        <icon type="theme">folder</icon>
        <statustip>Plugins related to ROS topics.</statustip>
      </group>
      <label>Interactive Script</label>
      <icon type="theme">system-help</icon>
      <statustip>Great user interface to provide real value.</statustip>
    </qtgui>
  </class>
</library>

Die Plugin xml enthält hauptsächlich Informationen, welche Klasse geladen werden soll, wo das Plugin gefunden werden kann und wo im Menü es auftauchen soll. Der library path hängt vom angegebenen Installationsort in der CMakeLists.txt ab, class Name, Type und Basisklassentyp vom Namespace und dem Klassennamen des Plugins.

CMakeLists.txt

CMakeLists.txt für rqt-Plugin

cmake_minimum_required(VERSION 3.5)
project(interactive_script)

# Default to C11
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 11)
endif()

# Default to C++17
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 17)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rqt_gui_cpp REQUIRED)

find_package(Qt5 COMPONENTS Core Widgets REQUIRED)

# source files
set(interactive_script_plugin_SRCS
    src/interactive_script_plugin/interactive_script_plugin.cpp
)

set(interactive_script_plugin_HDRS
    include/interactive_script/interactive_script_plugin.h
)

set(interactive_script_plugin_UIS
    src/interactive_script_plugin/interactive_script_plugin.ui
)

# includes and run qt moc
set(interactive_script_plugin_INCLUDE_DIRECTORIES
    include
    ${rclcpp_INCLUDE_DIRS}
    ${rqt_gui_cpp_INCLUDE_DIRS}
    ${Qt5Widgets_INCLUDE_DIRS}
)

qt5_wrap_cpp(interactive_script_plugin_MOCS ${interactive_script_plugin_HDRS})
qt5_wrap_ui(interactive_script_plugin_UIS_H ${interactive_script_plugin_UIS})

include_directories(
  ${interactive_script_plugin_INCLUDE_DIRECTORIES}
)

set(CMAKE_INCLUDE_CURRENT_DIR ON)

# build plugin
add_library(${PROJECT_NAME} SHARED
   ${interactive_script_plugin_SRCS}
   ${interactive_script_plugin_MOCS}
   ${interactive_script_plugin_UIS_H}
)

target_link_libraries(${PROJECT_NAME}
    ${rclcpp_LIBRARIES}
    ${rqt_gui_LIBRARIES}
    ${rqt_gui_cpp_LIBRARIES}
    Qt5::Widgets Qt5::Core
    MiniLua
)

# Install
install(TARGETS ${PROJECT_NAME}
  EXPORT ${PROJECT_NAME}
  ARCHIVE DESTINATION lib/${PROJECT_NAME}
  LIBRARY DESTINATION lib/${PROJECT_NAME}
  RUNTIME DESTINATION bin/${PROJECT_NAME}
  INCLUDES DESTINATION include
)

install(PROGRAMS scripts/interactive_script
  DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY
  include/
  DESTINATION include
)

install(FILES plugin.xml
  DESTINATION share/${PROJECT_NAME}
)

# register plugin
pluginlib_export_plugin_description_file(rqt_gui "plugin.xml")

ament_export_include_directories(include)
ament_export_libraries(${PROJECT_NAME})

ament_package()

Die CMakeLists.txt für ein rqt-Plugin ist etwas komplexer als für einen normalen Node, da zusätzlich der Qt Metaobject Compiler ausgeführt werden muss und das Plugin registriert wird. Es existiert kein fertiges Makro zum Bauen des Plugins, d.h. es muss mit add_library und target_link_libraries gearbeitet werden und alle ROS2-Abhängigkeiten beim linken und inkludieren manuell hinzugefügt werden.

Die Grundlegende zusätzliche Abhängigkeit für ein Plugin ist rqt_gui_cpp. Um das Python-Skript zu installieren wird zusätzlich noch ament_cmake_python benötigt.

Der moc wird über die Funktionen qt5_wrap_cpp und qt5_wrap_ui gestartet und generiert zusätzliche Header zB aus den ui Dateien.

Bei der Installation muss die plugin.xml in das share-Verzeichnis kopiert werden und nochmal mit pluginlib_export_plugin_description_file registriert werden.

Plugin schreiben

Plugin Header Beispiel

#include <rqt_gui_cpp/plugin.h>
#include <rclcpp/rclcpp.hpp>
#include <ui_interactive_script_plugin.h>
#include <QWidget>

namespace interactive_script_plugin {
class InteractiveScriptGui : public rqt_gui_cpp::Plugin
{ Q_OBJECT
public:
    InteractiveScriptGui() {
        setObjectName("InteractiveScript");
    }

    virtual void initPlugin(qt_gui_cpp::PluginContext& context) override {
        assert(node_);    
        ui_.setupUi(widget_);
    }

    virtual void shutdownPlugin() override;
    virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const override;
    virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings) override;

    Ui::InteractiveScriptWidget ui_;
private:
    QWidget* widget_ = nullptr;
};
}  // namespace rqt_graph_editor

Nach diesen ganzen Vorbereitungen kann das Plugin selbst implementiert werden. Das Plugin leitet hierzu von der Klasse rqt_gui_cpp::Plugin ab. Die Initialisierung des Plugins findet in der initPlugin Methode statt. Hier wird auch die Gui aus der ui-Datei mittels setupUI erzeugt. Um Verbindungen zu anderen Nodes aufzubauen muss nicht jedes Plugin selbst einen Node erzeugen. Stattdessen existiert ein zwischen den Plugins geteilter Node, auf den das Plugin einen shared_ptr (node_) besitzt.

Standalone Plugin

Standalone Plugin Skript

#!/usr/bin/env python3
import sys
from rqt_gui.main import Main

main = Main()
sys.exit(main.main(sys.argv, standalone='interactive_script_plugin/InteractiveScriptGui'))

setup.py

#!/usr/bin/env python

from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup

d = generate_distutils_setup(
    packages=['interactive_script'],
    package_dir={'': 'src'},
    scripts=['scripts/interactive_script']
)

setup(**d)

Ähnlich wie bei rqt_graph oder rqt_bag soll es möglich sein, das Plugin direkt als Standaloneanwendung zu starten. Ein ensprechendes Python-Skript ist ein Einzeiler und beispielhaft angegeben. Zusätzlich wird eine setup.py und die Abhängigkeit zu ament-python benötigt. Das Pythonskript muss im Installationsschritt an die richtige Position kopiert werden.

Wenn alles geklappt hat kann das Plugin ganz normal mit ros2 run ausgeführt werden.

ROS2: rviz2

RViz ist ein Visualisierungstool, das unter anderem den Ursprung von Koordinatensystemen oder Marker Messages beliebiger ROS Nodes in einer 3D view anzeigen kann.

Marker

Felder einer Marker-Message (gekürzt)

Header header                    # timestamp/frame_id
string ns
int32 id                         # ns/id geben dem Objekt eine unique ID
int32 type                       # ARROW, CUBE, SPHERE, CYLINDER, LINE_STRIP, TEXT_VIEW_FACING, ...
int32 action                     # ADD, DELETE, DELETEALL
geometry_msgs/Pose pose          # Pose des Objekts (Koordinatensystem im Header)
geometry_msgs/Vector3 scale      # Skalierung
std_msgs/ColorRGBA color         # Farbe
builtin_interfaces/Duration lifetime
bool frame_locked                # Pose des Objekts mit dem Frame bewegen
geometry_msgs/Point[] points     # Punkte für LINE_STRIP etc
std_msgs/ColorRGBA[] colors      # Farbe für LINE_STRIP etc
string text                      # Text für TEXT_VIEW_FACING

Der wichtigste Messagetyp, der von RViz verarbeitet wird ist visualization_msgs/Marker. Marker können verschiedene Formen (Quader, Zylinder, Text, Polygone, …) haben und es lassen sich beliebig platzieren. RViz (oder beliebige andere kompatible Programme, zB mit AR) kann Messages dieses Typs empfangen und entsprechend anzeigen. So lassen sich beliebige (nicht interaktive) 3D Visualisierungen erstellen.

Soll die Visualisierung stattdessen interaktiv sein, können stattdessen interaktive Marker verwendet werden.

Interactive Marker

Felder einer InteractiveMarker-Message

Header header                    # timestamp/frame_id
geometry_msgs/Pose pose          # Initial pose. Also, defines the pivot point for rotations.
string name                      # unique identifying string
string description               # kurze Beschreibung (z.B. für Tooltip)
float32 scale                    # Scale to be used for default controls (default=1).
MenuEntry[] menu_entries         # Kontextmenüeinträge für den Marker
InteractiveMarkerControl[] controls # List of controls displayed for this marker.

InteractiveMarkerServer

// component Header
#include <interactive_markers/interactive_marker_server.hpp>

std::unique_ptr<interactive_markers::InteractiveMarkerServer> marker_server;

// feedback callback
void marker_callback(const std::shared_ptr<visualization_msgs::msg::InteractiveMarkerFeedback>& feedback) {
    if (feedback->event_type == visualization_msgs::msg::InteractiveMarkerFeedback::MOUSE_UP) {
        auto new_x = feedback->pose.position.x;
    }
}

// on_configure
marker_server = std::make_unique<interactive_markers::InteractiveMarkerServer>("simple_marker",
    get_node_base_interface(),
    get_node_clock_interface(),
    get_node_logging_interface(),
    get_node_topics_interface(),
    get_node_services_interface());

// interaktiven Marker erstellen und senden
visualization_msgs::msg::InteractiveMarker int_marker;
int_marker.header.frame_id = "world";
int_marker.header.stamp = now();
int_marker.name = "marker_12345";
int_marker.pose = 

// Aussehen des Markers festlegen
visualization_msgs::msg::Marker box_marker; // Form, Farbe, Scale etc festlegen
                                            // Pose ist relativ zu int_marker!

// Die Form als nicht interaktive Control dem InteractiveMarker hinzufügen
visualization_msgs::msg::InteractiveMarkerControl box_control;
box_control.always_visible = true;
box_control.markers.push_back(box_marker);

// Control zum Verschieben erstellen
visualization_msgs::msg::InteractiveMarkerControl control;
control.name = "move_x";
control.orientation.w = 1;
control.orientation.x = 1;
control.orientation.y = 0;
control.orientation.z = 0;
control.interaction_mode = visualization_msgs::msg::InteractiveMarkerControl::MOVE_AXIS;
int_marker.controls.push_back(control);

marker_server->insert(int_marker, marker_callback);
marker_server->applyChanges();

In vielen Fällen sollen die Visualisierungen auch die Möglichkeit bieten aktiv Einstellungen etc vorzunehmen. In diesen Fällen können InteractiveMarkers verwendet werden um Controls (zB zum Verschieben oder Kontextmenüs) normalen Markern zuzuweisen und einen Callback als Reaktion zu registrieren.

Interactive Marker Structure

Dies geschieht ähnlich wie bei Actions über eine Klasse interactive_markers::InteractiveMarkerServer (package interactive_markers), die intern mehrere Topicverbindungen zu entsprechenden Clients (zB in RViz) aufbaut.

Interactive Marker Architecture

Der Server speichert intern eine Liste von Markern, die jeweils beliebig viele Controls zugewiesen bekommen können. Diese Liste wird nur beim Aufruf der applyChanges Methode mit dem Client abgeglichen. Der übliche Ablauf sieht also so aus, dass zunächst Marker erstellt werden, diese dann mit insert insert in die Markerliste übernommen werden und dann mit applyChanges gesammelt an den Client übertragen werden.

Interaction Modes

enum visualization_msgs::msg::InteractiveMarkerControl {NONE, MENU, BUTTON, MOVE_AXIS,
    MOVE_PLANE, ROTATE_AXIS, MOVE_ROTATE, MOVE_3D, ROTATE_3D, MOVE_ROTATE_3D};

Ein InteractiveMarker ist zunächst nicht sichtbar. Es muss immer ein Control erstellt werden mit dem interaction_mode visualization_msgs::msg::InteractiveMarkerControl::NONE (der default). Den markers können dann die sichbaren Formen hinzugefügt werden (siehe Listing). Werden keine Marker hinzugefügt, wird ein zum interaction_mode passender Standardmarker (für MOVE_AXIS beispielsweise ein Pfeil) verwendet.

Für Controls, deren interaction_mode sich auf eine Achse, wird diese Achse über die Orientierung des Controls festgelegt.

Felder einer InteractiveMarkerFeedback-Message (gekürzt)

Header header                    # Timestamp, Koordinatenframe
string marker_name               # identifiziert den Marker und Control
string control_name
uint8 event_type {KEEP_ALIVE, POSE_UPDATE, MENU_SELECT, BUTTON_CLICK, MOUSE_UP, MOUSE_DOWN}
geometry_msgs/Pose pose          # aktuelle Pose des Markers
uint32 menu_entry_id             # ausgewählter Eintrag bei MENU_SELECT

Beim Hinzufügen des InteractiveMarkers zum Server wird eine Callbackfunktion aufgerufen, die als Parameter ein InteractiveMarkerFeedback erhält. Dieses Feedback enthält den Namen des Markers und Controls, das auslösende Event und die Pose des Markers (die sich zB durch Verschieben geändert haben kann).

ROS2: Introspection

ROS2 bietet umfangreiche Möglichkeiten Informationen über den Node-Graph sowie launch Dateien zu erhalten.

Graph

launch

ROS2: ROS1 / ROS2 verbinden

ROS2: Best Practices

Nodes portieren

ROS2: Roadmap

  1. Abhängigkeiten klären und korrigieren in allen noch relevanten packages
  2. ROS1 packages mit colcon bauen
  3. CI build für alle packages einrichten
  4. Build von catkin nach ament_cmake umstellen
  5. Code an ros2 APIs anpassen
  6. Refactoring in lifecycle/component Node
Package Abhängigkeiten Colcon build CI Build CI Test ros2 Port Lifecycle/Component Anmerkungen
rosslt
blockly keine CMakeLists, Port nicht notwendig, deprecated -> interactive_script_blockly
gesture-node msgs und node noch nicht getrennt
hector_uav_msgs 3rd party
interactive_script
MiniLua kein catkin Projekt
quad_common_utils
quad_script
quad_state
sim_photo
trajectory_server deprecated

SE Projekte: Trajektorienplanung und Kollisionsvermeidung

Allgemeines

Projektbeschreibung

Ziele

Für das Quadcopterlabor des Instituts für Softwaretechnik und Programmiersprachen soll eine neue Trajektorienplanung und Kollisionsvermeidung implementiert werden. Diese soll physikalisch umsetzbare Trajektorien von beliebigen Start- in beliebige Zielzustände generieren können und anhand parametrisierbarer Optimierungskriterien (zB kürzeste Dauer, geringste Beschleunigung) möglichst optimale Trajektorien auswählen. Zusätzlich soll die Trajektorienplaung in der Lage sein dynamischen und statischen Hindernissen auszuweichen und mehrere (<10, potentiell verschiedene) Quadcopter kollisionsfrei koordinieren können. Die benötigte Zeit zur Planung einer Trajektorie soll im Durchschnitt weniger als 20ms benötigen und in 95% der Fälle weniger als 40ms (Thomas W.: Die genauen Zahlen können wir noch diskutieren). Zur Ansteuerung des Saphira Quadcoptermodells soll ein Treibernode implementiert werden, das kompatibel zur neuen Trajektorienplanung ist und die Steuerbefehle an die Regler der Drohne weiterleitet. Sämtliche Artefakte sollen in ausreichendem Maß dokumentiert und durch automatische und manuelle Tests qualitätsgesichert werden.

Architektur

Architektur alt

Architektur und Schnittstellen der neuen Trajektorienplanung

Architektur alt

Konzept Wegplanung

Die Wegplanung basiert auf dem A*-Algorithmus. Als Kostenfunktion f(x) = g(x) + h(x) wird die Summe aus den bisherigen Kosten g und den geschätzten Kosten bis zum Ziel h verwendet. Die Funktion g ist dabei definiert als g(start_knoten) = 0 und g(x) = g(vorgänger_von_x) + Abstand von x zu vorgänger_von_x. Die Funktion h ist definiert als h(x) = Abstand von x zu ziel_knoten. Durch diese Definition ist die Heuristik h zulässig und monoton. Da die Heuristik h monoton ist, kann der A*-Algorithmus als Instanz des Graph-Search-Algorithmus implementiert werden, da die mehrfache Expansion von Knoten nicht nötig ist. Im Schritt der Expansion wird erst überprüft, ob das Ziel direkt angeflogen werden kann. Falls dies der Fall ist, wird nur der Zielknoten zur Fringe hinzugefügt. Falls das Ziel nicht direkt angeflogen werden kann, werden alle Eckpunkte von bekannten Obstacles (+Sicherheitsabstand) betrachtet und diejenigen als Knoten zur Fringe hinzugefügt, die direkt angeflogen werden können und nicht bereits expandiert wurden. Als Obstacles werden derzeit nur Quader unterstützt (CUBOID). Die Wegplanung basiert nur auf der 2-dimensionalen x-y-Ebene. Es werden also keine Pfade über oder unter Obstacles geplant.

rapid_quadropter_trajectories

se_trajectory_schedule

se_trajectory_server

se_trajectory_client

se_visualization

se_quad_node

SE Projekte: Simulation

Einführung

Simulator Workspace (hector) einrichten

#!/bin/bash

# set ROS DISTRO
ROS_DISTRO=melodic

# install some (probably not all) dependencies
sudo apt install ros-$ROS_DISTRO-geographic-msgs ros-$ROS_DISTRO-joy-teleop ros-$ROS_DISTRO-joy ros-$ROS_DISTRO-joy-teleop python3-colcon-common-extensions python3-colcon-common-extensions lua5.3-dev

# create workspace
mkdir -p colcon_workspace/src
cd colcon_workspace/src

# clone repositories
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/bebop_autonomy.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/bebop_position_control.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/gesture-node.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/hector_gazebo.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/hector_localization.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/hector_quadrotor.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/parrot_arsdk.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/quad_common_utils.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/quad_obstacle.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/quad_script.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/quad_state.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/se-simulator.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/se_trajectory_msgs.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/se-trajektorien.git
git clone git@spgit.informatik.uni-ulm.de:quadrocopter/sim_photo.git

# build necessary packages for the hector simulation and the demo script
source /opt/ros/$ROS_DISTRO/setup.bash
cd ..
colcon build --packages-up-to sim_common
colcon build --packages-up-to hector_bebop_interface
colcon build --packages-up-to hector_quadrotor_description
colcon build --packages-up-to hector_quadrotor_gazebo
colcon build --packages-up-to hector_bebop_controllers
colcon build --packages-up-to se_trajectory_launch
colcon build --packages-up-to gazebo_kill_node

source install/local_setup.bash

# start simulation

# start gazebo and load quadlab world
roslaunch sim_common sim_se_server.launch ns:=/start_hector &
PROCESSID=$$
echo "spawned process $PROCESSID"
# spawn quadcopter and start demo
roslaunch hector_bebop_interface test_takeoff_travel_land.launch

echo "kill se Server"
kill -9 $PROCESSID
# Sollte Gazebo trozdem crashen:
# Gazebo zuerst in einem Terminal starten und anschliesend in einem anderen Terminal die simulation starten 
#

#Terminal A
#cd /install/dir/
source /opt/ros/melodic/setup.bash
source install/local_setup.bash
roslaunch sim_common sim_se_server.launch ns:=/start_hector

#Terminal B
#cd /install/dir/
source /opt/ros/melodic/setup.bash
source install/local_setup.bash
roslaunch hector_bebop_interface test_takeoff_travel_land.launch

Im Rahmen des se-simulator-Projekts wurde parallel mit 2 verschiedenen Simulationsumgebungen gearbeitet: BebopS und Hector. Der Grund für diese Redundanz war die Absicherung gegen Sackgassen, die uns an der Weiterentwicklung auf einem der beiden Wege hindern würden.

Da uns aber in beiden Fällen (noch) keine "Showstopper" begegnet sind, wurde bis zuletzt parallel an Hector und BebopS entwickelt.

Struktur se-simulator

Das se-simulator Repository beherbergt sowohl BebopS als auch Hector, weshalb die Struktur nicht ganz intuitiv ist. Rechts sieht man den groben Aufbau des Repositorys (vereinfachte Darstellung).

se-simulator
├── bebop_sim
   ├── BebopS
      ├── launch
         ├── bebopS.launch
         └── bebopS_script.launch
      └── ...
   ├── mav_comm
   └── rotors_simulator
├── hector_bebop_interface
   ├── launch
      ├── hector.launch
      └── hector_script.launch
   └── ...
├── hector_bebop_test
├── ...
└── sim_common
    ├── launch
       ├── gazebo.launch
       └── sim_se_server.launch
    ├── meshes
    ├── models
       ├── hardhat
       ├── net_static
       └── table_uni
    └── worlds
        └── drone_lab.world

Die Packagenamen (in der jeweilgen package.xml) entsprechen immer den Ordnernamen, leider mit Ausnahme von BebopS, dessen Package bebop_simulator heißt.

Die Packages BebopS (bzw. bebop_simulator) und hector_bebop_interface sind die Packages für die beiden unabhängigen Simulationen. Das Package sim_common beinhaltet (non-source) Dateien, die bei Hector und BebopS identisch sind und somit zusammengefasst werden können.

Befehle bei der echten Bebop, und deren Korrelate bei Hector / BebopS:

Befehl Topic Hector BebopS
Land land ja ja
Takeoff takeoff ja ja
Emergency reset nein nein
Piloting cmd_vel ja nein
Moving the Virtual Camera nein nein
Flight Animations (flipping) nein nein
Take on-board Snapshot snapshot nein nein
Set camera exposure set_exposure nein nein
Toggle on-board Video Recording record nein nein
Camera image_raw nein ja, aber nicht identisch zu Bebop
Odometry odom nein ja, aber anderes Topic
GPS fix nein nein
Joint States joint_states nein nein
TF tf ja, aber nicht so ja, aber nicht so
States (Battery) ? ja nein
States (Flightstatus ? ja nein

Gazebo

Gazebo ist eine Open Source Simulationsumgebung für Robotikanwendungen. Es enthält ODE als Physikengine, und spielt relativ gut mit ROS zusammen. In Gazebo kann man auch Sensoren simulieren, die ihre Umwelt so wahrnehmen wie ihre Vorbilder (z.B. Entfernungssensor). Im se-simulator wurde Gazebo für die Simulation benutzt.

Gazebo Plugin Entwicklung & Gazebo Topics

Beispiel Gazebo-Plugin

#include <gazebo/gazebo.hh>

namespace gazebo
{
  class MyWorldPlugin : public WorldPlugin
  {
    public: MyWorldPlugin() : WorldPlugin()
            {
              printf("Hello World!\n");
            }

    public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
            {
              // set up topic subscriptions and callbacks here
            }
  };
  GZ_REGISTER_WORLD_PLUGIN(MyWorldPlugin)
}

Um mit Gazebo zu interagieren, muss man ein Plugin erstellen, Gazebo bietet verschiedene Arten von Plugins für verschiedene Zwecke, wobei in unserem Anwendungsfall meistens sogenannte World Plugins benutzt werden. Dazu erstellt man am besten ein eigenes Catkin Package und geht dann so vor wie hier beschrieben.

Dabei ist es hilfreich zu wissen, dass Gazebo sein eigenes System zum Transport von Messages bereitstellt, welches zwar ROS sehr ähnelt, aber dennoch komplett unabhängig davon agiert. Deshalb ist es manchmal nötig, ein Gazebo-Plugin zu schreiben, welches einfach Gazebo-Nachrichten in ROS-Nachrichten "übersetzt", damit ROS etwas davon mitbekommt.

Zwei Beispiele für eher einfache Gazebo-Plugins sind das Collision-Plugin und das Obstacle-Plugin:.

Gazebo Collision Plugin

Das collision_plugin (unter se-simulator/collision_plugin/) kümmert sich um die Behandlung von Kollisionen in Gazebo. Dabei wird in plugin.cpp einfach nur auf das Gazebo-Topic ~/physics/contacts subscribed und eingehende Nachrichten auf dem ROS-Topic wall_contacts mit einem vereinfachten Nachrichtentypen gepublished.

Ein separater ROS-Node gazebo_collision_node.cpp übernimmt die tatsächliche Behandlung einer solchen Kollision, in unserem Fall wird eine Nachricht auf dem reset Topic verschickt, was je nach Drohnencontroller z.B. ein Ausschalten der Motoren auslöst bzw. generell als Emergency-Signal benutzt wird.

Gazebo Obstacle Plugin

Das obstacle_plugin (unter se-simulator/obstacle_plugin/) sort dafür, dass statische und dynamische Hindernisse auch in Gazebo angezeigt werden. Dazu werden die statischen und dynamischen Hindernisse aus den jeweiligen Topics (/start_collision_test/static_obstacles und /start_bebop/optitrack/dyn_points_output) ausgelesen.

Da im Labor meist Helme als dynamische Hindernisse verwendet werden, wird ein tatsächliches Helm-Modell benutzt, falls der Hindernis-Name das Wort "helm" enthält. Die Position der Dynamischen Hindernisse wird mit jedem Update des dyn_points_output-Topics aktualisiert. Die dynamischen Hindernisse sind in Gazebo auch mit der static-Eigenschaft versehen, da es sich sonst zwischen den Updates gemäß der simulierten Physik verhält und z.B. zu Boden fällt, was nicht wünschenswert ist.

Gazebo Kill Node

Da Gazebo sich nicht in angemessener Zeit schließen lässt und letztendlich sowieso durch ROS gewaltvoll beendet wird, gibt es die gazebo_kill_node. Die einzige Aufgabe dieser Node ist es, auf das Beenden von ROS zu warten und Gazebo dann über Terminal Befehle (killall) zu schließen. Der Nutzer kann hierbei durch Parameter beim Starten entscheiden, ob nur der Server oder der Client, oder beide geschlossen werden sollen und ob mit SIGTERM oder SIGKILL. Es muss beachtet werden, dass Gazebo auf der selben Maschine gestartet ist oder wird, wie die Gazebo Kill Node, da killall sonst keinen Effekt hat.

Parameter

Name Typ Defaultwert Beschreibung
~sigkill bool false true, falls Gazebo mittels SIGKILL geschlossen werden soll, false falls Gazebo mittels SIGTERM geschlossen werden soll
~server bool true true, falls der Server geschlossen werden soll, false, falls der Server nicht geschlossen werden soll
~client bool true true, falls der Client geschlossen werden soll, false, falls der Client nicht geschlossen werden soll

Topics/Services

keine

BebopS

BebopS ist OpenSource und für Ubuntu 16.04 und ROS Kinetic verfügbar. In der readme wird auf die Repos mav_comm und rotors_simulation verwiesen, welche ein Fork von der ETH etaz-asl sind. Um BebopS mit Ubuntu 18.04 und ROS Melodic zu verwenden, nutzen wir die Repos von der ETH (mav_comm, rotors_simulation). In diesen zwei Repos wurden keine Änderungen vorgenommen.

Um BebopS unter Ubuntu 18.04 und ROS Melodic zu laufen zu bringen, mussten ein paar Änderungen vorgenommen werden. Desweiteren musste man den Code des Controllers in die bisherige Umgebung einbauen.

BebopS Controller

Veranschaulicht den groben Aufbau des Controllers um sich besser zurechtzufinden und einen überblickt über den Informationsfluss zu haben.

alt text

BebopS in Gazebo

Beispiel der Einstellungen

    <physics name='default_physics' default='0' type='ode'>
      <gravity>0 0 -9.8066</gravity>
      <ode>
        <solver>
          <type>quick</type>
          <iters>10</iters>
          <sor>1.3</sor>
          <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
        </solver>
        <constraints>
          <cfm>0</cfm>
          <erp>0.2</erp>
          <contact_max_correcting_vel>100</contact_max_correcting_vel>
          <contact_surface_layer>0.001</contact_surface_layer>
        </constraints>
      </ode>
      <max_step_size>0.001</max_step_size>
      <real_time_update_rate>900</real_time_update_rate>
      <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
    </physics>

Einstellungen in Gazebo um mit BebopS fliegen zu können. In der *.world werden die Einstellung für Gazebo eingestellt. Wir hatten häufig das Problem, das uns die Drohne "abgehauen" ist. Eine Fehlerursache war eine zu hohe max_step_size. Dies liegt daran, dass der innere Controller alle 0.001 Sekunden neu berechnen will. Bei zu hoher Step Size übersteuert der Controller, da er von einer Schrittgröße von 0.001 Sekunden ausgeht.

Die Physikparameter zusammengefasst:

Hier mehr dazu.

Aufbau der launch Files

Um die BebopS in Gazebo nutzen zu können, muss man die Launch Datei includen. Zusätzlich ist es notwendig eine Gazebo Instanz zu starten. Eine launch File zum starten der Simulation liegt z.B. in bebops_launch/launch. Es können auch mehrere BebopS in das Launch File eingebunden werden. Einfach beliebig häufig den include machen. Dabei muss man der neuen Drohne einen neuen Namen geben, sonst kann man die Simulation nicht starten.

Include der Launch Datei

 <include file="$(find bebops_launch)/launch/bebopS/bebopS.launch" >
        <!-- Startposition -->
        <arg name="x" value="0.0" /> 
        <arg name="y" value="1.0" />
        <!-- Drone namespace -->
        <arg name="ns" value="$(arg ns)/Spearow"/> 
        <!-- Lua script, commands for the drone -->
        <arg name="script_path" value="$(find bebops_launch)/script/Spearow.lua" /> 
        <!-- Trajectory Server and client settings --> 
        <arg name="trajectory_request" value="$(arg trajectory_request)"/> 
        <arg name="trajectory_assignment" value="$(arg trajectory_assignment)"/>
        <!-- untested, to start the drone with the xbox controller --> 
        <arg name="joy_topic" value="joy"/> 
        <arg name="joy_config" value="xbox"/>
        <!-- namepace of the parent launch file -->
        <arg name="group_ns" value="$(arg ns)" /> 
        <!-- Name of the drone. We use normally pokemon names -->
        <arg name="quad_name" value="Spearow"/> 
        <!-- the maximum speed for the trajectory planning, the later speed may be lower -->
        <arg name="quad_speed" value="0.9" /> 
        <!-- config file for the controller. 
                It is useful to see the difference of trained and untrained parameters -->
        <arg name="controller_params" value="$(find bebop_simulator)/resource/controller_bebop_default_values.yaml" /> 
    </include>

Start Gazebo

<env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find bebops_launch)/models"/>
<env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find bebop_simulator)/models"/>
<!-- This file stays unchanged -->
<include file="$(find bebops_launch)/worlds/empty_world.launch"> 
<!-- World file, e.g. to laod the world with the lab. the real time factor is better without the lab -->
   <arg name="world_name" value="$(find bebops_launch)/worlds/drone_lab.world" /> 
   <arg name="paused" value="false"/> 
   <!-- true or false in the argument of the curernt launch file. The argument is used in the test files to 
        start gazebo headless --> 
</include>
<arg name="gui" value="$(arg gazebo_gui)"/> 

Besonderheiten

Bei einem land, takeoff und moveto ist darauf zu achten, dass die Befehle vollständig ausgeführt werden. Vor allem beim takeoff und moveto kommt es sonst zu Problemen, da der Fehler im Controller zu groß wird und die Drohne dann übersteuert und ins Netz fliegt.

Beispiele

 -- example for land 
 land()
 sleep(7)

 -- example for takeoff 
 takeoff()
 sleep(7)

 -- example for moveto
 moveto(1,1,1)
 wait(0.1) 

Fitness Node

Beispiel Launch File

<rosparam command="load" file="$(find fitness)/config/bebopS/evo_params.yaml" />
<group if="$(arg learn_PD)">
            <node pkg="fitness" type="evo_node" name="evo_alg_node" output="screen" />
</group>

Parameter

Name Typ Defaultwert Beschreibung
~drone_name string "" Name der Drohne
~ns string "" Namespace

Zusätzlich werden die Controller Paremter, welche in dem Argument controller_params geladen. Desweiteren wird noch das Config File für den Algorithmus geladen.

Topics/Services

Name Typ Art Beschreibung
update_params std_msgs/Empty Publication Parameter wurden aktualsiert und können nun vom Controller neu geladen werden
current_error se_trajectory_msgs/Error Subscription Zielfunktionen die minimiert werden sollen. Mit dem Erhalt der Nachricht, werden die Parameter mutiert

Controller trainieren

Die Controller Parameter von BebopS können mit der Simulation trainiert werden.

Aktivieren des Parametertraining und setzen der Parameter

<include file="$(find bebops_launch)/launch/bebopS/bebopS.launch" >
    <arg name="x" value="0.0" />
    <arg name="y" value="-1.0" />
    <arg name="ns" value="$(arg ns)/Pidgey"/>
    <arg name="script_path" value="$(find bebops_launch)/script/Pidgey.lua" />
    <arg name="trajectory_request" value="$(arg trajectory_request)"/>
    <arg name="trajectory_assignment" value="$(arg trajectory_assignment)"/>
    <arg name="joy_topic" value="joy"/>
    <arg name="joy_config" value="xbox"/>
    <arg name="group_ns" value="$(arg ns)" />
    <arg name="quad_name" value="Pidgey"/>
    <arg name="quad_speed" value="0.9" />
    <arg name="learn_PD" value="false" />
    <arg name="controller_params" value="$(find bebop_simulator)/resource/controller_bebop.yaml" />
</include>

Dazu wird das Argument learn_PD auf true gesetzt. Im Argument controller_params können auch die verwendeten Parameter für den Controller geändert werden. Damit kann man jede Drohne mit anderen Parameter fliegen.

Welche Parameter trainiert werden

Es ist nicht ganz klar welche Parameter in der controller_bebop.yaml im Paket BebopS für den P und D Teil des Controllers veranwortlich sind. Nach vielen Versuchen und Beobachtungen, werden U_xyz als P und beta_[phi|theta|psi] Teil angenommen.

Bei den ursprünglichen Werte des Controllers unterscheiden sich die Werte von U_xy und U_z. Im Laufe des Trainings haben sich diese Werte weitgehend angenähert. Daher werden nun die gleichen Werte für xyz verwendet.

Parameter trainieren

Zum Trainieren der Parameter wird im Lua Skript eine Flugstrecke festgelgt. Ausgehend von dieser Flugstrecke werden u.a. die Fehler OrientationError und DelayError ermittelt.

Beispiel für eine Flugstrecke

sleep(5) -- Wait for gazebo
takeoff() 
sleep(7) -- wait for the takeoff

print_error() -- Reset the errors 
while true do
    moveTo(5,2,1) -- fly to B from A (in the first round, A is the takeoff position)
    wait(0.1) 
    moveTo(-2,2,1) -- fly to A from B
    wait(0.1)
    sleep(2)
    land() -- land on B
    sleep(7) -- wait for landing 
    print_error() -- publish error and update controller parameters 
    takeoff() -- start new round 
    sleep(7) -- wait for takeoff
end

Lernen der Parameter

Die Einstellungen zum lernen der Parameter werden in bebopS/evo_params.yaml im Paket fitness festgelegt.

Erklärung der yaml

evo_params: {
    p_part: {
        topic: "position_controller_node", # topic name which is between drone_name and z_params/xy_params
        update_topic: "update_params", # update the params in the controller
        z_params: "U_xyz/U_z", # params to change
        xy_params: {x_params: "U_xyz/U_x",y_params: "U_xyz/U_y"},
        u_min: -0.1, # uniform real distribution
        u_max: 0.4,
        change_z: true, # change z parameter
        forget_after_x_gen: 0
    },
    d_part: {
        topic: "position_controller_node", # topic name which is between drone_name and z_params/xy_params
        update_topic: "update_params", # update the params in the controller
        z_params: "beta_psi/beta_psi", # params to change
        xy_params: {x_params: "beta_phi/beta_phi",y_params: "beta_theta/beta_theta"},
        u_min: -0.3,
        u_max: 0.1,
        change_z: false, # change z parameter
        forget_after_x_gen: 3
    },
    log: true # save log in ~/.ros/log_evo.csv
}

Die ursprüngliche Idee war, eine Normalverteilung für die Mutationsrate zu verwenden. Durch die Eigenschaft der Normalverteilung wurden teilweise zu große Schritte gemacht und die Drohne ist ins Netz gefolgen. Oder eine zu kleine Mutationsrate, welche nicht einen eindeutigen Unterschied bei den Fehlern geliefert hat.

Versuche haben gezeigt, dass größere positive Werte für den P und größere negative Werte für den D Teil den Fehler minimieren. Damit sich die Werte häufig in die richtige Richtung bewegen und keine großen Ausreißer haben, wurde eine Gleichverteilung verwendet. Mit einer geringen Wahrscheinlichkeit zu "schlechteren" Werten und einer großen zu "besseren" Werten.

Die Intervallgrenzen wurden nach viel probieren gefunden, dennoch sind es keine optimalen Grenzen. Ziel der Grenzen soll es sein, einen genügend großen Schritt zu machen um eine Verbesserung der Fehler zu erzielen, ohne dass die Drohne ins Netz fliegt.

Die Werte für P und D werden nach jedem Rundflug geändert. Zu Beginn der Versuche wurde mit einem Zufallswert der P oder D Teil geändert. Ziel war es, durch ein ein abwechselndes Ändern der Werte ein schnelles Lernen zu fördern. Durch die langen Flugstrecken, brachte das Vorgehen nicht den gewünschten Erfolg. Zusätzlich gab es keinen deutlichen Unterschied zu einem gleichzeitigen Ändern von P und D Werten.

Beim Lernen, wird immer der beste P Wert gespeichert und versucht diesen weiter zu verbessern. Der D Wert wird nach einer festgelegten Anzahl an Generationen verworfen und das aktuelle Kind/Offspring verwendet. Dieses Vorgehen soll das Verharren an einem lokalen Minimum verhindern. In der Simulation wird oft früh ein Minimum für D erreicht, welches jedoch mit besseren P Werten kein Minimum mehr ist und verworfen werden sollte.

Der Algorithmus vergleicht immer die letzten zwei Fehler einer Flugstrecke und nimmt die Parameter mit dem geringeren Fehler. Die Parameter werden anschließend mit einer zufälligen Mutationsrate geändert. Ein Crossover findet nicht statt, da die Population nur aus einem Individuum besteht.

log File Auswerten

Aufbau des Log Files:

u_x u_y y_z Delay Error beta_phi beta_theta beta_psi Orientation Error

Aus dem Log File können die neuen Werte für u_xyz und beta_[phi|theta] entnommen werden.

Beispiel einer Auswertung:

Optimierung von u_xyz (P-Teil)

Zeigt die Auswirkungen, wenn die Werte für u_xyz steigen. Es wurden zusätzlich die Werte für beta geändert, welche in der Grafik nicht abgebildet wurden

Optimierung von beta phi und theta (D-Teil)

Bei der Veränderung der u_xyz Werte müssen die beta Werte immer wieder neu angepasst werden, sonst ist der Orientation Fehler zu groß.

Gesamtübersicht

Die ersten zwei Grafiken zeigen die Optimierung von P und D. Beim P Teil ist dabei eine deutliche Optimierung des Fehlers zu erkennen. Beim D Teil findet ledeglich eine Korrektur der Werte um einen besseren Fehler zu erreichen statt. Diese Korrektur ist notwendig um auf die veränderten P Werte reagieren zu können und weiterhin eine geringe Schwankung zu behalten. In diesem Beispiel lernt die Drohne auf einer Flugstrecke indem die Drohne 7 Meter in eine Richtung und dann wieder zurück fliegt.

Fehlernode - error_functions

Beispiel launch file

<!-- In the drone namespace --> 
 <node pkg="error_function" type="error_function_node" name="error_node" output="screen" required="true" />

Der PositionError wird laufend ermittelt und auf den ongoing_error und der Mittelwert im current_error Topic ausgegeben

DelayError und OrientationError werden erst nach dem auführen des Lua Befehls print_error auf dem Topic current_error ausgegeben.

Im folgenden werden die Fehler im genauen beschrieben.

Parameter

keine

Topics/Services

Name Typ Art Beschreibung
current_error se_trajectory_msgs/Error Publication Wird nach einen print_error gepublished
ongoing_error std_msgs/Float64 Publication Wird laufend gepublished
trajectory_delay se_trajectory_msgs/Delay Publication Bei einem neuem moveto wird der DelayError des letzten moveto gepublished
odometry nav_msgs/Odometry Subscription Odomentry von BebopS
print_error std_msgs/Empty Subscription Published current_error und setzt die Summe des Fehlers wieder auf 0
target se_trajectory_msgs/ControlTarget Subscription Target vom moveto Befehl. Wird für den DelayError verwendet
trajectory_assignment se_trajectory_msgs/SeTrajectoryAssignment Subscription Anwort vom Trajektorienserver mit der Trajektorie und Zeit. Wird für den DelayError verwendet
trajectory_request se_trajectory_msgs/SeTrajectoryRequest Subscription Ziel der Trajektorie. Wird für den DelayError verwendet
/tf tf2_msgs/TFMessage Subscription Wird zur Positionsbestimmung verwendet

DelayError

Der DelayError beschreibt die Verspätung der Drohne zur Trajektorienplannung. Die Trajektorienplannung berechnet für eine Flugstrecke von A nach B für die Drohne eine Flugzeit. Der Fehler ermittelt die Verspätung der Drohne zur ermittelten Flugzeit. Der Fehler wird mit einem print_error im lua Skript gepublished und wieder auf 0 zurückgesetzt. Besteht die Flugbahn aus mehreren Trajektorien (wenn z.B. ein Hinderniss umflogen werden muss) oder aus mehreren moveto Befehlen so wird der Fehler aufsummiert bis zum nächsten print_error.

Dieser Fehler wird u.a. zur Optimierung der P Werte im BebopS Regler verwendet.

OrientationError

Der Orientation Fehler betrachtet die Schwankung von roll, pitch und yaw. Es werden dabei die Schwankungen um die 0 betrachtet. Bewegt sich eine Achse mehr als 0.005 Grad von dem 0 Winkel weg und dreht innerhalb von 0.25 Sekunden die Richtung und überschreitet wieder die 0.005 Grad in die andere Richtung, so wird die Distanz zwischen den maximalen und minmalen Winkel der Achse zum Fehler addiert. Der Fehler wird so lange aufsummiert, bis ein print_error ausgeführt wird. Danach startet der Fehler wieder bei 0.

Durch Analysen der BebopS Orientation Daten, war es sinnvoll Winkel unter einem Epsilon (0.005) nicht zu betrachten, damit auch tatsälich nur Schwankungen die nicht gewollt sind in den Fehler einfließen. Die Schwankungen müssen innerhalb von 0.25 Sekunden auftreten, sonst wird es als gewollte Richtungsänderung gewertet.

alt text

Hier ist der Verlauf von roll, pitch und yaw zu sehen. Sofern ein Schwanken auftritt, tritt es auch um die 0 auf. Wie es bei Pitch zu sehen ist. Daher wird nur das Schwanken um die 0 betrachtet und das Schwanken abseits der 0 ignoriert. Die blauen Säulen veranschaulichen den absoluten Abstand in den jeweiligen Zeitintervallen.

PoseErrorFunction

PositionError

Der Positionfehler ist der euklidische Abstand zwischen dem neuesten ControlTarget und der aktuellen Position (tf buffer) der Drohne. In diesem Fehler gibt es noch eine Abweichung t. Welche die Zeitdifferenz zwischen dem ControlTarget und tf angibt. Dieser Fehler wird als ongoing_error und im current_error gepublished.

Der Fehler wurde am Anfang zum trainieren der P Werte verwendet. Der Fehler hatte jedoch zu große Schwankungen und wurde durch den DelayError ersetzt.

Hector

Hector Bebop Interface

Damit Hector genau so angesteuert werden kann, wie eine echte Bebop Drohne mit bebop_autonomy, ist das Hector-Bebop Interface über Hector geschaltet. Es simuliert eine Bebop Drohne, indem es eingehende Nachrichten übersetzt und an die darunterliegende Hector Drohne weiterleitet.

Der Quad Script Node sendet takeoff und land Nachrichten an den Hector Bebop Interface Node, welcher diese in Actions übersetzt und diese an Hector weiterleitet

In der Grafik sendet der Quad Script Node takeoff und land Nachrichten an den Hector Bebop Interface Node, welcher diese in Actions übersetzt und dann an Hector weiterleitet.

Das Interface bietet auch einen Dynamic Parameter Server an, unter dem einige Parameter wie die maximale Steiggeschwindigkeit zur Laufzeit geändert werden können.

Bedienung

Hector Bebop Interface

Um Hector als simulierte Drohne nutzen zu können, muss zuerst ein Gazebo-Server auf dem gewünschten ROS Master-Server gestartet sein. Sobald dieser bereit ist, können die Hector Launchfiles mit ROS gestartet werden. Hierbei wird zwischen zwei Launchfiles unterschieden, der hector.launch Datei und der hector_script.launch Datei. Während die hector.launch Datei nur die nötigsten Nodes für die Drohne startet, wird bei der hector_script.launch Datei zusätzlich der Quad Script Node und der Client für die Trajektorienplanung gestartet. Weiter wird dies in der Subsektion Launch files erläutert. Für den normalen Gebrauch, so wie auch die echten Drohnen genutzt werden, sollte die hector_script.launch Datei benutzt werden, da die dann gestartete simulierte Drohne ohne weitere externe Eingabe mit der Trajektorienplanung fliegen kann.

Die hector.launch wird mit folgenden Parametern gestartet:

Die hector_script.launch wird mit den Parametern der hector.launch und folgenden zusätzlichen Parametern gestartet:

Launch files

Hector Bebop Interface

Beispiel eines Hector Launch files

    <launch>
      <arg name="ns" default="/test"/>
      <arg name="quad_name" default="drone1"/>
      <include file="$(find hector_bebop_interface)/launch/hector_script.launch">
        <arg name="ns" value="$(arg ns)"/>
        <arg name="script_path" value="$(find hector_bebop_test)/script/test_trajectory.lua"/>
        <arg name="quad_name" value="$(arg quad_name)"/>
      </include>
    </launch>

hector_script.launch und hector.launch sind hierarchisch aufgebaut. hector_script.launch importiert hector.launch und startet einige weitere Nodes im Drohnen Namespace.

Beispiel hector_script.launch Struktur

In der Abbildung ist die Struktur der beiden Launchfiles erkennbar. Als äußerster Kasten ist der Namespace, der als ns Argument an die Launchfiles mitgegeben werden kann. In diesem wird beispielhaft hector_script.launch ausgeführt, welches zunächst selbst hector.launch ausführt mitsamt der dort beinhalteten Nodes im ns/quad_name Namespace: quad_state_node, joy_teleop, bebop_position_control_node für den Flug, bebop_position_control_node für Start und Landung, bebop_switch_node, hector_bebop_interface und die simulierte Hector Drohne selbst. Zusätzlich startet hector_script.launch den quad_script_node und den se_trajectory_client_node.

Im Wesentlichen soll hector_script.launch das Analogon zum zentralen se_bebop.launch (se_trajectory_launch) Launchfile der echten Drohnen sein. hector.launch existiert zur klaren Aufteilung zwischen rein essenziellen und notwendigen Nodes und Nodes zum Flug mit der Trajektorienplanung und wird unter Anderem benutzt für die Aufnahme und das Abspielen aufgenommener Flugrouten, die unabhängig der Trajektorienplanung und Scripts laufen müssen.

Interface node

Hector Bebop Interface

Die Node des Hector Bebop Interfaces ist zuständig für das Übersetzen der entsprechenden Nachrichten, sowie für das Simulieren einer Batterie, als auch für das Angleichen des Flug-, Start- und Landeverhaltens an die echte Bebop Drohne.

Die Node akzeptiert und benutzt folgende Parameter:

Die Interface Node veröffentlich folgende Topics:

Auf folgenden Topics wird vom Interface Node gehört:

Das Interface kann sich in zwei Phasen befinden. Dem Init-Zustand, und dem Spin-Zustand.

Im Init-Zustand empfängt es noch keine Nachrichten. In diesem Zustand werden sämtliche Publisher und Subscriber auf den oben genannten Topics subscribed bzw. advertised und der Dynamic Reconfigure Server wird gestaret. Außerdem wird ein Service Client für den EnableMotors Service der Hector Drohne gestartet und auf diesen gewartet. Weiter wird ein TransformListener gestartet. Falls canTransform auf der Frame name fehlschlägt wird die Node mit einem Fehler beendet. Vor dem Übergang in den Spin-Zustand wird eine Nachricht mit dem aktuellen Batteriestand veröffentlich.

Im Spin-Zustand hört das Interface auf Nachrichten auf den oben genannten Topics. Neben dem Hören auf Nachrichten und ausführen der entsprechenden Callbacks wird hier primär auch das Flugverhalten beim Start und bei der Landung, sowie bei fehlender cmd_vel Nachrichten spezifiziert. Dies wird weiter in der Subsektion Flugverhalten erläutert. Zudem wird im Spin-Zustand auch der Batteriestand angepasst, falls die Motoren aktiv sind und gegebenenfalls wird bei Änderung des Batteriestands (in ganzen Prozent) eine Nachricht mit dem aktuellen Batteriestand veröffentlicht. Sollte dieser auf 0 fallen, begibt die Drohne sich automatisch in den Landevorgang. Erhält das Interface für no_signal_timeout keine cmd_vel Nachrichten, wird automatisch auf den Start- und Landekontroller umgeschaltet (cmd_vel_tol) welcher die Position dann halten soll. Sobald eine cmd_vel Nachricht geschickt wird und die Drohne weder im Start- noch im Landevorgang ist, wird wieder auf den Flugkontroller (cmd_vel) umgeschaltet.

Flugverhalten

Es gibt fünf Subzustände in denen sich das Interface befinden kann, sobald es im Spin-Zustand ist. Den takingOff-Zustand, den landing-Zustand, den resetting-Zustand, den holdPos-Zustand und den allgemeinen Zustand. Außerdem gibt es einen unabhängigen enginesOn-Zustand, der angibt, ob die Motoren gestartet sind. Wenn eine takeoff Nachricht empfangen wurde, wechselt das Interface in den takeOff-Zustand. Bei einer land Nachricht, wechselt es in den landing-Zustand und analog wechselt es bei einer reset Nachricht in den resetting-Zustand. Wenn für no_signal_timeout keine cmd_vel Nachrichten empfangen werden, die Drohne weder startet, landet oder resettet und falls die Motoren an sind, wechselt es in den holdPos-Zustand.

Im takingOff-Zustand startet die Drohne die Motoren und hebt auf etwa 1 Meter oberhalb der ihrer Position zum Zeitpunkt des Wechselns in den takingOff-Zustand ab. Sobald die gewünschte Höhe erreicht ist wechselt es in den allgemeinen Zustand.

Im landing-Zustand landet die Drohne auf dem Boden unterhalb ihrer Position, zum Zeitpunkt zu der sie in den landing-Zustand gewechselt ist. Sobald sie gelandet ist, werden die Motoren ausgeschaltet und sie wechselt in den allgemeinen Zustand.

Im resetting-Zustand verhält die Drohne sich wie im landing-Zustand.

Im holdPos-Zustand hält die Drohne ihre Position, indem sie den Start- und Landekontroller anweist die Position zu halten, bei der sie in den holdPos-Zustand gewechselt ist und dann auf cmd_vel_tol wechselt. Sobald sie eine cmd_vel Nachricht empfängt, wechselt sie in den allgemeinen Zustand.

Interface Zustandsdiagramm Interface Zustandsdiagramm Legende

PID Tuning Node

Die PID Tuning Node ist eine Node, mit der PID Werte für Hector gefunden werden können.

Sie akzeptiert folgende Parameter:

Funktionsweise

PID Tuning Zustandsdiagramm

Tests

Um andere Module, aber auch die simulierte Drohne selbst testen zu können, kann der Test Trajectory Node (test_trajectory) verwendet werden. Er führt eine Missionsdatei (lua-Script) aus und schließt erfolgreich ab, falls keine Fehlschlagkriterium bis zum Ende der Mission erfüllt wird.

Der Test akzeptiert und benutzt folgende Parameter:

Bedienung

Der test_trajectory Node sollte mit dem <test> tag in einer .launch oder .test Datei durch rostest gestartet werden. Ein voll funktionsfähiges Testfile ist nebenan sichtbar.

Beispiel Testfile für test_trajectory

  <launch>
    <arg name="ns" default="/test"/>
    <arg name="quad_name" default="drone1"/>
    <arg name="gui" default="false"/>

    <include file="$(find hector_bebop_interface)/launch/hector_se_server.launch">
      <arg name="ns" value="$(arg ns)"/>
      <arg name="gazebo_gui" value="$(arg gui)"/>
      <arg name="gazebo_world" value="$(find bebops_launch)/worlds/drone_lab.world"/>
    </include>

    <include file="$(find hector_bebop_interface)/launch/hector_script.launch">
      <arg name="ns" value="$(arg ns)"/>
      <arg name="script_path" value="$(find hector_bebop_test)/script/test_trajectory.lua"/>
      <arg name="use_mission_server" value="true"/>
      <arg name="quad_name" value="drone1"/>
    </include>

    <group ns="$(arg ns)/$(arg quad_name)">
      <node pkg="error_function" type="error_function_node" name="error_function" output="screen" required="true">
          <param name="ns" type="string" value="$(arg ns)"/>
          <param name="drone_name" type="string" value="$(arg quad_name)"/>
      </node>
    </group>

    <test test-name="trajectory_straight_line" pkg="hector_bebop_test" type="test_trajectory" ns="$(arg ns)/$(arg quad_name)" time-limit="9999999.0">
      <param name="error_topic" type="string"
             value="$(arg ns)/$(arg quad_name)/ongoing_error"/>
      <param name="error_threshold" type="double"
             value="0.3"/>
      <param name="script" type="string"
             value="$(find hector_bebop_test)/script/test_trajectory.lua"/>
      <param name="timeout" type="double"
             value="120.0"/>
    </test>
  </launch>

Funktionsweise

Der test_trajectory Test Node startet sobald wie möglich einen Timer, der nach Ablaufen den Test fehlschlagen lässt. Er verbindet sich dann mit dem Missions Server des Quad Script Node oder wartet auf diesen. Sollte während dieser Wartezeit der Timeout überschritten werden, wird der Test mit einem Fehler abgebrochen. Nachdem die Verbindung zum Missions Server steht, wird script an ihn übermittelt und die Mission wird ausgeführt. Sollte während dieser Zeit der Fehler aus error_topic den Wert von error_threshold überschreiten, wird der Test mit einem Fehler abgebrochen. Sobald der Missions Server die Mission als abgeschlossen kennzeichnet, wird der Test erfolgreich beendet. Bei einer Kollision mit einem Gegenstand oder einer Wand wird der Test mit einem Fehler beendet.

Testkriterien

Fehlerquellen

Beim Benutzen des test_trajectory Test Nodes sollten folgende Dinge beachtet werden:

SE Projekte: AR Visualisierung

Zur Visualisierung innerhalb von ROS kann ein Node Nachrichten aus visualization_msgs verschicken. Ein kompatibler Node, wie z.B. RViz kann die Nachrichten dann empfangen, interpretieren und in einer 3D-Ansicht anzeigen (siehe: ROS2: rviz2).

Um eine bessere Immersion zu erzielen, soll eine Anwendung geschaffen werden, die zu RViz kompatibel ist und visualization_msgs in einem Augmented Reality Overlay anzeigt.

Im nächsten Kapitel wird die Benutzung der exisiterenden App erklärt. In den darauffolgenden Kapiteln wird auf die zugrundeliegenden Einzelheiten eingegangen bzw. zusätzliche Information für die Entwicklung bereitgestellt.

Anleitung: Benutzung der App mit Tablet im Labor

Diese Anleitung soll helfen, das System zum Laufen zu kriegen.

0. Voraussetzungen

1. Tablethalterung in Motive aufnehmen

💡 Dieser Schritt muss nur einmal gemacht werden, bzw. wenn die Halterung sich ändert. Projekt danach speichern, damit man es nicht nochmal machen muss!

2. Rosbox vorbereiten

3. Unity App verbinden

4. Fertig

Nun müssten zumindest die Marker von der rosbox auf dem Tablet in der App angezeigt werden und man sollte beidseitig (RViz <-> Unity) interagieren können.

AR Interaktionskonzept für Interactive Markers

Im Kontext von ROS/RViz bezeichnen Marker primitive Formen, die eine Pose im Raum sowie einige andere Eigenschaften haben. Im Drohnenlabor könnte man damit z.B. die Positionen der Drohne, Hindernisse oder Wegpunkte visualisieren.

Die Interactive Markers sind eine Erweiterung dieses Konzepts, welche die Interaktion mit Markern ermöglicht. So kann man z.B. einen Marker entlang einer Achse oder Ebene verschieben, ihn rotieren, ein Kontextmenü anzeigen und auf Klicks reagieren.

In RViz ist bereits ein InteractiveMarkerClient implementiert, der diese Funktionalität übernimmt. Die im Rahmen dieses Projektes entwickelte App soll zumindest die wichtigsten Arten von interaktiven Markern unterstützen.

Die zu entwickelnde App unterscheidet sich jedoch in den verfügbaren Interaktionsmodalitäten: RViz wird traditionell mit Maus und Tastatur bedient, unsere App hingegen per Touch. Außerdem wiegt das Tablet fast 1kg, weshalb man es wohl auch nicht gerne für längere Zeit mit nur einer Hand halten möchte.

Aus diesen Gründen haben wir einige verschiedene Interaktionskonzepte erarbeitet. Es haben sich 3 Grundlegende Konzepte herauskristallisiert, und innerhalb dieser werden ggf. noch Varianten unterschieden. Diese Präsentation definiert die Konzepte, sodass sie zukünftig auch mit deren Nummern referenziert werden können. Außerdem werden Vor- und Nachteile erörtert.

AR Interaktionskonzepte Präsentation

Hier eine Zusammenfassung der 3 "Haupt-Varianten":

  1. Die naive/einfache Lösung: Der Benutzer hält das Tablet mit einer Hand, und interagiert mit der anderen Hand per Touch.

  2. wie 1., aber statt dem Finger wird ein kleines Fadenkreuz (sog. Reticle) angezeigt, und an der rechten Seite des Bildschirms befindet sich ein großer Button. So kann der Benutzer das Tablet weiter mit beiden Händen halten und wahrscheinlich das Tablet präziser bewegen. (Hier gibt es viele mögliche Varianten, siehe Präsentation)

  3. Der Benutzer wählt einen Marker durch einmaliges Tippen aus, und positioniert dann das Tablet so im Raum wie gewünscht. Nach Bestätigung durch erneutes Tippen wird der Marker wieder "losgelassen", und hat nun genau die Position & Rotation des Tablets. Der Vorteil dieses Konzeptes wäre das 1:1 Mapping zwischen dem echten und dem virtuellen Objekt, wodurch die Pose des letzteren gut vorstellbar ist.

Die Interaktionsmodi 1. und 2. sind eng verwandt, und 3. füllt eine Nische, die in der Benutzung manchmal sehr praktisch sein könnte. Daher soll im Rahmen des Projekts versucht werden, möglichst alle 3 Interaktionsmodi umzusetzen.

Unity App

Zur Umsetzung der Augmented-Reality-App wird Unity3D verwendet, ursprünglich eine Cross-Platform (Linux, Windows, Android, Playstation, etc.) Gaming Engine, welche hier zur Darstellung von Objekten im Raum genutzt wird. Unity3D-Projekte werden grundsätzlich in C# programmiert.

Beim Bauen der App wird eine .exe Datei generiert, welche dann auf dem Windows Device installiert werden kann.

Die folgende Dokumentation erläutert

Code-Style und Code-Analyse

# installiert Abhängigkeiten und führt das Tool aus
$ sudo ./ci/generate_inspection_report.sh -i -f unity_app_simple/unity_app_simple.sln
# Beispiel Output während des Checks (ohne -i)
$ ./ci/generate_inspection_report.sh -f unity_app_simple/unity_app_simple.sln
Running Code checker...
[...] generating output report in ./report_output
    >> JetBrains Inspect Code 2020.2
    >> Running in 64-bit mode, .NET runtime 3.1.8 under Unix 5.4.0.47
    >> Using toolset version 16.0 from /usr/share/dotnet/sdk/3.1.101
    >> Configuration: Debug, Platform: Any CPU
    >> Analyzing files
    >> Analyzing Tests.asmdef
    >> Analyzing Tests.csproj
    >> Analyzing EditModeTests.asmdef
    >> Analyzing EditModeTests.csproj
    >> Analyzing Assembly-CSharp-Editor.csproj
    >> Analyzing GameScriptsAssembly.asmdef
    >> Analyzing GameScriptsAssembly.csproj
    >> Inspecting Calibation.cs
    >> Inspecting Debug_AnroidConsole.cs
    >> Inspecting AndroidTest.cs
    >> Inspecting ImportMatrixFromJson.cs
    >> Inspecting CalibrationMatrix.cs
    # ...output shortened...
    >> Inspecting EditorTest.cs
    >> Inspecting TestHelper.cs
    >> Inspecting ResharperFileGenerator.cs
    >> Inspecting WindowsBuilder.cs
    >> Inspection report was written to /home/philipp/git/se-ar/report_output/2020-09-16_inspection_report.html
[DONE] finished.

Zur statischen Code-Analyse kommt das Tool inspectcode aus der ReSharper Erweiterung für Visual-Studio zum Einsatz.

Damit eine Auswertung des Codes auch ohne Visual-Studio funktioniert, steht im Projekt das Skript /ci/generate_inspection_report.sh bereit, welches automatisch die benötigten Abhängigkeiten (für Ubuntu!) herunterlädt und das ReSharper Tool ausführt.

Benutzung

Flag Kurz Flag Lang Required Beschreibung
-h --help Nein Informationen zum Skript
-i --install-deps Nein Installiert die nötigen Abhängigkeiten (z.B. dotnet, unzip)
-f --solution-file Ja Nach diesem Parameter muss der Pfad zur .sln-Datei des Projekts angegeben werden.

Unity Installation

Unity App Bauen

Aufbau der App

Das 3D Overlay wird durch eine Standard Unity Scene abgebildet und kann auch so verwendet werden. Abweichend von einer Standard Unity Scene ist nur die Hauptkamera. Diese verfügt über ein Camera-Rig, welches nicht wie üblicherweise von Nutzereingaben kontrolliert wird sondern welches mit einem Custom Controller von ROS-TF-Messages positioniert wird. Des weiteren wurde die Unity Skybox durch eine Textur ersetzt, welche als Rendertarget für den Input der Webcam dient.

Dateistruktur

In Unity unterliegt die Struktur eines Projekts einer gewissen Konvention, auf deren Einhaltung manche Funktionen der Engine aufbauen. Beispielsweise können nur Assets aus dem Order Resources zur Laufzeit als File referenziert werden, alle Assets ausserhalb müssen zur Compile-Zeit bereits in der Scene referenziert sein oder sind nichtmehr durch Engine Code zugreifbar (Ausgenommen hiervon sind Scripts/Gamecode).

Das Unity Project ist im Order /unity_app_simple zu finden, dieser Pfad kann mit Unity Geöffnet werden. In diesem Ordner befinden sich auch Project Dateien (*.csproj, *.sln) diese sollten jedoch AUSSCHLIESLICH von innerhalb der UnityEditor Umgebung geöffnet werden. Innerhalb Des Editors ist der Assets Ordner die 'höchste' angezeigte Hierarchie Ebene, alle anderen Files werden durch Unity selbst verwaltet und sollten nicht beeinflusst werden. Der Assets Ordner enthält für jedes Element (Dateien UND Ordner ) ein {NAME}.meta File. Dieses dient Unity zur File Verwaltung. (Selten kann dies zu Problemen Führen wenn Dateien gelöscht werden Unity die dazugehörigen Meta Files aber nicht automatisch mit löscht). Weiterführende Informationen können in den Unity Docs gefunden werden.

Aufbau des Assets Ordners

Ordner mit Besonderen Bedeutungen sind:

Die Übrigen Ordner sind nur durch die Konvention an ihre Funkion gebunden, haben jedoch keine besonderen Eigenschaften:

Nur dem scripts Order kommnt noch eine besondere Bedeutung zu, dieser enthält alle ("Game"-)Scripts die zur Funktion der gebauten anwendung notwenig sind. Jedoch enthält dieser auch Unterprojekte/Plugins. Einmal ist hier das RosSharp Projekt zu finden (in diesem befinden sich auch unsere Custom Erweitungen Assets\scripts\RosSharp\Scripts\RosBridgeCustom um die für unseren Anwendungsfall nötigen Modifikationen zu realisieren) und in RosSharp befindet sich auch ein Plugins Ordner, der die für RosSharp nötigen Dlls beinhaltet. Dies ist in unserem fall nötig, da für die Unit Tests die Anwendung in mehrere Teile gesplitet wird. Dies könnte zwar auch umgangen werden, ist aber mit enormen Mehraufwand verbunden.

Die Aufteilung (Split) des Projekts wird durch die *.asmdef Files definiert, diese sorgen dafür, dass der Ordner in dem sich diese Datei befindet und alle Unterordner in eine Dll gebaut werden. Desweitern wird in diesem File angegeben welche anderen Dlls für diesen Teil des Programms referenziert werden. Dies sorgt dafür, dass Testcode nicht mir in die "shipping" version von gebauten Anwendungen integriert wird.

Setup

Um die App aktiv nutzen zu können muss eine ROSBridge Verbindung (Typ WebSocket) geöffnet werden. Diese kann im RosConnector (Siehe Module) eingestellt werden.

Die Position des Camera Rig wird über einen TF2Subsciber gehandelt. Dieser muss für das Setup im Keller /tf Subscriben und auf die ChildFrameId tablet Lauschen. Das anzusteuernde Transform ist in diesem Fall das CameraRig selbst. Der Subscriber ist im _scripts Object zu finden.

Module der Unity App

In diesem Abschnitt werden die nennenswerten Bestandteile der Unity-App beschrieben und jeweils erklärt, wie sie zu handhaben sind.

RosConnector

Diese Komponente ist die Basis für die RosBridge Verbindung. Im Projekt ist diese im _Scripts Object zu finden.

Default Einstellungen:

Marker Messages

Ros Marker Messages werden über eine MarkerMsgSubscriber Komponente empfangen. Diese Benötigt eine MarkerMsgManager Komponente um die Marker anzuzeigen.

Einstellungen:

Interactive Marker Client

// InteractiveMarkerMsgSubscriber.cs
// ...
protected override void ReceiveMessage(InteractiveMarkerUpdate message)
{
    // Nachrichten werden in Queue des Managers eingereiht
    manager.Enqueue(message);
}
// ...


// InteractiveMarkerGizmoManager.cs
// ...
private void Update()
{
    // Queue wird bei jedem Aufruf von Update() abgearbeitet
    while (!_InitMessageQueue.IsEmpty && !_IsInitialized)
    {
        InteractiveMarkerInit current;
        _InitMessageQueue.TryDequeue(out current);
        // InitTransformHandles übernimmt die Anzeige der Daten
        InitTransformHandles(current.markers);
    }
}
// ...  

Interactive Marker Nachrichten werden mit Publishern und Subscribern versendet, bzw. empfangen. Empfangene Nachrichten werden vom Subscriber dann gemäß dem Producer/Consumer Pattern an den jeweiligen Manager übergeben.

Die Publisher enthalten eine Publish Methode, welche z.B. neue InteractiveMarkerFeedback Messages veröffentlicht.

Damit InteractiveMarkerClientDemo funktioniert, werden folgende Scripts in der Scene benötigt:

Interactive Marker Client Inspector Ansicht

Asynchrone Kommunikation zwischen Unity3D und RosBridge

Um die grafische Darstellung in Unity3d nicht 'ruckeln' oder einfrieren zu lassen, laufen User Interface (UI) und Netzwerkkommunikation in separaten Threads. Um einen Austausch der Nachrichten zwischen den beiden Threads zu realisieren, wird eine ConcurrentQueue benutzt.

Concurrent Queue zur Interprozesskommunikation

InteractiveMarker Implementierung

Grundsätzlich wird jeder InteractiveMarker durch ein Root-GameObject dargestellt. Dieses hat selbst keine Komponenten, es dient nur zur Gruppierung der einzelnen Controls. Der Name des Root-GameObjects entspricht dem Namen des InteractiveMarkers. Bei jedem InteractiveMarker-Update wird nur die Position des Root-Gameobjects aktualisiert (nicht die Rotation).

Ein separates GameObject, das rotationParent, dient der korrekten Behandlung der Rotation (Manche Controls erben die Rotation des InteractiveMarkers, andere sind unabhängig bzw. fixed). das rotationParent GameObject ist ein Kind des Root-GameObjects und erbt somit dessen Position. Zusätzlich wird bei jedem Interactive-Update die Rotation des rotationParent geupdated.

Für jedes Control gibt es dann ein GameObject. Dabei ist das GameObject entweder ein Kind des rotationParent, wenn es die Rotation seines InteractiveMarkers erbt (orientation_mode INHERIT), oder direkt ein Kind des Root-GameObjects, wenn die Rotation unabhängig vom Parent sein soll (orientation_mode FIXED & VIEW_FACING, wobei letzteres noch nicht implementiert ist).

Die Darstellung & Interaktion der Interactive Marker wird von einer Hierarchie an verschiedenen Skripten übernommen, die wie folgt mit den o.g. GameObjects zusammenhängen:

Im folgenden Video sieht man noch einmal die eben beschriebene Struktur aus GameObjects und Skripten, die so erzeugt wird. Bei dem Beispiel handelt es ich um einen InteractiveMarker mit 4 Controls als Kinder: Einem Würfel (mit interaction_mode NONE) zur Anzeige, und 3 MOVE_AXIS Controls für die 3 Achsen (mit orientation_mode FIXED).

InteractiveMarkerGizmo als Basisklasse für Gizmos

Die Gemeinsamen Funktionen der MoveAxis- und RotateAxis-Gizmos werden in der Basisklasse InteractiveMarkerGizmo zusammengefasst.

Dazu zählen z.B. Material und Farbe des Gizmos, sowie deren Änderung bei Mausinteraktion. Auch wird Information dort verwaltet, die von beiden Gizmos (und von ggf. neuen Gizmo-Implementierungen wahrscheinlich auch) benötigt werden.

Deshalb ist es wichtig, dass die einzelnen Gizmo-Implementierungen von InteractiveMarkerGizmo erben und die dort gebotenen Basisfunktionen korrekt nutzen. Im Falle von MoveAxis und RotateAxis wurden die Mausevent-Funktionen um die jeweils nötigen Schritte ergänzt.

Context Menus

Context Menus sind einfache Menüstrukturen, welche Interaktionen mit einem Objekt ermöglichen, z.B. Publish einer Nachricht bei Click, Ausführen von Roslaunch, Rosrun, etc. Mittels einer Baumstruktur können Context Menus verschachtelt dargestellt werden, sodass sich der Nutzer durch verschiedene Ebenen des Menüs clicken kann.

Einzelne Menüeinträge werden durch MenuEntrys repräsentiert, welche wiederum mit einer InteractiveMarker Message verschickt werden.

Beispielhaftes Context Menu im Game View von Unity3D

Implementierung der ContextMenus in Unity

Angezeigt werden die ContextMenus mittels einfacher Unity Buttons. Diese werden untereinander angeordnet angezeigt und bei einem Click, der keine Menü-Funktion auslöst, wieder ausgeblendet.

Verschiedene Klassen sind für die Implementierung dieses Verhaltens verantwortlich:

Unity App mit GitlabCI bauen

Testen der Unity App

public class TestScript
{
    [TestFixtureSetUp]
    public void Init() {
        // läuft einmal vor allen Tests
    }
    [TestFixtureTearDown]
    public void CleanUp() {
        // läuft einmal nach allen Tests
    }
    [SetUp]
    public void Setup()
    {
        // läuft vor jedem einzelnen Test
    }
    [TearDown]
    public void Teardown()
    {
        // läuft nach jedem einzelnen Test
    }

    // Normale Tests verhalten sich wie gewöhnliche Methoden
    [Test]
    public void NewTestScriptSimplePasses()
    {
        // hier Test Code
    }

    /** UnityTests verhalten sich wie Coroutinen in Unity
    *   `yield return null;` überspringt einen Frame
    */
    [UnityTest]
    public IEnumerator EditorTestWithEnumeratorPasses()
    {
        yield return null;
    }
}

Unity 3D Testrunner

Die Assert Library

Funktion Beschreibung
AreApproximatelyEqual Assert the values are approximately equal.
AreEqual Assert that the values are equal.
AreNotApproximatelyEqual Asserts that the values are approximately not equal.
AreNotEqual Assert that the values are not equal.
IsFalse Return true when the condition is false. Otherwise return false.
IsNotNull Assert that the value is not null.
IsNull Assert the value is null.
IsTrue Asserts that the condition is true.

Interactive Marker Test Node

Der interactive Node generiert 6 interaktive Marker welche zum Testen der App verwendet können. Die Marker werden in Form von Boxen angezeigt und können auf verschiedene Arten genutzt werden:

Name Beschreibung Handles
Context Menu Mittels Rechtsklick wird ein Kontextmenü angezeigt Nein
3D Control MOVE_3D Marker kann frei im Raum bewegt werden Nein
3D Control ROTATE_3D Marker kann um alle Achsen rotiert werden Nein
6-DOF Control Marker kann in alle Richtungen bewegt und rotiert werden Nein
3-DOF Marker kann in 3 Achsen über Handles bewegt werden Ja
Pan / Tilt Marker kann in 2 Achsen gedreht und geneigt werden Ja

Interactive Markers

Starten des Nodes (inklusive ROS+RViz)

Die per default eingestellten Verbindungsparameter sind:

Parameter Wert
Adresse localhost
Port 9090
Protokoll WebSocket
Serialisierung JSON
Topic Präfix /basic_controls/

Docker container

Bauen der Docker-Container unter Windows z.B. in PowerShell

PS C:/Users/luisb/git/se-ar> docker-compose up --build
Pulling ros-master (gramaziokohler/ros-base:19.11)...
19.11: Pulling from gramaziokohler/ros-base
34667c7e4631: Pull complete
d18d76a881a4: Pull complete
119c7358fbfc: Pull complete
2aaf13f3eff0: Pull complete
55ed0c069115: Pull complete
89ae18dff6ff: Pull complete
f7a1f7d02fbc: Pull complete
6721473f3fc2: Pull complete
f35a6933912e: Pull complete
826ee89684e9: Pull complete
0bdccefa2513: Pull complete
80d23f7d0afe: Pull complete
ad0597d4bfa5: Pull complete
7c6859c42cc5: Pull complete
986c2c0e8807: Pull complete
bd6fcc85c67a: Pull complete
Digest: sha256:1c12f865559fab60d3662cff7037fc682afcb4dfd15459151b379cbe6069133f
Status: Downloaded newer image for gramaziokohler/ros-base:19.11
Pulling gui (gramaziokohler/novnc:)...
latest: Pulling from gramaziokohler/novnc
27833a3ba0a5: Pull complete
9069a50427b6: Pull complete
a00fa66b7e18: Pull complete
d3c8528e96bd: Pull complete
Digest: sha256:ef6c4de102285486187fbe348ce3b3e6022376543d5559538aea9b3476acc3b6
Status: Downloaded newer image for gramaziokohler/novnc:latest
Building ros-se-ar
Step 1/5 : FROM osrf/ros:melodic-desktop-full
 ---> d89cfe0fc5b5
Step 2/5 : SHELL ["/bin/bash","-c"]
 ---> Using cache
 ---> 2b007b996423
Step 3/5 : ADD docker-rviz/image /
 ---> 4f933ee4260b
Step 4/5 : ADD interactive_marker_demo_node/se_ar_im /ros_ws/src/se_ar_im
 ---> c775b5eae877
Step 5/5 : ENTRYPOINT [ "/startup.sh" ]
 ---> Running in d8649cf66c82
Removing intermediate container d8649cf66c82
 ---> d2d7b1a1d70d
Successfully built d2d7b1a1d70d
Successfully tagged ros-se-ar:latest
Creating ros-master ... done
Creating gui        ... done
Creating ros-se-ar  ... done
Creating ros-bridge ... done

# sobald der VNC server läuft, kann das noVNC Interface aufgerufen werden:
PS C:/Users/luisb/git/se-ar> start chrome http://localhost:8080/vnc.html?autoconnect=true

Um die Entwicklung (v.a. unter Windows) einfacher zu machen, gibt es ein Docker-Compose File (/docker/docker-compose.yaml), das den o.g. Test-Node, RViz mitsamt Fenstermanager, sowie die rosbridge in ihren jeweiligen Docker-Containern startet. Auf das docker-compose file wird in /.env verwiesen.

Zum starten einfach im root-Verzeichnis des se-ar Repos docker compose up --build ausführen. Mit dem flag -d bzw. --detach werden keine Konsolenausgaben von den laufenden Containern angezeigt. Beim ersten Mal dauert das länger, da alle Abhängigkeiten heruntergeladen werden.

Nach erfolgreichem build sollte die GUI nun über noVNC im Browser auf localhost's Port 8080 (http://localhost:8080/vnc.html?autoconnect=true) aufrufbar sein. Auf localhost:5900 läuft außerdem ein "normaler" VNC Server (x11vnc), der mit einem VNC Viewer (z.B. realvnc für Windows) angesteuert werden kann.

Die rosbridge läuft standardmäßig auf localhost:9090, was der Einstellung im run_demo.launch file entspricht. Zu Beachten ist: es wird nicht die rosbridge aus dem launchfile verwendet, sondern eine in einem separaten container. Mit der rosbridge aus dem launchfile war der websocket nicht von außen zugänglich (aus unerklärlichen Gründen).

Dann sollte die Kommunikation zwischen Unity auf dem (Windows-)Host (links) und ROS im Docker-Container (rechts) funktionieren:

Docker: RViz kommuniziert mit Unity auf Windows

Die Framerate ist nicht atemberaubend, aber für einfache Tests wie Verschieben von Markern reicht es.

Kalibrierung

Die Kalibrierung der Kamera (das Kamera-Bild der echten Welt und das Kamera-Bild der Gerenderten Scene in Übereinstimmung bringen) kann automatisch oder manuell erfolgen.

Die Automatische Kalibrierung Benutzt OpenCV und eine Reihe von Bildern eines Kalibration-Boards um Automatisch den Offset der Tablethaltung (bzw. der daran angebrachten Positions-Markern und der Kamera) zu errechnen.

Die Manuelle Kalibrierung erlaubt dem Benutzer mit auf dem Bildschirm eingeblendeten Steuerelementen die virtuelle Kamera zu verschieben um die virtuelle Scene mit dem Kamera Bild (der echten Welt) in Übereinstimmung zu bringen.

Automatische Kalibrierung

Siehe Abschlussarbeit ab Seite 25.

TODO: Einfache Schritt für Schritt Anleitung schreiben ohne komplexe technische Hintergründe

Die automatische Kalibrierung benutzt zur Zeit eine Java Library die zu einer DLL konvertiert wurde und im Betrieb problemlos funktioniert. Jedoch treten beim bauen der App via Gitlab CI Fehler auf, aus diesem Grund befindet sich diese Funktionalität derzeit in einer separaten Branch.

Die automatische Kalibrierung startet automatisch mit dem Start der App und zeigt Debug-Code auf dem Bildschirm an. Es wird im Hintergrund nach dem Calibration-Pattern auf dem Calibration-Board gesucht.

Nach erfolgreicher Kalibration wird der Statustext geupdated und die virtuelle Kamera auf den richtigen Offset verschoben.

Detaillierte Beschreibung der automatischen Kalibration

Die Kalibrations-Routine (Calibration.cs) generiert eine neue CalibrationTask diese enthält alle notwendigen Daten für eine Kalibration. Die Routine Läuft pro Frame mit Folgendem Ablauf:

Messpunkt nehmen (TakeSample Funktion)

Wenn Kameradaten verfügbar sind wird auf der CPU die Textur kopiert. Dies ist ineffizient und sollte auf der GPU ausgeführt werden, leider ist die WebCamTexture von Unity nicht dafür ausgelegt und die NativePtr scheinen die GPU copy native Methode leider auch nicht zu unterstützen.

Die aktuellen Positionen des Boards und der Kamera werden vom TF2 abgefragt und gespeichert, da diese nur im Main Thread zur Verfügung stehen.

Nachdem die Start Zeit, die auch als Semaphore Dient (um den Start mehrerer Subthreads zu verhindern) gesetzt wurde, wird ein neuer Subthread gestartet, in dem das Kamerabild in eine Matrix konvertiert wird und vom OpenCV nach dem Kreis-Pattern gesucht wird.

Wenn ein Pattern gefunden wird und Daten über dieses verfügbar sind, wird ein neues Sample genommen. Dies umfasst die Positionsdaten (Board+Camera) ImgSize und Positionsdaten der Kreise im Bild. Wenn genug Samples gesammelt wurden, wird die eigentliche Berechnung der Kalibration gestartet (diese läuft weiter in diesem Subthread).

Am Ende wird die Semaphore mit 0.5 Sekunden Cooldown freigegeben.

Kalibrations Berechung

Intrinsisch

Zuerst werden die Daten in ein Speicherformat gebracht, wie es für den Camera Kalibration Algorithmus notwendig ist. Der verwendete Algorithmus ist bereits in OpenCV enthalten und wird einfach mit den erforderlichen Daten gefüttert.

Cv2.CalibrateCamera(
        objectPoints, // Beschreiben das zu Suchende Muster [Array]
        mCorners, // Die vom Computer Vision Gefundenen "Eckpunkte" des Musters [Array]
        imageSize, // die Größe des Bildes
        cameraMatrix, // Output für die Kamera Matrix, beginnend mit der Einheitsmatrix
        distortionVector, // Vector(Size 5x1) Der die Linsen-Verzerrungs-Koeffizenten beschreibt
        out rVecs, // Geschätzte Roatations-Vectoren für jedes Bild [Array]
        out tVecs, // Geschätzte Translations- (Verschiebungs-) Verktoren für jedes Bild [Array]
        flags); 
Flags

Wir Benutzen die Flags:

Flag Description
FixK4 The corresponding radial distortion coefficient is not changed during the optimization.
FixK5 The corThe corresponding radial distortion coefficient is not changed during the optimization.
ZeroTangentDist The corTangential distortion coefficients (p_1, p_2) are set to zeros and stay zero.
FixAspectRatio The corThe functions considers only fy as a free parameter. The ratio fx/fy stays the same as in the input cameraMatrix.
FixPrincipalPoint The corThe principal point is not changed during the global optimization.

Detail auf der Docs Seite: Docs

Parameter Erklärung
distortionVector Wird als Output genutzt und mit einem 0-Vector initialisiert
cameraMatrix Wird als Output genutzt und mit einer Einheitsmatrix initialisiert
imageSize Gibt die Größe der Bilder als Size an.
mCorners Die gefundenen / vom Bild errechneten Eckpunkte des Musters (Der Ist-Zustand)
objectPoints Eine Repräsentation der Bildpunkte (Der Soll-Zustand)

Für jedes Bild wird die gleiche Matrix verwendet da jedes Bild das gleiche Muster Zeigt. Die Matrix wird wie folgt erstellt.

List<Mat> objectPoints = new List<Mat>();
Mat objectPointMat = Mat.Zeros(1, 33, MatType.CV_32FC3);

for (int i = 0; i < 33; i++)
{
    float x = (i % 3) * 0.08f;
    float y = (i / 3) * 0.04f;
    if ((i / 3) % 2 == 1)
    {
        x += 0.04f;
    }
    objectPointMat.Set<Vec3f>(0, i, new Vec3f(x, y, 0));
}
for (int i = 0; i < mSamples.Count; i++)
{
    objectPoints.Add(objectPointMat);
}

Pattern

Aus dem Unterschied zwischen den Punkten des Musters und den gefundenen "Eckpunkten" ergibt sich die Linsenverzerrung

Der Letzte check Prüft ob einer der errechneten Faktoren ausserhalb der akzeptablen Grenzen liegt.

Falls dies der fall ist sind die einegebenen Samples nicht Genau Genug um eine Berechung mit verwentbarem Ergebiss anzustellen.

if (!Cv2.CheckRange(cameraMatrix) || !Cv2.CheckRange(distortionVector))
{
    return false;
}

Um den den entstehenden Fehler zu berechnen wird für jedes Bild eine Projection mit Hilfe der errechneten Parameter vorgenommen

Cv2.ProjectPoints(objectPoints[i], rVecs[i], tVecs[i],
                    cameraMatrix, distortionMatrix, projectedCorners);

Das Ergebnis wird mit dem real gemessenen Wert verglichen, siehe OpenCV norm

Die Quadrate der errechneten Fehler werden aufsummiert und durch die Anzahl der Messpunkte ("Ecken" in allen Bildern ) dividiert , die Wurzel dieser Kalkulation ergibt dann den Fehler.

Die Werte cameraMatrix und distortionMatrix scheinen uns in Unity nicht weiter zu helfen und können nicht direct als extrinsische Kameraverschiebung genutzt werden, deswegen muss diese separat berechnet werden.

Extrinsisch (V2)

Um Die automatische Kalibration zu erreichen, läuft eine Hintergrund Routine die ein paar mal in der Sekunde das aktuelle Kamerabild kopiert und dann mit Hilfe von OpenCV analysiert. Sollte das gesuchte Muster gefunden werden wird der aktuelle Frame gespeichert. Zusätzlich werden noch Position des Boards sowie Position und Rotation der Kamera gespeichert. Um Rechenzeit zu sparen wird die Pixelposition der erkannten Musterpunkte ebenfalls gespeichert, wobei für unteren Algorithmus nur der Punkt 0 von Relevanz ist.

Um die Position zu berechnen generieren wir nun ein Objekt als CamRig (Gizmo) und setzen dies als Parent-Object der Kamera, wird nun die lokale Position der Kamera gesetzt entspricht dies genau dem Offset (Das ganze lässt sich auch mit Matrixrechnung lösen, aber wer will das schon von Hand machen).

CamRig

Calib distance

Nachdem die "gemessene" Position nicht direkt in der Kameralinse liegt (Offset) entsteht eine Diskrepanz zwischen dem echten Punkt 0 auf dem Bild (Orange) und der berechneten Position (Blau/Cyan)

Um diesen Offset zu berechnen bedienen wir uns dem "Gradient descent" Verfahren. Die Diskrepanz berechnen wir in dem wir die Pixelkoordinaten als Vector2 interpretieren und voneinander abziehen (gespeichert als Vec3 mit z=0).

Da wir den Gradient descent mit einer Wertemenge bearbeiten wollen, berechnen wir die jeweilige Operation auf jedem Wert der Menge und summieren dies dann auf, anschliessend wird der Wert durch die Mächtigkeit der Menge geteilt.

Um den Gradienten zu erhalten benutzen wir die Numerische Differentiation

$f'(x) = (f(x+h)-f(x))/h$

    Vector3 r = new Vector3();
    float p = this.evaluate(CamOffset); // calculate point at offset once

    r.x = (this.evaluate(CamOffset + new Vector3(h, 0, 0)) - p) / h;
    r.y = (this.evaluate(CamOffset + new Vector3(0, h, 0)) - p) / h;
    r.z = (this.evaluate(CamOffset + new Vector3(0, 0, h)) - p) / h;

    return r;

Mit diesen Werten können wir dann schrittweise durch den Gradienten wandern.

    // calculate the step size 
    step = this.getSumOfDifferatiations(offset);
    // calc new offset
    offset = offset - (step * this.learingRate * ((1.0f-f)+ learnRatMulti*f )  );
    iterration++;
    f = (float)iterration / (float)maxInterrations; // calc how much "%" we have compleete
    // f is used to reduce step size over time 
    errorSize = this.getSumResSqr(offset); // calc remaining error size

Calib gdc

Manuelle Kalibrierung

Die App verfügt über manuelle Kalibrations-Regler.

Manuelle Kalibrations-Regler

Diese können genutzt werden um eine manuelle Kalibration einzurichten. Die Regler auf der rechten Seite sind zum Einstellen der Rotation und Regler auf der linken Seite der Translation. Der untere Regler ist jeweils für die X-Achse der seitliche Regler für die Y-Achse und der obere für die Z-Achse

Eine grob funktionierende Einstellung zum Testen ist Rotation: (0,-0.4963,1) Translation: (0,0,0)

Die Logik befindet sich im ManualSlider Script. Die Grenzen der Slider sind [-1,1] für die Translation gilt Angabe in Meter (1 = 1 Meter Offset). Für die Rotation gilt mal 180 (1 = 180° Rotation)

Kalibrierungs Unit Test

Für den Test der Kalibration steht ein Unit Test zur Verfügung, da das Labor aktuell nicht benutzt werden kann.

Dieser simuliert das Erstellen der Messpunkte mit Hilfe der vorgefertigten Kalibration-Board Prefab. Das Board wird von verschiedenen Positionen aus gerendert. Die Position der Boards in der Scene sowie die Position der virtuellen Kamera werden in diesem Fall als Messpunkte interpretiert. Es ist möglich einen Offset zwischen wirklicher Kameraposition und Messwert zu spezifizieren, um die Diskrepanz zwischen Positionsmarker und Kameralinse zu zu simulieren.

Unit Test Visualisirung

Relevante Optitrack Projekte im Keller

Die Projekte liegen momentan alle in C:\Users\quad\Documents\ (sollten evtl. mal aufgeräumt werden)

quad_ar App (Deprecated)

Die (native Android) quad_ar App wird ersetzt durch die SE-AR App auf Basis von Unity3D. Legacy Code, Bachelorarbeit und Dokumentation finden sich hier: App, Bachelorarbeit und ROS Node.

Hololens

Quad DSL

Um schnell und einfach Flugmissionen für Anwendungsprototypen etc zu schreiben, wurde eine DSL auf Basis von Lua erstellt. Die Lua Skripte werden vom quad_script_node oder dem interactive_script Plugin ausgeführt und in Befehle für die einzelnen ROS Komponenten umgesetzt.

Wird beispielsweise ein moveTo Befehl ausgeführt, wird ein PoseGoal erzeugt und über eine Pose Action an den TrajectoryClient weitergeleitet. Dieser fragt dann beim TrajectoryServer nach einer Trajektorie und führt diese aus. Der Fortschritt wird über die Pose Action mitgeteilt, so dass das Skript bei einem wait blocken kann.

Die Grundidee der Quad DSL ist, einen einzelnen Quadcopter über eine einfache imperative Sprache zu steuern ohne irgendwelche Eventverarbeitung, die den Kontrollfluss verkompliziert (Soll für Anfänger geeignet sein). Die Befehle sollen möglichst high-level sein und sich beispielsweise nicht um Kollisionsvermeidung kümmern müssen. Nur der moveTo Befehl ist asynchron, alle anderen Befehle sind synchron! Um zu Warten, bis ein Punkt angeflogen wurde muss also unbedingt auch ein wait oder sleep ausgeführt werden.

Quad DSL Befehle

moveTo

moveTo

moveTo(x, y, z)
moveTo(x, y, z, psi)
moveTo({x = x, y = y, z = z, psi = psi})
moveTo({x = pose().x + 3})       -- 3 Meter in x-Richtung bewegen
wait()                           -- warten bis das Ziel erreicht ist
moveTo(0, 0, 1)                  -- zurück über den Ursprung fliegen
sleep(5)                         -- 5 Sekunden warten

Beschreibung

moveTo bewegt den Quadcopter zu einer gegebenen Zielposition. Dabei wird Hindernissen etc. automatisch ausgewichen.

moveTo erwartet entweder die Zielposition (x, y, z, psi) in Weltkoordinaten (die Drehung ist optional und per default 0), oder ein table mit einer beliebigen Kombination der Keys (x, y, z, psi). Fehlende Keys werden mit der aktuellen Position des Quadcopters aufgefüllt.

moveTo blockt nicht, bis die Bewegung abgeschlossen ist; es wird ein zusätzlicher wait oder sleep Befehl benötigt.

Parameter

Rückgabewert

keiner

Fehler

Liegt die übergebene Position außerhalb des Flugraums oder kann nicht angeflogen werden, wird das Flugmanöver nicht begonnen.

pose

pose

{x = x, y = y, z = z, psi = psi} = pose()
{x = x, y = y, z = z, psi = psi} = pose(object)
moveTo(pose('gesture_wand'))     -- an Position von gesture_wand fliegen
wait()                           -- warten bis das Ziel erreicht ist
p = pose()                       -- pose abfragen (sollte grob der Position von gesture_wand zu Beginn des Flugmanövers entsprechen)

Beschreibung

pose gibt die aktuelle Pose eines Objekts zurück. Ohne Argument wird die Pose des Objekts "quad1" zurückgegeben. Ein Objekt ist ein String mit einem existierenden tf-Frame Namen. Die zurückgegebene Pose ist immer relativ zum Weltkoordinatensystem.

Parameter

Rückgabewert

Lua-Table mit Position (x, y, z) in Meter und Rotation um die z-Achse (psi) in Radian. psi = 0 enspricht Richtung der x-Achse.

Fehler

Ist das übergebene Objekt unbekannt (das Koordinatensystem existiert nicht), wird das Skript abgebrochen.

sleep

sleep

sleep(t)
moveTo(1,2,3,0)                  -- an Position fliegen
sleep(0.3)                       -- 0,3s warten, bis man es sich anders überlegt
moveTo(0,0,2,0)
sleep(10)                        -- diesmal länger warten, sodass der Quadcopter tatsächlich ankommt

Beschreibung

sleep pausiert die Ausführung für die angegebene Zeitdauer (in Sekunden).

Parameter

Rückgabewert

keiner

wait

wait

wait()
wait(delta)
wait(dx, dy)
wait(dx, dy, dz)
wait(dx, dy, dz, dpsi)
moveTo(1,2,3,0)                  -- an Position fliegen
wait(0.5)                        -- warten, bis der Quadcopter sich dem Ziel auf 0,5m genährt hat
moveTo(0,0,2,0)
sleep(10)                        -- diesmal länger warten, sodass der Quadcopter tatsächlich ankommt

Beschreibung

wait wartet bis der Quadcopter seine aktuelle Zielpose eingenommen hat. Die Toleranz, wie nah der Quadcopter an das Ziel herankommen muss, kann dabei eingestellt werden.

Parameter

Rückgabewert

keiner

takeoff

takeoff

takeoff()
takeoff(channel)
takeoff()
sleep(5)                         -- warten, bis der Quadcopter tatsächlich gestartet ist.
moveTo(2, 0, 1)
wait()

Beschreibung

takeoff startet den Quadcopter und schickt zusätzlich eine channel select Nachricht um im Switch Node die Kontrolle vom Gamepad auf den Quadcopter zu übertragen.

Parameter

Rückgabewert

keiner

Fehler

Invalide Channel Nummern (< 0) werden ignoriert.

land

land

land()
takeoff()                        -- Quadcopter starten…
sleep(5)
land()                           -- …und wieder landen

Beschreibung

land leitet die Landung für den Quadcopter ein. Zusätzlich wird eine channel select Nachricht geschickt, sodass der Switch Node die Kontrolle an das Gamepad zurückgibt (Channel wird auf 0 gesetzt).

Parameter

keine

Rückgabewert

keine

gesture

gesture

{x = x, y = y, z = z, psi = psi, orientation = {w = ow, x = ox, y = oy, z = oz}} = gesture()
{x = x, y = y, z = z, psi = psi, orientation = {w = ow, x = ox, y = oy, z = oz}} = gesture(g)
moveTo(gesture('roll_right'))    -- An Position fliegen, an der die roll_right Geste gemacht wurde
wait()

Beschreibung

gesture blockt so lange, bis die übergebene Geste erkannt wurde. Falls keine Geste übergeben wurde, blockt gesture bis irgendeine Geste erkannt wurde.

Parameter

Mögliche Gesten: roll_left, roll_right, pitch_up, pitch_down

Rückgabewert

Pose des Gestenstabs (frame: gesture_wand) zum Zeitpunkt der Geste (x, y, z, psi) in Meter bzw Radian. Das zusätzliche Feld orientation enthält die Orientierung des Gestenstabs als Quaternion (ow, ox, oy, oz).

Fehler

Unbekannte Gesten führen zu keinem Fehler, das Skript hängt einfach.

button

button

{...} = button()
{...} = button(b)
button('A')                      -- auf A Button warten um zu starten
takeoff()
b = button()
if b['B'] or b.X then
    land()                       -- landen, falls B oder X gedrückt wurde
end

Beschreibung

Die Funktion Button blockt bis der übergebene Button am angeschlossenen Gamepad gedrückt wurde. Falls kein Argument übergeben wird bis der nächste Button gedrückt wurde. Die Funktion gibt dann ein Table zurück, das für jeden Button den aktuellen Zustand (gedrückt/nicht gedrückt) enthält.

Parameter

Mögliche Werte: "A", "B", "X", "Y", "LB", "RB", "Back", "Start", "XBox", "L3", "R3"

Rückgabewert

Table string -> boolean, das für jeden Button String als Key anzeigt, ob dieser gerade gedrückt ist (true) oder nicht (false).

Fehler

Das Skript bricht mit einem Fehler ab, wenn ein unbekannter Button übergeben wird.

buttons

buttons

{...} = buttons()
b = buttons()
if b['B'] or b.X then
    land()                       -- landen, falls B oder X gedrückt wurde
end

Beschreibung

buttons gibt den aktuellen Zustand der Buttons des Gamepads zurück. Im Unterschied zu button (ohne s) blockt buttons jedoch nicht. Die Funktion gibt dann ein Table zurück, das für jeden Button den aktuellen Zustand (gedrückt/nicht gedrückt) enthält.

Parameter

keine

Rückgabewert

Table string -> boolean, das für jeden Button String als Key anzeigt, ob dieser gerade gedrückt ist (true) oder nicht (false).

photo

photo

p = photo()
display(photo())

Beschreibung

Macht ein Foto mit der Kamera des Quadcopters.

Parameter

keine

Rückgabewert

String mit dem Dateipfad des Bildes auf dem Rechner.

display

display

display(p)
display(photo())

Beschreibung

Zeigt ein Foto an und blockt bis das Fenster geschlossen wird.

Parameter

Rückgabewert

keine

read_qr

read_qr

text = read_qr(p)
print(read_qr(photo()))

Beschreibung

Liest einen QR Code in einem Bild und liefert die enthaltenen Daten zurück.

Parameter

Rückgabewert

String mit den im QR-Code enhaltenen Daten.

Fehler

Wird kein QR-Code erkannt, bricht das Skript mit einem Fehler ab.

flip

flip

flip(dir)
flip('right')

Beschreibung

Führt einen Salto in eine der vier Richtungen aus.

Parameter

Mögliche Werte für dir sind: "forwards", "backwards", "left", "right"

Rückgabewert

keine

Beschreibung

Informiert den ErrorFunction-Node, dass der Error ausgegeben werden soll. Einige Fehlerfunktion messen den Fehler über eine Flugstrecke, welche aus mehreren moveto Befehlen bestehen kann.

Parameter

keine

Rückgabewert

keine

channel

channel

channel(n)
channel(1)                       -- enable trajectory_client
channel(0)                       -- return to manual control

Beschreibung

Setzt den Kanal auf dem switch_node, sodass die Kontrolle vom Gamepad auf die Trajektorienplanung oder umgekehrt transferiert werden kann. Kanal 0 ist gewöhnlich manuelle Kontrolle, Kanal 1 die Trajektorienplanung.

Parameter

Rückgabewert

keine

Fehler

Falls eine negative Kanalnummer angegeben wird, wird der Befehl ohne Fehlermeldung ignoriert.

Blockly

Blockly ist eine von Google entwickelte Bibliothek um graphische Programmiereditoren zu erstellen. Die Quad DSL Befehle wurden in Blockly verfügbar gemacht, sodass eine Quadcoptermission auch graphisch erstellt werden kann.

Blockly

MiniLua

quad_script_node

Beispiel launch file

<!-- quad_script_node -->
<node name="quad_script_node"
      pkg="quad_script"
      type="quad_script_node"
      output="screen"
      required="true">
  <param name="use_mission_server"
         type="bool"
         value="false"/>
  <param name="script_path"
         type="string"
         value="$(find quad_script)/script/photo_sim_test.lua"/>
  <param name="photo_service"
         type="string"
         value="$(arg ns)/photo_service"/>
</node>

Der quad_script_node führt Quad DSL Skripte aus und steuert entsprechend den Quadcopter. Das auszuführende Skript kann entweder über einen Mission Client gesetzt werden, der dann Rückmeldung bekommt, wenn das Skript abgearbeitet wurde oder manuell über den Parameter ~script_path. Der Mission Server muss in diesem Fall deaktiviert sein.

Nach Beenden des Skripts oder wenn ein Fehler aufgetreten ist, hält der Quadcopter einfach die letzte gegebene Position.

Parameter

Name Typ Defaultwert Beschreibung
~world_frame string "world" Name des globalen Koordinatensystems
~pose_action string "action/pose" Topicname
~mission_action string "action/mission" Topicname
~joy_input string "joy" Topicname
~gesture_input string "gesture" Topicname
~photo_service string "photo_service" Topicname
~flip_output string "flip" Topicname
~land_output string "land_output" Topicname
~channel_select string "channel_select" Topicname
~takeoff_output string "takeoff_output" Topicname
~print_error_output string "print_error" Topicname
~skip_wait_for_pose_action bool false Der Node überspringt das Warten auf eine verbundene Pose Action, bevor das Skript gestartet wird
~use_mission_server bool true Skripte werden über den Mission Server geladen, statt über ~script_path
~script_path string Skriptdatei die ausgeführt werden soll

Topics/Services

Name Typ Art Beschreibung
~pose_action hector_uav_msgs/PoseAction action client Pose Action um Flugziele an TrajectoryClient zu senden
~mission_action quad_script/MissionAction action server Mission Server um neue Skripte zu laden und auszuführen
~joy_input sensor_msgs/Joy subscription Input von Gamepad
~gesture_input gesture_node/Gesture subscription Input von Gestenstab
~photo_service sim_photo/Photo service client Foto mit Quadcopter machen
~flip_output std_msgs/UInt8 publisher Flip mit Quadcopter machen
~land_output std_msgs/Empty publisher Quadcopter landen
~channel_select std_msgs/Int32 publisher Select channel for switch_node
~takeoff_output std_msgs/Empty publisher Quadcopter starten
~print_error_output std_msgs/Empty publisher Info an den ErrorFunction-Node, dass der Error ausgegeben werden soll

Die kursiven Namen werden über entsprechende Parameter eingestellt.

interactive_script

interactive_script ist ein Plugin für rqt, das die Entwicklung von Quadcopter Skripten noch weiter vereinfachen soll, indem es eine Live-Vorschau des Flugplans anzeigt. Der Code kann über die Live Vorschau verändert werden um Beispielsweise Wegpunkte zu verschieben. Durch einen Klick auf "run script" kann dann das Skript direkt in einer Simulation oder mit realem Quadcopter ausgeführt werden.

Interactive Script

Da sich das Plugin noch in der Entwicklung befindet, werden bisher nur Teile der Quad DSL unterstützt (derzeit moveTo, pose, wait und sleep).

Abhängigkeiten installieren

sudo apt install qtwebengine5-dev   # wird für blockly-view benötigt

Parameter

keine

Topics/Services

Name Typ Art Beschreibung
simple_marker visualization_msgs/InteractiveMarker interactive marker client Interaktive Marker für Visualisierung in rviz
action/pose hector_uav_msgs/PoseAction action client Pose Action um Flugziele an TrajectoryClient zu senden

ROS nodes

quad_state

gesture_node

sim_photo

vrpn_client

bebop_autonomy

Werkzeuge

ros_launch_analyze

rosslt

Rosslt ist eine Bibliothek für ROS1/2, die das Tracking von Werten durch das ROS-Netzwerk ermöglicht. Das Repository besteht aus mehreren Paketen:

Die folgenden Abschnitte geben einen Einblick in die Funktionsweise der einzelnen Komponenten und wie sie für eigene Projekte genutzt werden können. Auf die allgemeine Funktionsweise des Source Location Trackings wird nur kurz eingegangen.

Source Location Tracking in a nutshell

einfaches Beispiel zum SLT

local a = 5

function foo(x)
    slider({name='change me', value=x})
end

foo(a)

Source Location Tracking (SLT) ist eine Technik um einfach -- durch Value Tracing zur Laufzeit -- Werte im Programm mit ihrem Ursprung zu verknüpfen. Als Beispiel kann der Code in der Seitenspalte betrachtet werden: zunächst wird eine lokale Variable erzeugt, dann eine Funktion definiert, die ein GUI-Slider-Widget erstellen soll und diese dann aufgerufen. Zu beachten ist hierbei, dass die Slider-Funktion kein Callback o.Ä. übergeben bekommt, falls der Slider durch den Nutzer verschoben wird. Die Idee ist stattdessen, dass die Quelle des Wertes, der bei der Erzeugung angegeben wurde (das Zahlenliteral 5) direkt angepasst wird. Wird der Code danach erneut ausgeführt, befindet sich der Slider exakt an der Stelle, zu der er durch den Nutzer verschoben wurde.

Damit die Quelle des Wertes der Variablen x bekannt ist muss der Interpreter/Compiler der Sprache diese Information bewahren, indem er den Wert 5 damit annotiert. Wird der Wert von einer Variablen in eine andere kopiert, so wird die Quelle einfach mitkopiert.

Eine weitere Voraussetzung ist, dass der Code immer wieder ausgeführt wird. Dies ist bei stark eventbasierten Systemen wie ROS natürlicherweise gegeben.

Zusätzlich muss die Quelle änderbar sein. Dies ist bei kompilierten Sprachen wie C++ ein Problem, sodass nicht jedes Literal einfach geändert werden kann. Es existieren dennoch Quellen, auf die dies zutrifft, wie z.B. Parameter. Diese können problemlos zur Laufzeit geändert werden. Auch eine Anbindung an eine Skriptsprache, die SLT voll unterstützt (siehe Abschnitte MiniLua/interactive_script) wäre möglich.

SLT Beispiel

Die Abbildung zeigt einen schwierigeren Fall: Es wird ein Punkt gezeichnet, der analog zum Slider-Beispiel in allen 3 Dimensionen verschiebbar ist. Es ergeben sich zwei Probleme:

  1. Die x und y-Koordinate sind nicht unabhängig und hängen vom gleichen Wert (3) ab. Dieses Problem wird einfach ignoriert. Sobald der Punkt in y-Richtung verschoben wird, ändert sich auch die x-Koordinate und der Punkt landet nicht an der Stelle, an die er vom Benutzer gezogen wurde. Dieser Effekt ist meist explizit erwünscht, da sonst die Koordinaten ja nicht voneinander abhängig wären.
  2. auf die x-Koordinate wurde 2 addiert. Wird der Marker in x-Richung verschoben ist die Quelle der +-Operator. Das Ergebnis kann angepasst werden, indem einer oder beide Summanden angepasst werden. In diesem Fall wird eine Heuristik angewendet, die bevorzugt die linke Seite des Operators anpasst, sofern sie eine anpassbare Quelle besitzt.

Tracked-Template

Beispiele für den Umgang mit SLT Werten in C++

Tracked<int32_t> foo(const Tracked<int32_t>& x,
                     const Tracked<int32_t>& y) {
    x = x*y;                     // Arithmetische Operationen auf getrackten Werten sind möglich
    return x + 2;                // Es können auch ungetrackte Werte als Operanden verwendet werden
}

Tracked<int32_t> a = loc(5);     // Um eine änderbare Quelle zu erzeugen kann z.B. die TrackingNode::loc-Methode verwendet werden
Tracked<int32_t> b = loc(3);
auto c = foo(a, b);              // c == 17

force_value(c, 14);              // TrackingNode::force_value ändert die Quelle loc(5) auf den Wert 4, sodass c == 4*3+2 == 14 ist

Da C++ keine Unterstützung für SLT bietet, werden getrackte Werte mittels des Tracked<T>-Templates (Header rosslt/tracked.h) umgesetzt. Dieses übernimmt die Buchhaltung, welche Operationen auf dem Wert ausgeführt wurden und aus welcher Quelle er stammt. Arithmetische Operationen werden an den unterliegenden Datentyp T weitergeleitet, sodass im Idealfall nur ein paar Variablentypen angepasst werden müssen um bestehenden Code SLT-fähig zu machen.

Beispiel zu GET_FIELD/SET_FIELD

Tracked<visualization_msgs::msg::Marker> message;
Tracked<double> new_z = loc(0.0);

Tracked<geometry_msgs::msg::Pose> pose = GET_FIELD(message, pose);
Tracked<geometry_msgs::msg::Position> position = GET_FIELD(pose, position);

SET_FIELD(position, z, new_z);

Der .-Operator kann in C++ leider nicht überladen werden, sodass der Zugriff auf Felder und Methoden des getrackten Objekts zusätzlichen Aufwand erfordert. Es stehen die Makros GET_FIELD(struct, field) und SET_FIELD(struct, field, value) zur Verfügung um auf ein Feld aus einer getrackten ROS-Message zuzugreifen.

GET_FIELD(struct, field) kopiert struct.field und gibt das Feld als getrackten Wert zurück. Umgekehrt kann mit SET_FIELD(struct, field, value) das Feld struct.field = value gesetzt werden. Dabei wird die Quellinformation übernommen.

Es existiert eine automatische Typumwandlung von einer ROS-Message XyzTracked zu Tracked<Xyz>. Eine empfangene Nachricht vom Typ MarkerTracked kann also automatisch in Tracked<visualization_msgs::msg::Marker> umgewandelt werden oder umgekehrt.

In manchen Fällen kann es nötig sein, statt auf die nächste Ausführung zu warten direkt den Wert zu aktualisieren. Die TrackingNode::reevaluate Methode kann dazu den aktuellen Wert aktualisieren, indem es den Wert der Quelle abfragt und alle Operationen auf dem Wert erneut auswertet.

Erklärung zu LocationHeader

Intern nutzen getrackte Werte einen LocationHeader um die SLT Information zu speichern. Bei Tracked-Werten ist er eine unordered_map<string, Location>, wobei der Key der Feldname als String mit "/" als Trennzeichen ist. Also z.B. "foo/bar/baz" für ein Feld foo.bar.baz des getrackten Objekts. Das Gesamtobjekt ist durch den Key "." identifiziert.

Der Location Wert besteht jeweils aus dem Node, der den Wert produziert hat, einer eindeutigen ID zur Identifikation der Source Location durch den Node und der seitdem darauf angewendeten Operationen als RPN-Expression.

Die entsprechende ROS-Message LocationHeader verwendet statt der map zwei Listen der Keys und Locations.

Eigene Tracked Messages erstellen

Beispiel: MarkerTracked.msg

rosslt_msgs/LocationHeader location
visualization_msgs/Marker data

Um einen eigenen, durch rosslt getrackten, Messagetyp zu erstellen reicht es, eine Message zu erstellen, die genau 2 Felder besitzt: ein Feld location vom Typ rosslt_msgs/LocationHeader und ein Feld data des getrackten Typs. Ein Beispiel ist in der rechten Spalte abgebildet. Die Namen der Felder sind wichtig, da nur so die automatische Konvertierung in ein Tracked<T> Objekt funktioniert.

TrackingNode

Die TrackingNode Klasse leitet von der rclcpp::Node bzw. ros::NodeHandle Klasse ab und ist die Voraussetzung um einen Node SLT-fähig zu machen. Die Klasse bietet Hilfsmethoden um Locations zu erzeugen, zu verwalten und getrackte Werte an der Quelle zu ändern. Der ros::NodeHandle, bzw rclcpp::Node eines bestehenden Nodes kann einfach durch einen TrackingNode (Header rosslt/trackingnode.h) ersetzt werden.

Der TrackingNode erweitert den Standard-Node um einen LocationManager und einige Hilfsmethoden. Der LocationManager legt einen zusätzlichen Publisher und Subscriber für /sc an um Informationen über Änderungen von Source Locations zu erhalten. Zusätzlich wird ein Service angeboten (~/get_slt_value) um den aktuellen Wert einer Source Location des Nodes abzufragen.

Als Interface bietet der TrackingNode drei Methoden an:

Eigene Locations

Eigene Locationimplementierung (map als Speicher)

class MyTrackingNode : public TrackingNode {
public:
    Tracked<int> int_loc(int value, const source_location& sl = source_location::current()) {
        LocationFunc lf {
            // get
            [this](int32_t id) -> std::string {
                return to_string(location_storage[id]);
            },

            // set
            [this](int32_t id, const std::string& new_val) {
                location_storage[id] = sto<int>(new_val);
            }
        };

        id = loc_mgr->create_location(lf, sl);
        location_storage[id] = value;

        Location location {node_name, id};
        return Tracked<int>(value, location);
    }

private:
    map<int32_t, int> location_storage;
};

Anstatt der durch Parameter umgesetzten Locations, die durch TrackingNode::loc erzeugt werden, kann auch eine beliebige andere Implementierung genutzt werden, solange sie das Setzen und Lesen von Werten unterstützt (struct LocationFunc bietet das entsprechende Interface). Der LocationManager des Nodes bietet die Methode create_location, die diese LocationFunc nutzt. Das Listing zeigt ein relativ minimales Beispiel.

slt_talker/slt_listener

Diese Nodes zeigen anhand eines einfachen talker/listener Beispiels, wie rosslt verwendet werden kann. Der interne Counter des Talkers wird aber nicht innerhalb des Talkers erhöht, sondern durch eine Änderung der Quelle des Counters durch den Listener.

vis_talker/vis_listener

Diese Nodes zeigen den Umgang mit komplexeren Marker-Messages. Der Listener erzeugt einen interaktiven Marker aus der MarkerTracked-Nachricht und verändert die Quelle als Callback des Markers. Der vis_listener kann auch mit der experimentellen rosslt-Version des interactive-script Nodes verwendet werden, der MarkerTracked verschickt anstatt selbst die interaktiven Marker zu erzeugen.

RQT Graph Editor 2

Allgemein

Oberfläche des Plugins

Der RQT Graph Editor 2 ist ein im Rahmen einer Bachelorarbeit geschriebenes rqt plugin. Sein Ziel ist es sowohl eine intuitivie Beobachtung als auch eine intuitive Manipulation eines laufenden ROS-Systems zu ermöglichen sowie dieses System zu speichern und zu laden. Das ultimative Ziel von ihm wäre es die manuelle Bearbeitung von launch-files abzulösen, aber davon ist er noch etwas entfernt.

Dieser Editor ist in Python mit PyQt5 als Qt-binding geschrieben. Zugriff auf das laufende ROS-System erfolgt über rospy oder die ROS-Kommandozeilentools. Es exisitiert auch ein in C++ geschriebener Prototyp von Thomas Witte, der RQT Graph Editor (1).

Installation

Plugin Installation

cd ~/catkin_ws/src
git clone https://spgit.informatik.uni-ulm.de/research-projects/quadrocopter/rqt_graph_editor_2
cd ..
catkin_make
rqt --force-discover

Das Repository muss nur in einen Catkin Workspace ausgecheckt und gebaut werden. Danach sollte es im Menü von rqt unter „Introspection“ verfügbar sein. Gegebenenfalls kann mithilfe des --force-discover switches das Suchen von rqt-Plugins erzwungen werden.

Bedienung

Monitoring of the system

To allow for a intuitive overview of the currently running ROS-system the core component of the editor, the graph view is deployed. It is located in the bottom right of the editor. Currently running nodes are displayed as gray rounded rectangles while topics are displayed as blue rectangles. The green connection arrows represent a subscription or publication of a node to a topic. Connections always point in the direction of the information flow, meaning from topic to node if it is a subscribing connection and from node to topic if it is a publishing one. A gray circle in the middle of a connection indicates that this connection has been remapped meaning the attached node has been started with a remap argument that leads to this connection being remapped from its original topic to the topic it is now attached to (see "Remapping of connections" for more details). The services offered by a node can be viewed by right-clicking it. They can also be called from there when clicking the respective service name. When a node is clicked it becomes selected. Deselecting all nodes is possible by clicking on an empty spot in the graph view. Parameters in the parameter view are highlighted if they share the same namespace (prefix) as a currently selected node.

A package that has been also considered in this depiction of the system is actionlib, which allows for more complex service calls that need a long time to execute. Such calls often come with the user's wish to receive periodic feedback or being able to cancel the call, which are functionalities actionlib offers. However, those features are implemented by using 5 additional topics to communicate the different messages between service provider and caller. As this obviously restricts the clarity of the graph view, actionlib topics are being collapsed into only one topic named after the prefix of the collapsed topics.

Graph Toolbar

Nodes as well as topics can be freely moved around by drag and drop and can be newly drawn within the currently visible part of the scene using the reload button on the very left in the graph toolbar. It is also possible to hide several kinds of topics or nodes using the checkboxes in the graph toolbar next to Hide. Hiding debug for instance leads to all nodes containing /rosout or /rqt_py_node (to hide the editor/rqt itself) in their name as well as topics with /rosout, /rosout_agg, /tf, /tf_static in their name to be hidden. Hidden nodes or topics are invisible and unselectable. They maintain their position though. So deselecting the appropriate checkbox will lead to all items reappearing at their old positions. Connections of which at least one item they are connected to is hidden, are also hidden. It is possible to also hide dead ends which are topics with only one connection attached to them as well as island nodes, which are nodes with no connections (apart from one connection to /rosout that always exists) attached to them.

The filter option to the very right of the graph toolbar allows for the user to insert a string as a filter. When pressing enter in this field only nodes and topics stay visible, that contain this string. The Highlight functionality is explained in the "Highlight dead ends" section.

Saving and loading

The original launch-file. It includes another launch-file named drone.launch

<?xml version="1.0"?>

<launch>
    <arg name="ns" default="/start_collision_test"/>
    <arg name="trajectory_assignment" default="$(arg ns)/trajectory_assignment"/>
    <arg name="trajectory_request" default="$(arg ns)/trajectory_request"/>

    <group ns="$(arg ns)">
        <!-- drone1 -->
        <include file="/home/dominik/drone.launch" >
            <arg name="x" value="-2" />
            <arg name="y" value="0" />
            <arg name="ns" value="$(arg ns)/drone1" />
            <arg name="script_path" value="/home/dominik/collision files/collision1.lua" />
            <arg name="trajectory_request" value="$(arg trajectory_request)"/>
            <arg name="trajectory_assignment" value="$(arg trajectory_assignment)"/>
        </include>
    </group>
</launch>
\end{lstlisting}

The launch-file drone.launch included in the main file

<?xml version="1.0"?>

<launch>
    <arg name="ns" default="/drone"/>
    <arg name="x" default="0"/>
    <arg name="y" default="0"/>
    <arg name="z" default="0"/>
    <arg name="gesture_topic" default="gesture"/>
    <arg name="script_path"/>
    <arg name="trajectory_assignment"/>
    <arg name="trajectory_request"/>
    <arg name="qr_map" default="$(find sim_photo)/map/test.txt"/>
    <arg name="delay" default="0.2"/>

    <group ns="$(arg ns)">
        <!-- trajectory_client_node -->
        <node name="trajectory_client_node"
            pkg="trajectory_server"
            type="trajectory_client_node"
            output="screen" >
            <param name="x"
                type="double"
                value="$(arg x)" />
            <param name="y"
                type="double"
                value="$(arg y)" />
            <param name="name"
                type="string"
                value="$(eval arg('ns').split('/')[-1])" />
            <param name="trajectory_assignment_input"
                type="string"
                value="$(arg trajectory_assignment)" />
            <param name="trajectory_request_output"
                type="string"
                value="$(arg trajectory_request)" />
        </node>
    </group>
</launch>

The resulting launch-file when loading the others and saving again. It is now only one file without any arg tags or comments.

<?xml version="1.0" ?>
<launch>
    <group ns="start_collision_test">
        <group ns="drone1">
            <node name="quad_script_node" pkg="quad_script" type="quad_script_node">
                <param name="script_path" value="/home/dominik/collision files/collision1.lua"/>
                <param name="gesture_input" value="gesture"/>
                <param name="use_mission_server" value="False"/>
                <param name="photo_service" value="/start_collision_test/drone1/photo_service"/>
            </node>
                <node name="traj_client_node" pkg="traj_server" type="traj_client_node">
                <param name="y" value="0.0"/>
                <param name="x" value="-2.0"/>
                <param name="name" value="drone1"/>
                <param name="traj_request_output" value="/start_collision_test/traj_request"/>
                <param name="traj_assignment_input" value="/start_collision_test/traj_assignment"/>
            </node>
            <node name="quad_node" pkg="traj_server" type="quad_node">
                <param name="delay" value="0.2"/>
                <param name="y" value="0.0"/>
                <param name="z" value="0.0"/>
                <param name="name" value="drone1"/>
                <param name="x" value="-2.0"/>
            </node>
            <node name="sim_photo_node" pkg="sim_photo" type="sim_photo_node">
                <param name="quad_name" value="drone1"/>
                <param name="sim_file" value="/home/dominik/catkin_ws/src/sim_photo/map/test.txt"/>
                <param name="photo_service" value="/start_collision_test/drone1/photo_service"/>
                <param name="use_simulation" value="True"/>
            </node>
        </group>
    </group>
</launch>

Saving and loading of the current state of the system is possible via the two buttons placed second and third from the left in the graph toolbar respectively. Using them opens an appropriate file dialog to select a file to save into or load from respectively. It should be noted that loading a launch-file this way executes it via roslaunch. As the editor shows the currently running system, executing a launch-file corresponds to an expected loading functionality with the content of the launch-file being added to the already running system. It should be noted that the already running components are not replaced but complemented by the new ones. The usual workflow therefore consists of firstly running the roscore, secondly starting rqt and thirdly loading the desired launch-file.

When saving the current state into a launch-file there are several considerations to keep in mind: Currently used tags when saving are:

Currently unused tags when saving are consecutively:

There are several consequences that result from this setup. Node tags may never be made mandatory as they sometimes need to be restarted when remapping a connection of them for example and shutting down a mandatory node results in the whole system crashing. Also while the launch-files are saved in pretty XML and group tags are used to summarize nodes and parameters inside the same namespaces, the resulting files are usually not easy to work with manually, mostly due to the reasons that everything is saved into one file (even loaded files that were previously separated). Also the fact that the order of the inserted tags may change when loading and saving again can proof to be quite obnoxious for users. Comments are naturally unable to be recovered as well as their information is not transferred into the running system. Additional static analysis of the originally loaded files may help facing these issues that have also been raised in the evaluation of this editor. The ultimate goal of this editor should be to make working inside launch-files completely unnecessary.

Setting parameters

Adding a new parameter is possible by clicking the green plus button in the parameter toolbar. When doing so a new dialog opens that allows for specifying the parameter name and value.

Setting a new value has usually no immediate effect on the running system as nodes most of the time only query parameters when they start. So restarting the respective nodes might be necessary to trigger the desired effects.

Shutting down of nodes

Shutting down of currently running ROS nodes is possible via right clicking the respective node item in the graph view and then clicking shutdown node in the consecutively opening context menu. Starting existing nodes did not make it into the current version of the editor due to time restrictions. However it can still be performed via conventional means like rosrun. An option that turned out to be quite useful as well, was restarting the node as many nodes only load their parameters when starting and do not register parameter changes when already running. It is also possible via the context menu of a node.

Highlighting of dead ends

Regarding the practitioners interviewed in the requirements-engineering and the evaluation an error sometimes occurring is misspelling a topic name or messing up a namespace so that the communication between two nodes fails due to them publishing and subscribing to a differently named topic. To have a tool supporting said debugging case the highlighting of dead ends feature has been implemented. It is selectable as a checkbox next to Highlight in the graph toolbar. When selected all topics in the graph view that only have one connection attached to them become outlined red as they are more likely to be the result of a mistake.

Creating & deleting of nodes (ad hoc nodes)

The editor allows for the instant creation of so called ad hoc nodes. These are python scripts created from templates that derive a lot of the necessary information like topic types automatically and insert the import statement for instance accordingly. The goal hereby is to diminish the code the user has to insert manually as much as possible. The ad hoc node window can be opened from three different context menus each opened by right clicking another component in the graph view:

Rqt graph editor2 publisher

The ad hoc node dialog with the publisher template on the topic /turtle1/cmd_vel. Clearly visible are the already inserted topic types in the import statement or the msg constructor call (Twist()) as well as the already inserted topic name in the publisher initialization. The commented message signature should provide a better understanding of the message the user wants to create or handle.

Syntax highlighting as well as indentation support is provided for all code snippets loaded and created. All of the templates would technically run if executed without any manipulation but would send empty messages or do nothing with the messages received.

Enabling service calls

Another convenience feature implemented is the service call functionality. The context menu opening when right clicking a node in the graph view, lists all available service calls on this node. All nodes offer the services get_loggers and set_logger_level by default which allow getting and setting of the respective logger level of this node \cite{rosout}. Clicking a service opens a service call dialog with the field type on the left and the name of the field inserted in the respective text field by default. As service call request messages are standard ROS messages they can theoretically be arbitrarily complex. Complex, hierarchical built request messages are displayed in an accordingly designed window. After replacing all defaults with the actual arguments the service can be executed via the ``call''-button. Afterwards a feedback message is displayed below with an error message or the service answer.

Search/Filter function

With growing sizes of ROS systems it becomes more and more necessary to only show smaller parts of it to maintain clarity. The search/filter function serves exactly this purpose. Its interface is placed in the very right of the graph toolbar via a simple text field. When pressing enter in it the currently inserted string is used to filter out all nodes and topics in the graph view that do not contain this string. Resetting is possible via entering an empty string and pressing enter.

Remapping of connections

Sometimes a developer wants a node to subscribe or publish on another topic than the current one. Redirecting this connection to another topic is called remapping and is performed via restarting the node using rosrun with a special remap argument: old_topic:=new_topic.

There are currently two possibilities implemented for performing a remap on a node:

In both cases a type check is performed that prevents the user from remapping to another topic that has messages of a different type running trough it and an appropriate error dialog is shown.

If a connection is the result of a remapping argument, it is displayed with a little gray circle in the middle.

Changing parameters

Changing the values of already existing parameters opposed to creating new ones can be performed in the parameter view on the left of the main widget. Here all parameters currently registered at the ROS master are shown including their name (left) and value (right). It is possible to click into the value text field, change the value and confirm the new value by pressing enter. A little green animation flashes up in the value field to notify the user that his change was successful.

Architektur

Architektur des RQT Graph Editor 2

The two main modules are GUI and Backend. All complex operations regarding ROS are located in the Backend module, which is aside from one call to the Util package (query the current system state when saving it) independent. The pivotal file hereby is the RosHelperInterface. It offers all of the backend's functionality and acts as an interface. All calls to it get delegated to the appropriate helper class which actually implements the function. Sometimes these helper classes need to call each other. When that happens the calls also run via the Interface to make the individual helper files easier replaceable. Often times the client library rospy or the existing command line tools are used to interact with the running ROS system.

The GUI contains first of all the EditorWidget which is used by rqt to place the editor as plugin into the framework. This widget contains two additional major widgets: The ParamWidget which includes the parameter toolbar and the parameter view as well as the (EditorView and in that) EditorScene. This scene displays the three item types used to picture the current ROS system: TopicItem, NodeItem and ConnectionItem. The scene processes regularly incoming (currently at 0.5 Hz) update signals. It then adds or removes items as necessary to keep the graph view up to date. Calling the Backend's more complex functionality is done from anywhere within the GUI.

The update signals processed by the EditorScene are regularly emitted by the Poller from the Util package by querying the current system state from the ROS master and then calculating the deltas compared to the previously saved system state. This Util package is a leftover from the early development times and could be most likely integrated into the Backend package. Doing so would result in a cleaner architecture and is encouraged for the subsequent developers of this editor. The model of the editor is mostly represented by the currently running ROS system itself. The few additional flags that are needed to represent the current state of the GUI (for instance the checkbox' selection states) are saved in the GUI itself.

Hinweise für zukünftige Programmierer

Umstieg auf Python 3

Der Support für Python 2 wurde wohl am 01.01.2020 eingestellt, sodass eine Portierung auf Python 3 sehr sinnvoll wäre.

Umstieg auf ROS 2

Eine Anpassung des Editors um ROS 2 zu unterstützen, wäre vermutlich mittelfristig auch sinnvoll. Entscheidet ihr wie wichtig euch das ist.

Verwendung von python_qt_bindings

Es existieren zwei beliebte Qt Binding-Bibliotheken für Python: PyQT und PySide. Der Editor verwendet momentan PyQt. Einige ROS-Entwickler haben mit python_qt_bindings eine Bibliothek geschrieben, die es einem erlaubt beide Binding-Bibliotheken dahinter zu verwenden. Diese zusätzliche Abstraktionsstufe hat aber bei mir dafür gesorgt, dass meine Auto-completion features der IDE (VS Code) kapituliert haben.

Verschieben der Dateien von src zu scripts

Ich bin mir nicht ganz sicher in der Frage ob Python Skripte in den Ordner src (wo sie gerade liegen) oder in den Ordner Skripts gehören. Das ist zwar nicht allzu wichtig, aber könnt ihr ja vielleicht mal nachlesen.

Polling verbessern

Auf dem master Branch liegt eine funktionierende Version, bei der im util package die Funktionalität den Systemzustand abzufragen sowie die Deltas zu berechnen liegt. Semantisch gehört das nach meiner Auffassung eher in das Backend. Außerdem ist der momentane Ansatz etwas fehleranfällig, da er die SystemState und SystemChange (Das DeltaObjekt) mehrfach wiederverwendet. Manchmal kommt die GUI beim Starten des Programms nicht hinterher die Updates zu verarbeiten und das zweite Update überschreibt das erste bevor dieses verarbeitet wurde. Der Poller im util package erbt von QThread. Ich habe noch einige Versuche gestartet dieses Zustandspolling in das Backend zu verfrachten und dabei auf Abhängigkeiten auf Qt zu verzichten. Ich muss leider sagen, dass ich dabei trotz einiger Stunden Aufwand nicht erfolgreich war. Meine Versuche können auf dem Nebenbranch "improve_polling" angeschaut werden.

Kontakt

Ich weiß wie hart es sein kann komplexen Code eines anderen zu übernehmen. Solltet ihr als meine Nachfolger Hilfe benötigen, dürft ihr euch gerne bei mir unter dominik.schindler-zins@gmx.de melden. Da ich unter der Woche arbeite, antworte ich aber vermutlich erst am Wochenende.

Beispiele

Flug mit RViz

Anleitung: Flug mit Trajektorienplanung im Keller

Grundgerüst über alias starten

se_server

In separatem Terminal das Launchfile passend zur gewählten Drohne starten, z.B. Noctowl:

roslaunch se_trajectory_launch Noctowl.launch

Simulation mit BebopS

Flug mit Hector

Girls' Day

Ideen, Projekte, Abschlussarbeiten

Dieser Abschnitt soll Ideen für Projekte, Abschlussarbeiten, kurze Einarbeitungsaufgaben etc. sammeln. Jeder Unterabschnitt soll die Idee vorstellen mit grob geschätzem Aufwand und Startpunkten um sich einzuarbeiten.

rosslt Visualisierung/Dashboard

Art Umfang
Abschlussarbeit / Projekt 1 Semester

Ziel

Das Location-Tracking von ROS-Messages erlaubt es, den Ursprung (Node, Parameter) einzelner Nachrichten festzustellen. Ein BA Thema könnte sein, dies graphisch aufzubereiten und interaktiv zu visualisieren. Eine solche Oberfläche sollte es erlauben ROS-Topics zu abbonieren, Nachrichten anzuzeigen und zu verändern. Dabei sollte die Source-Location der Nachricht genutzt werden um im Ursprungsnode so Änderungen vorzunehmen (z.B. Parameter setzen), dass sich die Änderung auch auf die folgenden Nachrichten entsprechend auswirkt.

rosslt Python Bibliothek

Art Umfang
Abschlussarbeit / Projekt 1 Semester

Ziel

Die rosslt-Client-Bibliothek (Tracked<int> statt int) wurde bisher nur prototypisch in C++ umgesetzt. Eine Implementierung in Python (passend zu den rclpy ROS2-Bindings) könnte potentiell die nötigen Änderungen an einem Node für das Tracking stark reduzieren. Python ist eine dynamisch typisierte Sprache, die weit bessere Introspection Möglichkeiten als C++ bietet. Eventuell bieten sich auch die Möglichkeiten, die aspektorientierte Programmierung bietet an um die nötigen Änderungen auf (fast) null zu reduzieren.

Alternative Sprachkonzepte für Quad DSL

Art Umfang
Abschlussarbeit / Projekt 1 Semester

Ziel

Die Skriptsprache und die Bewegungsprimitiven in interactive_script wurden relativ willkürlich gewählt um einen einzelnen Quadcopter einfach programmieren zu können. Es wäre interessant, alternative Konzepte, wie z.B. ein reaktives oder eventbasiertes Programmiermodell auszuprobieren. Insbesondere wäre interessant, wie mehrere Quadcopter gleichzeitig gesteuert werden können und wie das Programm visualisiert (und -- mittels Source Location Tracking -- manipuliert) werden kann.

Die Quad DSL hat derzeit einige Limitierungen:

Diese können eventuell durch andere Primitiven in der Sprache, zusätzliche Sprachkonzepte oder andere Sprachparadigmen gelöst werden.

ros-launch-analyze verbessern

Art Umfang
Abschlussarbeit / Projekt 1 Semester

Ziel

https://doi.org/10.1145/3196558.3196559 https://spgit.informatik.uni-ulm.de/research-projects/quadrocopter/ros-launch-analyze

MiniLua GUI

Art Umfang
Abschlussarbeit / Projekt 1 Semester

Ziel

InteractiveScript Debugging

Art Umfang
Abschlussarbeit / Projekt 1 Semester

Ziel

InteractiveScript Fehler highlighten

Art Umfang
Arbeitspaket 1 Woche

Ziel

Die Fehler beim Parsen/Ausführen statt in der Konsole im Code highlighten. Eventuell sind dazu größere Änderungen am Parser notwendig.

InteractiveScript Syntax-Highlighting

Art Umfang
Arbeitspaket 2 Tage

Ziel

Lua Syntax-Highlighting in InteractiveScript Plugin einbauen.

InteractiveScript Timeline Darstellung

Art Umfang
Arbeitspaket 1 Monat

Ziel

Als alternative zur spatialen Darstellung in rviz eine zeitliche Darstellung über Timeline entwickeln. Änderungen sollen wie bei rviz zurück in den Code propagiert werden.

Fehlende QuadDSL Befehle in InteractiveScript integrieren

Befehl Umfang Kommentar
moveTo 2 Stunden Variante mit 4 Parametern existiert bereits
wait 2 Stunden Variante ohne Parameter existiert bereits
takeoff < 1 Tag Wie graphisch darstellen?
land < 1 Tag Wie graphisch darstellen?
gesture 1 Tag Aktuelle Geste sollte in der Vorschau irgendwie setzbar sein
button(s) 1 Tag Werte direkt von angeschlossenem Gamepad lesen?
photo < 1 Tag Wie darstellen?
display < 1 Tag Wie in Vorschau darstellen? Gar nicht?
read_qr 1 Tag Wie in Vorschau darstellen? Gar nicht? Was ist mit Werten, die gelesen werden um daraus Positionen zu berechnen? tonumber Funktion fehlt in MiniLua
flip < 1 Tag Wie in Vorschau darstellen? Gar nicht?

Ziel

Die meisten Befehle werden noch nicht in interactive_script unterstützt. Ziel soll sein, dass der gleiche Funktionsumfang wie beim Script node unterstützt wird.

quad_control_gui ausgraben und anpassen

Art Umfang
Arbeitspaket 2 Wochen

Ziel

Die Gui von Jona an aktuellen Softwarestand anpassen und wieder lauffähig bekommen.

QuadDSL: display command um debug-Grafiken anzuzeigen

Mögliche Syntax:

display("text", Hallo Welt!, {x=1, y=0, z=2})
display("line", pose("quad1"), pose("helm_gelb"), "blue")
display("circle", pose(), radius, "red")
display("cube", pose(), size, "green")
Art Umfang
Arbeitspaket 1 Woche

Ziel

Möglichkeit über die DSL visualization_msgs zu verschicken -> zB um AR-Hinweise anzuzeigen oder für interactive_script!

https://spgit.informatik.uni-ulm.de/research-projects/quadrocopter/quad_script/issues/1

ROS2 Lifecycle Manager

Art Umfang
Arbeitspaket 1 Woche

Ziel

Die GUI von Julian vervollständigen, den aktuellen State von Nodes monitoren.

Konzept für ROS2 launch files

Art Umfang
Konzept 2 Wochen

Ziel

In launch einarbeiten, Anforderungen, Struktur, Guidelines entwickeln, wie die launch-Files in Zukunft aussehen könnten.

rqt_graph_editor weiterentwickeln

Art Umfang
Projekt 1 Semester

Ziel

Die Bachelorarbeit von Dominik weiterentwickeln.

Unterschiede und Überlappung mit Node Manager feststellen und evtl Code / Konzepte übernehmen.

Sim Photo refactoring

Art Umfang
Arbeitspaket 1 Monat

Ziel